From a1f1463f420f12bcbb926885519b0107ff4fceba Mon Sep 17 00:00:00 2001
From: Joschua Gosda <joschua.gosda@control.lth.se>
Date: Thu, 19 May 2022 12:10:15 +0200
Subject: [PATCH] two python modules but still only working when importing one
lib in python, otherwise pointers get messed up
---
c++/CMakeLists.txt | 13 +++++++++++--
c++/include/loadKinematicModel.hpp | 16 ++++++++++++++--
c++/src/loadKinematicModel.cpp | 16 ++++++++++++++--
c++/src/main.cpp | 2 +-
c++/src/py2gpmbind.cpp | 20 +++++++++++++++-----
python/taskspace_placement/computeJoints.py | 17 +++++++++--------
6 files changed, 64 insertions(+), 20 deletions(-)
diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt
index 2cfe3a6..b82a71a 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 ca6d3ad..ebcbbc2 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 553ad68..581511b 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 b6a4228..9c7e572 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 d9c9add..73412f7 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 df36d94..8113c54 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')
--
GitLab