Skip to content
Snippets Groups Projects
Commit 0d013db7 authored by Joschua Gosda's avatar Joschua Gosda
Browse files

pointer solution works :D

parent 903dce34
Branches
No related tags found
1 merge request!2Cleanup
...@@ -17,10 +17,19 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo ...@@ -17,10 +17,19 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// FORWARD KINEMATICS // FORWARD KINEMATICS
rl::mdl::Kinematic* kinematic = (rl::mdl::Kinematic*) kinematic_ptr; rl::mdl::Kinematic* kinematic = (rl::mdl::Kinematic*) kinematic_ptr;
std::cout << "pointer in gpm" << kinematic_ptr << std::endl; std::cout << "pointer in gpm" << kinematic_ptr << std::endl;
//rl::mdl::Kinematic* kinematic = kinematicPtr; //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 // forward kinematics for the right arm
kinematic->setPosition(joint_anlges);
kinematic->forwardPosition();
kinematic->calculateJacobian();
std::cout << kinematic->getJacobian() << std::endl;
*/
kinematic->setPosition(jointAngles); kinematic->setPosition(jointAngles);
kinematic->forwardPosition(); kinematic->forwardPosition();
kinematic->calculateJacobian(); kinematic->calculateJacobian();
......
...@@ -19,6 +19,16 @@ void * loadKinematicModel(std::string path) { ...@@ -19,6 +19,16 @@ void * loadKinematicModel(std::string path) {
rl::mdl::Kinematic* my_pointer = &model_kin; rl::mdl::Kinematic* my_pointer = &model_kin;
std::cout << "pointer in func "<< my_pointer << std::endl; 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 (void*) my_pointer;
//return kinematic; //return kinematic;
} }
...@@ -34,7 +34,7 @@ computedPose_left = np.zeros((len(p1[:,0]),6)) ...@@ -34,7 +34,7 @@ computedPose_left = np.zeros((len(p1[:,0]),6))
error_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_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 # loop for the left arm
...@@ -61,7 +61,7 @@ error_right = np.zeros((len(p1[:,0]),6)) ...@@ -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 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 # 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) desPose = np.concatenate((pos, phi), axis=0)
desVelocities = np.concatenate((vel, phi_dot), 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, kinematic_model_ptr_R)
...@@ -71,7 +71,7 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)): ...@@ -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 jointVelocities = (desJointAngles_left[index, :] - desJointAngles_left[index-1, :])/dt
print('\n error', desPose - result[1]) print('\n error', desPose - result[1])
error_right[index, :] = desPose - result[1] error_right[index, :] = desPose - result[1]
jointAngles = result[0] jointAngles = result[0] """
# see development of joint values # see development of joint values
fig = plt.figure() fig = plt.figure()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment