From 4fa169a8c353a6b1b67e667f278dcdf19adfa652 Mon Sep 17 00:00:00 2001
From: Joschua Gosda <joschua.gosda@control.lth.se>
Date: Thu, 12 May 2022 14:54:57 +0200
Subject: [PATCH] IK works OMG
---
c++/gpm.cpp | 11 +++++++----
python/taskspace_placement/computeJoints_left.py | 12 ++++++++++--
2 files changed, 17 insertions(+), 6 deletions(-)
diff --git a/c++/gpm.cpp b/c++/gpm.cpp
index 21313a2..e3705e4 100644
--- a/c++/gpm.cpp
+++ b/c++/gpm.cpp
@@ -113,6 +113,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
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 GRADIENTS
// define min and max values for the joints of Yumi
@@ -172,12 +174,13 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
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;
- broccoli::core::math::solvePseudoInverseEquation(J, m_inverseWeighing, effectiveTaskSpaceInput, nullSpaceVelocity, m_activationFactorTaskSpace);
+ 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
- jointAnglesDelta << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
- // integrate value over one time step
- jointAnglesDelta *= dt;
+ 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;
diff --git a/python/taskspace_placement/computeJoints_left.py b/python/taskspace_placement/computeJoints_left.py
index 10b7432..7b1e4f4 100644
--- a/python/taskspace_placement/computeJoints_left.py
+++ b/python/taskspace_placement/computeJoints_left.py
@@ -64,8 +64,8 @@ computedPose = np.zeros((len(p1[:,0]),6))
error = np.zeros((len(p1[:,0]),6))
for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_total, dphi)): # loop through all the desired position of left arm
- desPose = np.concatenate((pos, phi_const), axis=0) # note that phi_const is used -> same orientation throughout the movement
- desVelocities = np.concatenate((vel, dphi_const), axis=0) # same here
+ desPose = np.concatenate((pos, phi), axis=0) # note that phi_const is used -> same orientation throughout the movement
+ desVelocities = np.concatenate((vel, phi_dot), axis=0) # same here
# call the c++ egm function, return joint values and resulting pose
result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, 1)
desJointAngles[index,:] = result[0] # computed joint values from IK
@@ -113,6 +113,14 @@ plt.legend()
plt.title('view on the x-z plane from the right arm side of yumi')
plt.show()
+fig = plt.figure()
+plt.plot(computedPose[:,3], label='rx')
+plt.plot(computedPose[:,4], label='ry')
+plt.plot(computedPose[:,5], label='rz')
+plt.legend()
+plt.title('euler angles over trajectories')
+plt.show()
+
np.save('desJointAngles_left', desJointAngles)
--
GitLab