diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt
index 2cfe3a63f828b10e83f7d86ecdcf341ab389a577..b82a71a00d1d0b76235aa4e0b08b084bc326f0a8 100644
--- a/c++/CMakeLists.txt
+++ b/c++/CMakeLists.txt
@@ -24,14 +24,23 @@ target_link_libraries (
# dependencies for pybind11
add_subdirectory(deps/pybind11) # include pybind11 as a submodule
-pybind11_add_module(invKin src/py2gpmbind.cpp)
+pybind11_add_module(invKin_R src/py2gpmbind.cpp)
target_link_libraries(
- invKin PUBLIC
+ invKin_R PUBLIC
my_funcs
${RL_LIBRARIES}
eat::broccoli
)
+pybind11_add_module(invKin_L src/py2gpmbind.cpp)
+target_link_libraries(
+ invKin_L PUBLIC
+ my_funcs
+ ${RL_LIBRARIES}
+ eat::broccoli
+)
+
+
add_executable(myMain src/main.cpp src/gpm.cpp src/loadKinematicModel.cpp src/pointer_example.cpp)
target_link_libraries(
myMain
diff --git a/c++/include/loadKinematicModel.hpp b/c++/include/loadKinematicModel.hpp
index ca6d3ad89a8065de7bca25333817ecceeea3fc3e..ebcbbc2687743351cc8c32537c7f509d0a77ef54 100644
--- a/c++/include/loadKinematicModel.hpp
+++ b/c++/include/loadKinematicModel.hpp
@@ -1,8 +1,20 @@
#include <rl/mdl/Kinematic.h>
-class Arm {
+class Arm_R {
public:
- Arm(std::string path);
+ Arm_R(std::string path);
+
+ void* get_pointer2arm();
+
+ private:
+ rl::mdl::Kinematic* kin_pointer;
+ rl::mdl::Kinematic kin_model;
+ void* void_pointer;
+ };
+
+ class Arm_L {
+ public:
+ Arm_L(std::string path);
void* get_pointer2arm();
diff --git a/c++/src/loadKinematicModel.cpp b/c++/src/loadKinematicModel.cpp
index 553ad68b91d19ff4fc49d0c527ebe6108fd84b78..581511b0ca9912d003ec04b74b6538430b76ef43 100644
--- a/c++/src/loadKinematicModel.cpp
+++ b/c++/src/loadKinematicModel.cpp
@@ -5,7 +5,7 @@
#include "loadKinematicModel.hpp"
-Arm::Arm(std::string path){
+Arm_R::Arm_R(std::string path){
rl::mdl::UrdfFactory factory;
std::shared_ptr<rl::mdl::Model> model(factory.create(path));
kin_pointer = dynamic_cast<rl::mdl::Kinematic*>(model.get());
@@ -15,7 +15,19 @@ Arm::Arm(std::string path){
std::cout << "adress of pointer load " << void_pointer << std::endl;
}
-void* Arm::get_pointer2arm() {return void_pointer;}
+void* Arm_R::get_pointer2arm() {return void_pointer;}
+
+Arm_L::Arm_L(std::string path){
+ rl::mdl::UrdfFactory factory;
+ std::shared_ptr<rl::mdl::Model> model(factory.create(path));
+ kin_pointer = dynamic_cast<rl::mdl::Kinematic*>(model.get());
+ kin_model = *(kin_pointer);
+ rl::mdl::Kinematic* tmp_pointer = &kin_model;
+ void_pointer = (void*) tmp_pointer;
+ std::cout << "adress of pointer load " << void_pointer << std::endl;
+ }
+
+void* Arm_L::get_pointer2arm() {return void_pointer;}
/* class Arm {
public:
diff --git a/c++/src/main.cpp b/c++/src/main.cpp
index b6a42286f31da16631b44a98b78aa1e23cb02bb2..9c7e5728600159e39744d9b8738e9a39232459d3 100644
--- a/c++/src/main.cpp
+++ b/c++/src/main.cpp
@@ -23,7 +23,7 @@ int main(int, char**) {
std::shared_ptr<rl::mdl::Model> model(factory.create("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf"));
rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
- Arm right_arm("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf");
+ Arm_R right_arm("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf");
void* test_pointer = right_arm.get_pointer2arm();
// Is Values
diff --git a/c++/src/py2gpmbind.cpp b/c++/src/py2gpmbind.cpp
index d9c9addeb5f3614af4bc5a82efc79670f53768ff..73412f70f88a25225f5594f8582259db91a6c1c3 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -9,7 +9,7 @@ namespace py = pybind11;
-PYBIND11_MODULE(invKin, m) {
+PYBIND11_MODULE(invKin_R, m) {
m.doc() = "pybind11 binding to C++ function that computes IK based on GPM"; // optional module docstring
m.def("gpm", &gpm, "A function to compute the invserse kinematics for a 7 DOF serial manipulator on vecocity level with comfort pose and manipulabilty gradient",
py::arg("desired pose"),py::arg("desired Velocities"), py::arg("joint angles"), py::arg("joint velocities"), py::arg("left arm -> 1, right arm -> 0"), py::return_value_policy::copy);
@@ -18,10 +18,20 @@ PYBIND11_MODULE(invKin, m) {
// https://pybind11.readthedocs.io/en/stable/advanced/functions.html#return-value-policies
- m.def("set_pointer", &set_pointer, "set pointer", py::arg("pointer"), py::return_value_policy::move);
- m.def("get_pointer", &get_pointer, py::return_value_policy::move);
+ //m.def("set_pointer", &set_pointer, "set pointer", py::arg("pointer"), py::return_value_policy::move);
+ //m.def("get_pointer", &get_pointer, py::return_value_policy::move);
- py::class_<Arm>(m, "Arm")
+ py::class_<Arm_R>(m, "Arm_R")
.def(py::init<std::string>())
- .def("get_pointer2arm", &Arm::get_pointer2arm);
+ .def("get_pointer2arm", &Arm_R::get_pointer2arm);
+ }
+
+PYBIND11_MODULE(invKin_L, m) {
+ m.doc() = "pybind11 binding to C++ function that computes IK based on GPM"; // optional module docstring
+ m.def("gpm", &gpm, "A function to compute the invserse kinematics for a 7 DOF serial manipulator on vecocity level with comfort pose and manipulabilty gradient",
+ py::arg("desired pose"),py::arg("desired Velocities"), py::arg("joint angles"), py::arg("joint velocities"), py::arg("left arm -> 1, right arm -> 0"), py::return_value_policy::copy);
+
+ py::class_<Arm_L>(m, "Arm_L")
+ .def(py::init<std::string>())
+ .def("get_pointer2arm", &Arm_L::get_pointer2arm);
}
\ No newline at end of file
diff --git a/python/taskspace_placement/computeJoints.py b/python/taskspace_placement/computeJoints.py
index df36d945882ef2a38dbfda8c1454b8235822ab8e..8113c54100a64fe6ff8fcecd517cdbc090a75226 100644
--- a/python/taskspace_placement/computeJoints.py
+++ b/python/taskspace_placement/computeJoints.py
@@ -4,7 +4,8 @@ from matplotlib import pyplot as plt
import numpy as np
import copy
-from libs.invKin import *
+import libs.invKin_R as invKin_R
+import libs.invKin_L as invKin_L
from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi
@@ -33,18 +34,18 @@ desJointAngles_left = np.zeros((len(p1[:,0]),7))
computedPose_left = np.zeros((len(p1[:,0]),6))
error_left = np.zeros((len(p1[:,0]),6))
-left_arm = Arm("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
+left_arm = invKin_L.Arm_L("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
left_arm_p = left_arm.get_pointer2arm()
-right_arm = Arm("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
-right_arm_p = right_arm.get_pointer2arm()
+#right_arm = invKin_R.Arm_R("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
+#right_arm_p = right_arm.get_pointer2arm()
# loop for the left arm
for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_delta, dphi)): # loop through all the desired position of left arm
desPose = np.concatenate((pos, phi), axis=0)
desVelocities = np.concatenate((vel, phi_dot), axis=0)
# call the c++ egm function, return joint values and resulting pose
- result = gpm(desPose, desVelocities, jointAngles, jointVelocities, left_arm_p)
+ result = invKin_L.gpm(desPose, desVelocities, jointAngles, jointVelocities, left_arm_p)
desJointAngles_left[index,:] = result[0] # computed joint values from IK
computedPose_left[index, :] = result[1] # resulting pose with joint values from IK
if index > 0:
@@ -64,10 +65,10 @@ error_right = np.zeros((len(p1[:,0]),6))
jointAngles = np.array([-110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0]) * np.pi/180.0
# loop for the right arm
-for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)): # loop through all the desired position of left arm
+""" for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)): # loop through all the desired position of left arm
desPose = np.concatenate((pos, phi), axis=0)
desVelocities = np.concatenate((vel, phi_dot), axis=0)
- result = gpm(desPose, desVelocities, jointAngles, jointVelocities, right_arm_p)
+ result = invKin_R.gpm(desPose, desVelocities, jointAngles, jointVelocities, right_arm_p)
desJointAngles_right[index,:] = result[0]
computedPose_right[index, :] = result[1]
if index > 0:
@@ -75,7 +76,7 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)):
print('\n error', desPose - result[1])
error_right[index, :] = desPose - result[1]
jointAngles = result[0]
-
+ """
# see development of joint values
fig = plt.figure()
plt.plot(desJointAngles_left[:,0], label='joint1')