diff --git a/c++/include/yumi.hpp b/c++/include/yumi.hpp
index 28f8410505583b81a2857dc6049cdcae9b905077..d3612ab2d2ec172cab262a39b5dc184ee283cccd 100644
--- a/c++/include/yumi.hpp
+++ b/c++/include/yumi.hpp
@@ -59,7 +59,7 @@ class Yumi {
Eigen::Matrix<double, 7, 1> m_jointAnglesDelta;
// private functions
- void doForwardKinematics(rl::mdl::Kinematic* kinematic);
+ void doForwardKinematics();
Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);
void compTaskSpaceInput();
diff --git a/c++/src/main.cpp b/c++/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0587fe14c331c93897b71411ff9bafbda062165c
--- /dev/null
+++ b/c++/src/main.cpp
@@ -0,0 +1,76 @@
+#include <iostream>
+#include "yumi.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**) {
+
+ std::string path2yumi_l = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf";
+ Yumi yumi_l(path2yumi_l);
+
+ 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);
+
+
+ // Desired Values
+ Eigen::Matrix<double, 6, 1> desPose;
+ desPose << 0.300, 0.200, 0.200, 0.0*rl::math::DEG2RAD, 0.0*rl::math::DEG2RAD, 0.0*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;
+
+ yumi_l.set_desPoseVel(desPose, desVelocity);
+
+ yumi_l.process();
+
+
+ Eigen::Matrix<double, 7, 1> newJointValues;
+ newJointValues = yumi_l.get_newJointValues();
+
+ std::cout << "old joint values: " << jointAngles << std::endl;
+ std::cout << "new joint values: " << newJointValues << std::endl;
+
+
+
+ //enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
+
+
+/* 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());
+
+ // 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, kinematic);
+
+ std::cout << "desired joint values: \n" << result.first << std::endl;
+ std::cout << "current pose: \n" << result.second << std::endl; */
+
+ return 0;
+}
diff --git a/c++/src/yumi.cpp b/c++/src/yumi.cpp
index c835410e192007b1efed795fffe639e913a62560..bc7315d67f4225ffff4796889072b4724756ce90 100644
--- a/c++/src/yumi.cpp
+++ b/c++/src/yumi.cpp
@@ -24,17 +24,17 @@ void Yumi::set_jointValues(Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matr
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);
+void Yumi::doForwardKinematics(){
+ m_kinematic->setPosition(m_jointAngles);
+ m_kinematic->forwardPosition();
+ m_kinematic->calculateJacobian();
+ rl::math::Transform t = m_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);
+ doForwardKinematics();
std::cout << "pose " << m_position << "\n" << m_orientation << std::endl;
}
@@ -84,7 +84,7 @@ void Yumi::compTaskSpaceInput(){
void Yumi::process(){
- doForwardKinematics(&m_kin_model);
+ doForwardKinematics();
compTaskSpaceInput();
Eigen::Matrix<double, 7, 1> jointVelocities;
@@ -102,7 +102,7 @@ Eigen::Matrix<double, 7, 1> Yumi::get_newJointValues(){
}
Eigen::Matrix<double, 6, 1> Yumi::get_newPose(){
- doForwardKinematics(&m_kin_model);
+ doForwardKinematics();
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 8939414ae0307f187b38c610ca80e18f45fcfbe5..d1e40d98b02a1363af69333fc937595de70c7e85 100644
--- a/python/taskspace_placement/computeJoints.py
+++ b/python/taskspace_placement/computeJoints.py
@@ -64,7 +64,7 @@ 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)
yumi_right.set_jointValues(jointAngles, jointVelocities)
@@ -77,7 +77,7 @@ jointAngles = np.array([-110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0]) * np
if index > 0:
jointVelocities = (compJointAngles_left[index, :] - compJointAngles_left[index-1, :])/dt
error_right[index, :] = desPose - computedPose_right[index, :]
- jointAngles = compJointAngles_right[index,:] """
+ jointAngles = compJointAngles_right[index,:]
# see development of joint values
fig = plt.figure()