cleared all pointers from function arguments

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();
 

c++/src/main.cpp

+#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;
+}

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;

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()