diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt
index 2cfe3a63f828b10e83f7d86ecdcf341ab389a577..37cd050b08162f4bb4eb5495e7c62d60cdb7350e 100644
--- a/c++/CMakeLists.txt
+++ b/c++/CMakeLists.txt
@@ -14,7 +14,7 @@ find_package (broccoli 3.0.0 COMPONENTS eigen REQUIRED)
find_package(RL COMPONENTS MDL REQUIRED)
# dependencies for gpm function
-add_library(my_funcs SHARED src/gpm.cpp src/loadKinematicModel.cpp src/pointer_example.cpp)
+add_library(my_funcs SHARED src/yumi.cpp)
target_link_libraries (
my_funcs
${RL_LIBRARIES}
@@ -32,7 +32,7 @@ target_link_libraries(
eat::broccoli
)
-add_executable(myMain src/main.cpp src/gpm.cpp src/loadKinematicModel.cpp src/pointer_example.cpp)
+add_executable(myMain src/main.cpp src/yumi.cpp)
target_link_libraries(
myMain
eat::broccoli
diff --git a/c++/include/gpm.hpp b/c++/include/gpm.hpp
index ca063b958908ed495248287a64d2c18d8b39fd36..f147010370278ed5ba8bf48d9370084abfa44a1b 100644
--- a/c++/include/gpm.hpp
+++ b/c++/include/gpm.hpp
@@ -3,4 +3,4 @@
std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> gpm(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity,
-Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, void * kinematic_ptr);
+Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, rl::mdl::Kinematic* kinematic);
diff --git a/c++/include/loadKinematicModel.hpp b/c++/include/loadKinematicModel.hpp
index ca6d3ad89a8065de7bca25333817ecceeea3fc3e..4e77030644afacabaaedfea47547db08ab364d17 100644
--- a/c++/include/loadKinematicModel.hpp
+++ b/c++/include/loadKinematicModel.hpp
@@ -4,10 +4,10 @@ class Arm {
public:
Arm(std::string path);
- void* get_pointer2arm();
+ std::shared_ptr<void> get_pointer2arm();
private:
rl::mdl::Kinematic* kin_pointer;
rl::mdl::Kinematic kin_model;
- void* void_pointer;
+ std::shared_ptr<void> sharedvoid_pointer;
};
diff --git a/c++/include/pointer_example.hpp b/c++/include/pointer_example.hpp
deleted file mode 100644
index 2f2ad00d9db55c8615b9f48a049dbf06324cc780..0000000000000000000000000000000000000000
--- a/c++/include/pointer_example.hpp
+++ /dev/null
@@ -1,5 +0,0 @@
-
-
-void * get_pointer();
-
-void set_pointer(void * pointer);
\ No newline at end of file
diff --git a/c++/include/yumi.hpp b/c++/include/yumi.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..89c4ae77a3087915de5261509436150bbf8b32d0
--- /dev/null
+++ b/c++/include/yumi.hpp
@@ -0,0 +1,39 @@
+#include <iostream>
+
+#include <Eigen/Eigen>
+
+#include <broccoli/control/kinematics/ComfortPoseGradient.hpp>
+#include <broccoli/core/math.hpp>
+
+#include <rl/math/Transform.h>
+#include <rl/mdl/Kinematic.h>
+#include <rl/mdl/Model.h>
+#include <rl/mdl/UrdfFactory.h>
+
+class Yumi {
+ public:
+
+ //functions
+ Yumi(std::string path);
+
+ void set_jointValues(Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity);
+
+ void print_pose();
+
+ private:
+ // vars for rl library
+ rl::mdl::Kinematic* m_kinematic;
+ rl::mdl::Kinematic m_kin_model;
+
+ // vars to store configuration
+ Eigen::Matrix<double, 6, 1> m_desPose = Eigen::Matrix<double, 6, 1>::Zero();
+ Eigen::Matrix<double, 6, 1> m_desVelocity = Eigen::Matrix<double, 6, 1>::Zero();
+
+ Eigen::Matrix<double, 7, 1> m_jointAngles = Eigen::Matrix<double, 7, 1>::Zero();
+ Eigen::Matrix<double, 7, 1> m_jointVelocity = Eigen::Matrix<double, 7, 1>::Zero();
+
+ rl::math::Vector3 m_position = rl::math::Vector3::Zero();
+ rl::math::Vector3 m_orientation = rl::math::Vector3::Zero();
+
+ void doForwardKinematics(rl::mdl::Kinematic* kinematic);
+ };
\ No newline at end of file
diff --git a/c++/src/gpm.cpp b/c++/src/gpm.cpp
index 69674105660c001cee58daeb513f9ff203c8ad84..d21b1dea0f7555004d9d350768be65cbe92da38a 100644
--- a/c++/src/gpm.cpp
+++ b/c++/src/gpm.cpp
@@ -1,5 +1,4 @@
#include <iostream>
-#include <string>
#include <broccoli/control/kinematics/ComfortPoseGradient.hpp>
#include <broccoli/core/math.hpp>
@@ -11,17 +10,11 @@
std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> gpm(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity,
-Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, void * kinematic_ptr) {
+Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, rl::mdl::Kinematic* kinematic) {
- // FORWARD KINEMATICS
- rl::mdl::Kinematic* kinematic = (rl::mdl::Kinematic*) kinematic_ptr;
- std::cout << "adress of pointer gpm " << kinematic_ptr << std::endl;
-
-
- //std::cout << "pointer in gpm" << kinematic_ptr << std::endl;
- //rl::mdl::Kinematic* kinematic = kinematicPtr;
+ // FORWARD KINEMATICS
kinematic->setPosition(jointAngles);
kinematic->forwardPosition();
kinematic->calculateJacobian();
@@ -33,20 +26,12 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
J(j, i) = kinematic->getJacobian()(j, i);
}
}
-
-
- // check if matrices are the same
- //std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;
- //std::cout << "myJacobian \n" << J << std::endl;
- //std::cout << "Manipulability meassure \n" << kinematic->calculateManipulabilityMeasure() << std::endl;
// extract orientation and position for the right arm
rl::math::Transform t = kinematic->getOperationalPosition(0);
rl::math::Vector3 position = t.translation();
rl::math::Vector3 orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
- //std::cout << "Joint configuration in degrees: " << jointAngles.transpose() * rl::math::RAD2DEG << std::endl;
- //std::cout << "FK end-effector position: [m] " << position.transpose() << " orientation [deg] " << orientation.transpose() * rl::math::RAD2DEG << std::endl;
-
+
// INVERSE KINEMATICS
// compute translation and orientation error
Eigen::Matrix3d desOrientation;
@@ -55,17 +40,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
*Eigen::AngleAxisd(desPose(4), Eigen::Vector3d::UnitY())
*Eigen::AngleAxisd(desPose(3), Eigen::Vector3d::UnitX());
- //std::cout << "reverse euler angles" << desOrientation.eulerAngles(2, 1, 0).reverse();
-
- // check if these two rotation matrices match!
- //std::cout << "desOrientation \n" << desOrientation << std::endl;
- //std::cout << "RL Orientation \n" << t.rotation() << std::endl;
-
- //Eigen::Vector3d einheitsvektor;
- //einheitsvektor << 1.0, 0.0, 0.0;
- //std::cout << "desOrientation x einheitsvektor \n" << desOrientation* einheitsvektor << std::endl;
- //std::cout << "RL Orientation x einheitsvektor\n" << t.rotation()*einheitsvektor << std::endl;
-
// define Quaternion with coefficients in the order [x, y, z, w]
Eigen::Vector3d desiredTranslation = desPose.head(3);
@@ -95,9 +69,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
effectiveTaskSpaceInput.tail(3) = gainDriftCompensation/dt * errorRotationInWorldFrame + desVelocity.tail(3);
//std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
-
-
-
// COMPUTE CPG GRADIENT
// define min and max values for the joints of Yumi
Eigen::Matrix< double, 7, 1> q_min;
@@ -146,8 +117,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
Eigen::Matrix<double, 7, 1> nullSpaceGradient = Eigen::Matrix<double, 7, 1>::Zero();
nullSpaceGradient = 0*manipGradient + 0*cpgGradient;
-
-
// ASC desired effective velocity does not work -> implement myself
Eigen::Matrix<double, 7, 7> m_inverseWeighing = Eigen::Matrix<double, 7, 7> ::Identity();
double m_activationFactorTaskSpace = 1.0;
@@ -160,11 +129,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
Eigen::Matrix<double, 7, 1> jointAnglesDelta;
jointAnglesDelta << jointVelocities * dt;
-
- //std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
- //std::cout << "delta qs in DEG \n" << jointAnglesDelta * rl::math::RAD2DEG << std::endl;
- //std::cout << "next qs in DEG \n" << (jointAngles+jointAnglesDelta)* rl::math::RAD2DEG << std::endl;
-
// forward kinematics with the new joints values from IK
kinematic->setPosition(jointAngles+jointAnglesDelta);
kinematic->forwardPosition();
@@ -172,9 +136,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
rl::math::Transform dest = kinematic->getOperationalPosition(0);
rl::math::Vector3 dposition = dest.translation();
rl::math::Vector3 dorientation = dest.rotation().eulerAngles(2, 1, 0).reverse();
- //std::cout << "IK joint configuration in degrees: " << (jointAngles+jointAnglesDelta).transpose() * rl::math::RAD2DEG << std::endl;
- //std::cout << "IK end-effector position: [m] " << dposition.transpose() << " orientation [deg] " << dorientation.transpose() * rl::math::RAD2DEG << std::endl;
-
Eigen::Matrix<double, 6, 1> resPose;
resPose << dposition.transpose()(0), dposition.transpose()(1), dposition.transpose()(2), dorientation.transpose()(0), dorientation.transpose()(1), dorientation.transpose()(2);
diff --git a/c++/src/loadKinematicModel.cpp b/c++/src/loadKinematicModel.cpp
index 553ad68b91d19ff4fc49d0c527ebe6108fd84b78..8e6350fa1473e9db7937f424aa129d3967dba4b0 100644
--- a/c++/src/loadKinematicModel.cpp
+++ b/c++/src/loadKinematicModel.cpp
@@ -8,14 +8,17 @@
Arm::Arm(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;
+
+
+ //kin_pointer = dynamic_cast<rl::mdl::Kinematic*>(model.get());
+ sharedvoid_pointer = (std::shared_ptr<void>) model;
+ //kin_model = *(kin_pointer);
+ //rl::mdl::Kinematic* tmp_pointer = &kin_model;
+ //oid_pointer = (void*) tmp_pointer;
+ //std::cout << "adress of pointer load " << void_pointer << std::endl;
}
-void* Arm::get_pointer2arm() {return void_pointer;}
+std::shared_ptr<void> Arm::get_pointer2arm() {return sharedvoid_pointer;}
/* class Arm {
public:
diff --git a/c++/src/main.cpp b/c++/src/main.cpp
index b6a42286f31da16631b44a98b78aa1e23cb02bb2..c46dbf2e22569bb2ea2819c7f9c9f4b366e9fea5 100644
--- a/c++/src/main.cpp
+++ b/c++/src/main.cpp
@@ -1,7 +1,7 @@
#include <iostream>
#include "gpm.hpp"
#include "loadKinematicModel.hpp"
-#include "pointer_example.hpp"
+#include "yumi.hpp"
#include <Eigen/Eigen>
#include <rl/math/Unit.h>
@@ -12,20 +12,28 @@
int main(int, char**) {
- enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
+ std::string path2yumi_l = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf";
+ Yumi yumi_l(path2yumi_l);
- //rl::mdl::Kinematic *kinematic_ptr = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf");
+ Eigen::Matrix<double, 7, 1> jointAngles;
+ jointAngles << 90.48, 17.87, -25.0, 48.0, -137.0, 122.0, -74.21;
+ jointAngles *= rl::math::DEG2RAD;
+
+ Eigen::Matrix<double, 7, 1> jointVelocities;
+ jointVelocities << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ yumi_l.set_jointValues(jointAngles, jointVelocities);
+ yumi_l.print_pose();
+
+
+
+ //enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
-
- //std::cout << *((int*)p) << std::endl;
- rl::mdl::UrdfFactory factory;
+/* rl::mdl::UrdfFactory factory;
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");
- void* test_pointer = right_arm.get_pointer2arm();
-
// Is Values
Eigen::Matrix<double, 6, 1> actualPosition;
Eigen::Matrix<double, 7, 1> jointAngles;
@@ -44,21 +52,10 @@ int main(int, char**) {
desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> result;
- result = gpm(desPose, desVelocity, jointAngles, jointVelocity, test_pointer);
+ result = gpm(desPose, desVelocity, jointAngles, jointVelocity, kinematic);
std::cout << "desired joint values: \n" << result.first << std::endl;
- std::cout << "current pose: \n" << result.second << std::endl;
-
-
-
- int number = 7;
- int *mypointer = &number;
- set_pointer(mypointer);
-
- void * p = get_pointer();
- set_pointer(p);
-
-
+ std::cout << "current pose: \n" << result.second << std::endl; */
return 0;
}
\ No newline at end of file
diff --git a/c++/src/pointer_example.cpp b/c++/src/pointer_example.cpp
deleted file mode 100644
index c140dbdbcdc78339ae4c30720ae037c509c6121a..0000000000000000000000000000000000000000
--- a/c++/src/pointer_example.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include <iostream>
-
-void * get_pointer(){
- static int number = 7;
- int *p2number = &number;
- std::cout << "pointer_ref" << (void*) p2number << std::endl;
- return ((void *) p2number);
-}
-
-void set_pointer(void * pointer){
- std::cout << "int value from pointer " << *((int*) pointer) << std::endl;
- std::cout << "void pointer " << pointer << std::endl;
-}
\ No newline at end of file
diff --git a/c++/src/py2gpmbind.cpp b/c++/src/py2gpmbind.cpp
index d9c9addeb5f3614af4bc5a82efc79670f53768ff..f84831a5dde8e8806e751e8aac0ebfdd7a80e45c 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -2,7 +2,7 @@
#include <pybind11/eigen.h>
#include "gpm.hpp"
#include "loadKinematicModel.hpp"
-#include "pointer_example.hpp"
+#include "yumi.hpp"
#include <rl/mdl/UrdfFactory.h>
namespace py = pybind11;
@@ -11,17 +11,18 @@ namespace py = pybind11;
PYBIND11_MODULE(invKin, 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);
+
+ //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_<Yumi>(m, "Yumi")
+ .def(py::init<std::string>())
+ .def("set_jointValues", &Yumi::set_jointValues, py::arg("joint angles"), py::arg("joint velocities"))
+ .def("printPose", &Yumi::print_pose);
// load model and return ptr for future function calls
// 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);
-
- py::class_<Arm>(m, "Arm")
+ /* py::class_<Arm>(m, "Arm")
.def(py::init<std::string>())
- .def("get_pointer2arm", &Arm::get_pointer2arm);
+ .def("get_pointer2arm", &Arm::get_pointer2arm); */
}
\ No newline at end of file
diff --git a/c++/src/yumi.cpp b/c++/src/yumi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..019601077d791884744ee99e709b0d61bcc5065e
--- /dev/null
+++ b/c++/src/yumi.cpp
@@ -0,0 +1,38 @@
+#include <iostream>
+
+#include <broccoli/control/kinematics/ComfortPoseGradient.hpp>
+#include <broccoli/core/math.hpp>
+
+#include <rl/math/Transform.h>
+#include <rl/mdl/Kinematic.h>
+#include <rl/mdl/Model.h>
+#include <rl/mdl/UrdfFactory.h>
+
+#include <yumi.hpp>
+
+// constructor
+Yumi::Yumi(std::string path){
+ rl::mdl::UrdfFactory factory;
+ std::shared_ptr<rl::mdl::Model> model(factory.create(path));
+ rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
+ m_kin_model = *(kinematic);
+ m_kinematic = &m_kin_model;
+}
+
+void Yumi::set_jointValues(Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity){
+ m_jointAngles = jointAngles;
+ m_jointVelocity = jointVelocity;
+}
+
+void Yumi::doForwardKinematics(rl::mdl::Kinematic* kinematic){
+ kinematic->setPosition(m_jointAngles);
+ kinematic->forwardPosition();
+ rl::math::Transform t = kinematic->getOperationalPosition(0);
+ m_position = t.translation();
+ m_orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
+}
+
+void Yumi::print_pose(){
+ Yumi::doForwardKinematics(&m_kin_model);
+ std::cout << "pose " << m_position << "\t" << m_orientation << std::endl;
+}
\ No newline at end of file