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
deleted file mode 100644
index 4d367ee8fe7bd37f4b59e8723fc6fa5305cabd32..0000000000000000000000000000000000000000
--- a/c++/include/gpm.hpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include <Eigen/Eigen>
-#include <rl/mdl/Kinematic.h>
-
-
-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, std::shared_ptr<rl::mdl::Model> model);
diff --git a/c++/include/loadKinematicModel.hpp b/c++/include/loadKinematicModel.hpp
deleted file mode 100644
index b6be6d077a34fe2110592ffb0dfc54b6c7bcbf6e..0000000000000000000000000000000000000000
--- a/c++/include/loadKinematicModel.hpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#include <rl/mdl/Kinematic.h>
-
-class Arm {
- public:
- Arm(std::string path);
-
- std::shared_ptr<rl::mdl::Model> get_pointer2arm();
-
- private:
- //rl::mdl::Kinematic kin_model;
- std::shared_ptr<rl::mdl::Model> model;
- };
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..28f8410505583b81a2857dc6049cdcae9b905077
--- /dev/null
+++ b/c++/include/yumi.hpp
@@ -0,0 +1,67 @@
+#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 set_desPoseVel(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity);
+ void set_driftCompGain(double gain);
+ void set_sampleTime(double sampleTime);
+
+ void process();
+
+ Eigen::Matrix<double, 7, 1> get_newJointValues();
+ Eigen::Matrix<double, 6, 1> get_newPose();
+
+ // for debugging
+ void print_pose();
+
+ private:
+ // vars for rl library
+ rl::mdl::Kinematic* m_kinematic;
+ rl::mdl::Kinematic m_kin_model;
+
+ // vars for tuning
+ double m_driftCompGain = 1.0;
+ double m_sampleTime = 0.0125;
+ double m_activationFactorTaskSpace = 1.0;
+
+ // vars to store configuration
+ rl::math::Vector3 m_desPosition = rl::math::Vector3::Zero();
+ rl::math::Vector3 m_desPositionDot = rl::math::Vector3::Zero();
+ rl::math::Vector3 m_desOrientation = rl::math::Vector3::Zero();
+ rl::math::Vector3 m_desOrientationDot = rl::math::Vector3::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(); //Euler ZYX convention
+
+ Eigen::Matrix<double, 6, 1> m_effectiveTaskSpaceInput = Eigen::Matrix<double, 6, 1>::Zero();
+
+ Eigen::Matrix<double, 7, 7> m_inverseWeighing = Eigen::Matrix<double, 7, 7> ::Identity();
+ Eigen::Matrix<double, 7, 1> m_nullSpaceGradient = Eigen::Matrix<double, 7, 1>::Zero();
+
+ Eigen::Matrix<double, 7, 1> m_jointAnglesDelta;
+
+ // private functions
+ void doForwardKinematics(rl::mdl::Kinematic* kinematic);
+ Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);
+ void compTaskSpaceInput();
+
+
+ };
\ No newline at end of file
diff --git a/c++/src/gpm.cpp b/c++/src/gpm.cpp
deleted file mode 100644
index 4aac2751893a816ed9a4080254fb81f40a179063..0000000000000000000000000000000000000000
--- a/c++/src/gpm.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-#include <iostream>
-#include <string>
-
-#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>
-
-
-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, std::shared_ptr<rl::mdl::Model> model) {
-
- rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
-
- // FORWARD KINEMATICS
-
-
- //std::cout << "pointer in gpm" << kinematic_ptr << std::endl;
- //rl::mdl::Kinematic* kinematic = kinematicPtr;
-
- kinematic->setPosition(jointAngles);
- kinematic->forwardPosition();
- kinematic->calculateJacobian();
-
- Eigen::Matrix<double, 6, 7> J;
- // copy entries from RL jacobian to Eigen::jacobian in order to use it in brocolli function
- for (int i = 0; i < 7; i++){
- for (int j = 0; j < 6; j++){
- 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;
- // go from Euler ZYX to rotation matrix
- desOrientation = Eigen::AngleAxisd(desPose(5), Eigen::Vector3d::UnitZ())
- *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);
- //std::cout << "desiredTranslation" << desiredTranslation << std::endl;
- Eigen::Quaterniond desiredOrientation(desOrientation);
-
- // set the current positions
- Eigen::Matrix<double, 3, 1> currentTranslation;
- currentTranslation << position.x(), position.y(), position.z();
- Eigen::Quaterniond currentOrientation(t.rotation());
-
- // chose the correct quaternion such that the distance between desired and current
- // quaternion is the shortest
- if (desiredOrientation.dot(currentOrientation) < 0.0) {
- currentOrientation.coeffs() *= -1.0;
- }
- // calculate delta between quaternions
- Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
- Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
-
- // compute task space velocity with drift compensation
- const double gainDriftCompensation = 0.1;
- const double dt = 0.0125; // refers to 80 Hz
- Eigen::Matrix<double, 6, 1> effectiveTaskSpaceInput = Eigen::Matrix<double, 6, 1>::Zero();
- 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;
-
-
-
-
- // COMPUTE CPG GRADIENT
- // define min and max values for the joints of Yumi
- Eigen::Matrix< double, 7, 1> q_min;
- Eigen::Matrix< double, 7, 1> q_max;
- q_min << -168.5, -143.5, -168.5, -123.5, -290, -88, -229;
- q_min *= rl::math::DEG2RAD;
- q_max << 168.5, 43.5, 168.5, 80, 290, 138, 229;
- q_max *= rl::math::DEG2RAD;
- // create CompfortPoseGradient object
- broccoli::control::ComfortPoseGradient<7> cpg;
- // compute CPG gradient
- cpg.useRangeBasedScaling(q_min, q_max);
- //cpg.setWeight() by default it is 1
-
- Eigen::Matrix<double, 7, 1> cpgGradient = Eigen::Matrix<double, 7, 1>::Zero();
- cpgGradient = cpg.process(jointAngles, jointVelocity); // if gradient is zero then the ASC is just a resolved motion ik method
-
- // COMPUTE MANIPULABILTY GRADIENT
- // compute Jacobian derivative - code snippet from Jonas Wittmann
- std::array<Eigen::Matrix<double, 6, 7>, 7> dJ; // NOLINT
- for (auto& matrix : dJ) {
- matrix = Eigen::Matrix<double, 6, 7>::Zero();
- }
- Eigen::Matrix<double, 3, 7> transJ = Eigen::Matrix<double, 3, 7>::Zero();
- transJ = J.block<3, 7>(0, 0);
- Eigen::Matrix<double, 3, 7> rotJ = Eigen::Matrix<double, 3, 7>::Zero();
- rotJ = J.block<3, 7>(3, 0);
- const int numOfJoints = 7;
- for (int jj = 0; jj < numOfJoints; ++jj) {
- for (int ii = jj; ii < numOfJoints; ++ii) {
- dJ.at(jj).block<3, 1>(0, ii) = rotJ.col(jj).cross(transJ.col(ii));
- dJ.at(jj).block<3, 1>(3, ii) = rotJ.col(jj).cross(rotJ.col(ii));
- if (ii != jj) {
- dJ.at(ii).block<3, 1>(0, jj) = dJ.at(jj).block<3, 1>(0, ii);
- }
- }
- }
-
- Eigen::Matrix<double, 7, 1> manipGradient = Eigen::Matrix<double, 7, 1>::Zero();
- double cost = sqrt((J * J.transpose()).determinant());
- // Compute the manipulability gradient.
- for (int jj = 0; jj < 7; ++jj) {
- manipGradient[jj] = cost * ((J * J.transpose()).inverse() * dJ.at(jj) * J.transpose()).trace();
- }
- // add both gradients
- 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;
- 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;
-
- // perform integration over one timestep to obtain positions that can be send to robot
- 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();
-
- 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);
-
- // return desired joint angles for the next step and pose for computing the IK accuracy
- return std::make_pair((jointAngles+jointAnglesDelta), resPose);
-}
-
diff --git a/c++/src/loadKinematicModel.cpp b/c++/src/loadKinematicModel.cpp
deleted file mode 100644
index 856f00af1bae7364f73f7ce67541035008e3e00e..0000000000000000000000000000000000000000
--- a/c++/src/loadKinematicModel.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-#include <iostream>
-#include <rl/mdl/Model.h>
-#include <rl/mdl/UrdfFactory.h>
-#include <rl/mdl/Kinematic.h>
-#include "loadKinematicModel.hpp"
-
-
-Arm::Arm(std::string path){
- rl::mdl::UrdfFactory factory;
- std::shared_ptr<rl::mdl::Model> model_tmp(factory.create(path));
- model = model_tmp;
-
-
- //std::cout << "adress of pointer load " << void_pointer << std::endl;
- }
-
-std::shared_ptr<rl::mdl::Model> Arm::get_pointer2arm() {return model;}
-
-
-/* class Arm {
- public:
- // constrcutor
- 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;
- }
-
- void* get_pointer2arm() { return void_pointer; }
-
- 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
deleted file mode 100644
index 43be50a532d9bcfabc7ffff62de23767ff0f2f3c..0000000000000000000000000000000000000000
--- a/c++/src/main.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-#include <iostream>
-#include "gpm.hpp"
-#include "loadKinematicModel.hpp"
-#include "pointer_example.hpp"
-
-#include <Eigen/Eigen>
-#include <rl/math/Unit.h>
-#include <rl/mdl/Kinematic.h>
-#include <rl/mdl/Model.h>
-#include <rl/mdl/UrdfFactory.h>
-
-
-int main(int, char**) {
-
- enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
-
- //rl::mdl::Kinematic *kinematic_ptr = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf");
-
-
- //std::cout << *((int*)p) << std::endl;
-
- 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");
- std::shared_ptr<rl::mdl::Model> abc = right_arm.get_pointer2arm();
-
- // Is Values
- Eigen::Matrix<double, 6, 1> actualPosition;
- Eigen::Matrix<double, 7, 1> jointAngles;
- //jointAngles << 90.48, 17.87, -25.0, 48.0, -137.0, 122.0, -74.21; // start position left arm, angles from RS
- jointAngles << -110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0; // start position right arm, angles from RS
- //jointAngles << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
- jointAngles *= rl::math::DEG2RAD;
-
- Eigen::Matrix<double, 7, 1> jointVelocity;
- jointVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
-
- // Desired Values
- Eigen::Matrix<double, 6, 1> desPose;
- desPose << 0.300, 0.200, 0.200, 33.4*rl::math::DEG2RAD, 157.0*rl::math::DEG2RAD, 39.4*rl::math::DEG2RAD; // obtained from RS with stated joint values
- Eigen::Matrix<double, 6, 1> desVelocity;
- 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, abc);
-
- 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);
-
-
-
- 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..9aa1f932e39e45a1fec487bdcbb02a87730923cb 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -1,8 +1,6 @@
#include <pybind11/pybind11.h>
#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 +9,13 @@ 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);
-
- // 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_<Yumi>(m, "Yumi")
.def(py::init<std::string>())
- .def("get_pointer2arm", &Arm::get_pointer2arm);
+ .def("set_jointValues", &Yumi::set_jointValues, py::arg("joint angles"), py::arg("joint velocities"))
+ .def("set_desPoseVel", &Yumi::set_desPoseVel, py::arg("desired pose"), py::arg("desired velocities"))
+ .def("process", &Yumi::process)
+ .def("get_newJointValues", &Yumi::get_newJointValues)
+ .def("get_newPose", &Yumi::get_newPose)
+ .def("printPose", &Yumi::print_pose);
}
\ No newline at end of file
diff --git a/c++/src/yumi.cpp b/c++/src/yumi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c835410e192007b1efed795fffe639e913a62560
--- /dev/null
+++ b/c++/src/yumi.cpp
@@ -0,0 +1,109 @@
+#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();
+ kinematic->calculateJacobian();
+ 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 << "\n" << m_orientation << std::endl;
+}
+
+void Yumi::set_driftCompGain(double gain){
+ m_driftCompGain = gain;
+}
+
+void Yumi::set_sampleTime(double sampleTime){
+ m_sampleTime = sampleTime;
+}
+
+void Yumi::set_desPoseVel(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity){
+
+ m_desPosition = desPose.head(3);
+ m_desOrientation = desPose.tail(3);
+ m_desPositionDot = desVelocity.head(3);
+ m_desOrientationDot = desVelocity.tail(3);
+}
+
+Eigen::Matrix3d Yumi::euler2rotMatrix(rl::math::Vector3 eulerAngles){
+ Eigen::Matrix3d desOrientation;
+ // go from Euler ZYX to rotation matrix
+ desOrientation = Eigen::AngleAxisd(eulerAngles(2), Eigen::Vector3d::UnitZ())
+ *Eigen::AngleAxisd(eulerAngles(1), Eigen::Vector3d::UnitY())
+ *Eigen::AngleAxisd(eulerAngles(0), Eigen::Vector3d::UnitX());
+ return desOrientation;
+}
+
+void Yumi::compTaskSpaceInput(){
+ Eigen::Quaterniond desiredOrientation(euler2rotMatrix(m_desOrientation));
+ Eigen::Quaterniond currentOrientation(euler2rotMatrix(m_orientation));
+
+ // chose the correct quaternion such that the distance between desired and current
+ // quaternion is the shortest
+ if (desiredOrientation.dot(currentOrientation) < 0.0) {
+ currentOrientation.coeffs() *= -1.0;
+ }
+ // calculate delta between quaternions
+ Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
+ Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
+
+ m_effectiveTaskSpaceInput.head(3) = m_driftCompGain/m_sampleTime * (m_desPosition - m_position)
+ + m_desPositionDot;
+ m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInWorldFrame + m_desOrientationDot;
+
+}
+
+void Yumi::process(){
+
+ doForwardKinematics(&m_kin_model);
+ compTaskSpaceInput();
+
+ Eigen::Matrix<double, 7, 1> jointVelocities;
+ Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * m_nullSpaceGradient;
+ jointVelocities = broccoli::core::math::solvePseudoInverseEquation((&m_kin_model)->getJacobian(), m_inverseWeighing, m_effectiveTaskSpaceInput,
+ nullSpaceVelocity, m_activationFactorTaskSpace);
+
+ m_jointAnglesDelta << jointVelocities * m_sampleTime;
+
+ m_jointAngles += m_jointAnglesDelta;
+}
+
+Eigen::Matrix<double, 7, 1> Yumi::get_newJointValues(){
+ return m_jointAngles;
+}
+
+Eigen::Matrix<double, 6, 1> Yumi::get_newPose(){
+ doForwardKinematics(&m_kin_model);
+ Eigen::Matrix<double, 6, 1> pose;
+ pose << m_position, m_orientation;
+ return pose;
+}
diff --git a/python/taskspace_placement/computeJoints.py b/python/taskspace_placement/computeJoints.py
index df36d945882ef2a38dbfda8c1454b8235822ab8e..8939414ae0307f187b38c610ca80e18f45fcfbe5 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 *
+import libs.invKin as invKin
from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi
@@ -29,62 +29,65 @@ jointVelocities = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
dt = 0.0125
# create arrays to store values of loop
-desJointAngles_left = np.zeros((len(p1[:,0]),7))
+compJointAngles_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_p = left_arm.get_pointer2arm()
+yumi_left = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
+yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
-right_arm = Arm("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
-right_arm_p = right_arm.get_pointer2arm()
+#yumi_left.printPose()
# 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)
- desJointAngles_left[index,:] = result[0] # computed joint values from IK
- computedPose_left[index, :] = result[1] # resulting pose with joint values from IK
+ yumi_left.set_jointValues(jointAngles, jointVelocities)
+ yumi_left.set_desPoseVel(desPose, desVelocities)
+ yumi_left.process()
+
+ compJointAngles_left[index,:] = yumi_left.get_newJointValues() # computed joint values from IK
+ computedPose_left[index, :] = yumi_left.get_newPose() # resulting pose with joint values from IK
if index > 0:
- jointVelocities = (desJointAngles_left[index, :] - desJointAngles_left[index-1, :])/dt # only true in the ideal case where result of ik matches the desired pose
- #print('IK joints:', result[0])
- #print('IK resulting pose', result[1])
- print('\n error', desPose - result[1])
- error_left[index, :] = desPose - result[1]
- jointAngles = result[0]
+ jointVelocities = (compJointAngles_left[index, :] - compJointAngles_left[index-1, :])/dt # only true in the ideal case where result of ik matches the desired pose
+ error_left[index, :] = desPose - computedPose_left[index, :]
+ jointAngles = compJointAngles_left[index, :]
+
-desJointAngles_right = np.zeros((len(p1[:,0]),7))
+compJointAngles_right = np.zeros((len(p1[:,0]),7))
computedPose_right = np.zeros((len(p1[:,0]),6))
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)
- desJointAngles_right[index,:] = result[0]
- computedPose_right[index, :] = result[1]
+ yumi_right.set_jointValues(jointAngles, jointVelocities)
+ yumi_right.set_desPoseVel(desPose, desVelocities)
+ yumi_right.process()
+
+ compJointAngles_right[index,:] = yumi_right.get_newJointValues() # computed joint values from IK
+ computedPose_right[index, :] = yumi_right.get_newPose() # resulting pose with joint values from IK
+
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]
+ jointVelocities = (compJointAngles_left[index, :] - compJointAngles_left[index-1, :])/dt
+ error_right[index, :] = desPose - computedPose_right[index, :]
+ jointAngles = compJointAngles_right[index,:] """
# see development of joint values
fig = plt.figure()
-plt.plot(desJointAngles_left[:,0], label='joint1')
-plt.plot(desJointAngles_left[:,1], label='joint2')
-plt.plot(desJointAngles_left[:,2], label='joint3')
-plt.plot(desJointAngles_left[:,3], label='joint4')
-plt.plot(desJointAngles_left[:,4], label='joint5')
-plt.plot(desJointAngles_left[:,5], label='joint6')
-plt.plot(desJointAngles_left[:,6], label='joint7')
+plt.plot(compJointAngles_left[:,0], label='joint1')
+plt.plot(compJointAngles_left[:,1], label='joint2')
+plt.plot(compJointAngles_left[:,2], label='joint3')
+plt.plot(compJointAngles_left[:,3], label='joint4')
+plt.plot(compJointAngles_left[:,4], label='joint5')
+plt.plot(compJointAngles_left[:,5], label='joint6')
+plt.plot(compJointAngles_left[:,6], label='joint7')
plt.title('joint values over samples, left arm')
plt.legend()
plt.show()
@@ -108,25 +111,25 @@ plt.scatter(computedPose_left[:,0], computedPose_left[:,2], label='resulting pos
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.title('poses left')
plt.show()
-fig = plt.figure()
+""" fig = plt.figure()
plt.plot(computedPose_left[:,3], label='rx')
plt.plot(computedPose_left[:,4], label='ry')
plt.plot(computedPose_left[:,5], label='rz')
plt.legend()
plt.title('euler angles over trajectories')
-plt.show()
+plt.show() """
fig = plt.figure()
-plt.plot(desJointAngles_right[:,0], label='joint1')
-plt.plot(desJointAngles_right[:,1], label='joint2')
-plt.plot(desJointAngles_right[:,2], label='joint3')
-plt.plot(desJointAngles_right[:,3], label='joint4')
-plt.plot(desJointAngles_right[:,4], label='joint5')
-plt.plot(desJointAngles_right[:,5], label='joint6')
-plt.plot(desJointAngles_right[:,6], label='joint7')
+plt.plot(compJointAngles_right[:,0], label='joint1')
+plt.plot(compJointAngles_right[:,1], label='joint2')
+plt.plot(compJointAngles_right[:,2], label='joint3')
+plt.plot(compJointAngles_right[:,3], label='joint4')
+plt.plot(compJointAngles_right[:,4], label='joint5')
+plt.plot(compJointAngles_right[:,5], label='joint6')
+plt.plot(compJointAngles_right[:,6], label='joint7')
plt.title('joint values over samples, right arm')
plt.legend()
plt.show()
@@ -149,9 +152,9 @@ plt.scatter(computedPose_right[:,0], computedPose_left[:,2], label='resulting po
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.title('poses right')
plt.show()
-np.save('data/desJointAngles_left', desJointAngles_left)
-np.save('data/desJointAngles_right', desJointAngles_right)
+np.save('data/compJointAngles_left', compJointAngles_left)
+np.save('data/compJointAngles_right', compJointAngles_right)