pointer solution works :D

c++/src/gpm.cpp

@@ -16,11 +16,20 @@ 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 << "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();

c++/src/loadKinematicModel.cpp

@@ -19,6 +19,16 @@ void * loadKinematicModel(std::string path) {
     rl::mdl::Kinematic* my_pointer = &model_kin;
     std::cout << "pointer in func "<< my_pointer << std::endl;
 
+    /*
+    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;
+    */
+
 	return (void*) my_pointer;
     //return kinematic;
 }

python/taskspace_placement/computeJoints.py

@@ -34,7 +34,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")
+#kinematic_model_ptr_R = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
 
 
 # loop for the left arm
@@ -61,7 +61,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) 
     result = gpm(desPose, desVelocities, jointAngles, jointVelocities, kinematic_model_ptr_R)
@@ -71,7 +71,7 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)):
         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()