diff --git a/c++/include/loadKinematicModel.hpp b/c++/include/loadKinematicModel.hpp
index 4690a0f103a4b8663b94bafabdb862ca0325af11..ca6d3ad89a8065de7bca25333817ecceeea3fc3e 100644
--- a/c++/include/loadKinematicModel.hpp
+++ b/c++/include/loadKinematicModel.hpp
@@ -1,3 +1,13 @@
#include <rl/mdl/Kinematic.h>
-void * loadKinematicModel(std::string path);
+class Arm {
+ public:
+ Arm(std::string path);
+
+ void* get_pointer2arm();
+
+ private:
+ rl::mdl::Kinematic* kin_pointer;
+ rl::mdl::Kinematic kin_model;
+ void* void_pointer;
+ };
diff --git a/c++/src/gpm.cpp b/c++/src/gpm.cpp
index e65a514e10a88922810bdb5d3624a5d2e3ef7c6e..69674105660c001cee58daeb513f9ff203c8ad84 100644
--- a/c++/src/gpm.cpp
+++ b/c++/src/gpm.cpp
@@ -16,20 +16,12 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// 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;
+ //std::cout << "pointer in gpm" << kinematic_ptr << std::endl;
//rl::mdl::Kinematic* kinematic = kinematicPtr;
- /*
- Eigen::Matrix<double, 7, 1> joint_anlges;
- joint_anlges << 2.0, 1.0, 0.0, 2.0, 1.0, 0.0, 2.0;
- // forward kinematics for the right arm
- kinematic->setPosition(joint_anlges);
- kinematic->forwardPosition();
- kinematic->calculateJacobian();
- std::cout << kinematic->getJacobian() << std::endl;
- */
kinematic->setPosition(jointAngles);
kinematic->forwardPosition();
kinematic->calculateJacobian();
@@ -42,7 +34,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
}
}
- const clock_t t2 = clock();
// check if matrices are the same
//std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;
@@ -53,11 +44,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
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;
+ //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;
- const clock_t t3 = clock();
- std::cout << "time for extracting & printing: \t" << t3-t2 << std::endl;
// INVERSE KINEMATICS
// compute translation and orientation error
Eigen::Matrix3d desOrientation;
@@ -104,11 +93,10 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
effectiveTaskSpaceInput.head(3) = gainDriftCompensation/dt * (desiredTranslation - currentTranslation)
+ desVelocity.head(3);
effectiveTaskSpaceInput.tail(3) = gainDriftCompensation/dt * errorRotationInWorldFrame + desVelocity.tail(3);
- std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
+ //std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
+
- const clock_t t4 = clock();
- std::cout << "time for taskspace input: \t" << t4-t3 << std::endl;
// COMPUTE CPG GRADIENT
// define min and max values for the joints of Yumi
@@ -158,9 +146,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;
- const clock_t t5 = clock();
- std::cout << "time for gradients: \t" << t5-t4 << std::endl;
- //std::cout << "gradient \n" << nullSpaceGradient << std::endl;
// ASC desired effective velocity does not work -> implement myself
@@ -169,14 +154,12 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * nullSpaceGradient;
Eigen::Matrix<double, 7, 1> jointVelocities;
jointVelocities = broccoli::core::math::solvePseudoInverseEquation(J, m_inverseWeighing, effectiveTaskSpaceInput, nullSpaceVelocity, m_activationFactorTaskSpace);
- std::cout << "resulting jointVelocities: \n" << jointVelocities << std::endl;
+ //std::cout << "resulting jointVelocities: \n" << jointVelocities << std::endl;
// perform integration over one timestep to obtain positions that can be send to robot
Eigen::Matrix<double, 7, 1> jointAnglesDelta;
jointAnglesDelta << jointVelocities * dt;
- const clock_t t6 = clock();
- std::cout << "time for pseudo: \t" << t6-t5 << std::endl;
//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;
@@ -189,8 +172,8 @@ 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;
+ //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 fe64f5a48c5da032e6c79ec80b7a1ec4ae3dbc09..553ad68b91d19ff4fc49d0c527ebe6108fd84b78 100644
--- a/c++/src/loadKinematicModel.cpp
+++ b/c++/src/loadKinematicModel.cpp
@@ -2,33 +2,37 @@
#include <rl/mdl/Model.h>
#include <rl/mdl/UrdfFactory.h>
#include <rl/mdl/Kinematic.h>
+#include "loadKinematicModel.hpp"
-void * loadKinematicModel(std::string path) {
- rl::mdl::UrdfFactory factory;
+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;
+ }
+
+void* Arm::get_pointer2arm() {return void_pointer;}
- // demo code but should be the same: https://github.com/roboticslibrary/rl/blob/master/demos/rlJacobianDemo/rlJacobianDemo.cpp
- //std::shared_ptr<rl::mdl::Kinematic> kinematic;
- //kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(path));
-
+/* class Arm {
+ public:
+ // constrcutor
+ Arm(std::string path){
+ rl::mdl::UrdfFactory factory;
std::shared_ptr<rl::mdl::Model> model(factory.create(path));
-
- rl::mdl::Kinematic* my_p= dynamic_cast<rl::mdl::Kinematic*>(model.get());
-
- static rl::mdl::Kinematic model_kin = *(my_p);
- rl::mdl::Kinematic* my_pointer = &model_kin;
- std::cout << "pointer in func "<< my_pointer << std::endl;
+ 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;
+ }
- /*
- Eigen::Matrix<double, 7, 1> joint_anlges;
- joint_anlges << 2.0, 1.0, 0.0, 2.0, 1.0, 0.0, 2.0;
- // forward kinematics for the right arm
- my_pointer->setPosition(joint_anlges);
- my_pointer->forwardPosition();
- my_pointer->calculateJacobian();
- std::cout << my_pointer->getJacobian() << std::endl;
- */
+ void* get_pointer2arm() { return void_pointer; }
- return (void*) my_pointer;
- //return kinematic;
-}
+ private:
+ rl::mdl::Kinematic* kin_pointer;
+ rl::mdl::Kinematic kin_model;
+ void* void_pointer;
+ }; */
diff --git a/c++/src/main.cpp b/c++/src/main.cpp
index be12f883ff7c323e01ea2e45c572d32774a94f2d..b6a42286f31da16631b44a98b78aa1e23cb02bb2 100644
--- a/c++/src/main.cpp
+++ b/c++/src/main.cpp
@@ -23,6 +23,9 @@ 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");
+ void* test_pointer = right_arm.get_pointer2arm();
+
// Is Values
Eigen::Matrix<double, 6, 1> actualPosition;
Eigen::Matrix<double, 7, 1> jointAngles;
@@ -41,7 +44,7 @@ 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, kinematic);
+ result = gpm(desPose, desVelocity, jointAngles, jointVelocity, test_pointer);
std::cout << "desired joint values: \n" << result.first << std::endl;
std::cout << "current pose: \n" << result.second << std::endl;
diff --git a/c++/src/py2gpmbind.cpp b/c++/src/py2gpmbind.cpp
index 5f7cf7503c09d74c720b8ed345080619a42d1614..d9c9addeb5f3614af4bc5a82efc79670f53768ff 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -3,9 +3,12 @@
#include "gpm.hpp"
#include "loadKinematicModel.hpp"
#include "pointer_example.hpp"
+#include <rl/mdl/UrdfFactory.h>
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",
@@ -13,8 +16,12 @@ PYBIND11_MODULE(invKin, m) {
// load model and return ptr for future function calls
// https://pybind11.readthedocs.io/en/stable/advanced/functions.html#return-value-policies
- m.def("loadKinematicModel", &loadKinematicModel, "Load model from URDF", py::arg("Path to URDF file"), 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")
+ .def(py::init<std::string>())
+ .def("get_pointer2arm", &Arm::get_pointer2arm);
}
\ No newline at end of file
diff --git a/python/taskspace_placement/computeJoints.py b/python/taskspace_placement/computeJoints.py
index 3d79c67d99a839f6b62fcd7fe388107e19bcbdd8..df36d945882ef2a38dbfda8c1454b8235822ab8e 100644
--- a/python/taskspace_placement/computeJoints.py
+++ b/python/taskspace_placement/computeJoints.py
@@ -4,7 +4,7 @@ from matplotlib import pyplot as plt
import numpy as np
import copy
-from libs.invKin import gpm, loadKinematicModel
+from libs.invKin import *
from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi
@@ -33,16 +33,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))
-kinematic_model_ptr_L = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
-#kinematic_model_ptr_R = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
+left_arm = Arm("/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()
# 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, kinematic_model_ptr_L)
+ result = 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:
@@ -51,7 +53,8 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_delta, dphi)):
#print('IK resulting pose', result[1])
print('\n error', desPose - result[1])
error_left[index, :] = desPose - result[1]
- jointAngles = result[0]
+ jointAngles = result[0]
+
desJointAngles_right = np.zeros((len(p1[:,0]),7))
@@ -61,17 +64,17 @@ 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, kinematic_model_ptr_R)
+ result = gpm(desPose, desVelocities, jointAngles, jointVelocities, right_arm_p)
desJointAngles_right[index,:] = result[0]
computedPose_right[index, :] = result[1]
if index > 0:
jointVelocities = (desJointAngles_left[index, :] - desJointAngles_left[index-1, :])/dt
print('\n error', desPose - result[1])
error_right[index, :] = desPose - result[1]
- jointAngles = result[0] """
+ jointAngles = result[0]
# see development of joint values
fig = plt.figure()
@@ -139,6 +142,16 @@ plt.legend()
plt.title('errors right')
plt.show()
+# show trajectory in workspace of yumi
+fig = plt.figure()
+plt.plot(p2[:,0], p2[:,2], label='desired profile') # plot z over x
+plt.scatter(computedPose_right[:,0], computedPose_left[:,2], label='resulting pose from IK')
+fig.get_axes()[0].set_xlabel('x axis of yumi')
+fig.get_axes()[0].set_ylabel('z axis of yumi')
+plt.legend()
+plt.title('view on the x-z plane from the right arm side of yumi')
+plt.show()
+
np.save('data/desJointAngles_left', desJointAngles_left)
np.save('data/desJointAngles_right', desJointAngles_right)