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

IK works OMG

parent fcac1a32
No related branches found
No related tags found
1 merge request!1all code snippets work
...@@ -113,6 +113,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo ...@@ -113,6 +113,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
effectiveTaskSpaceInput.head(3) = gainDriftCompensation/dt * (desiredTranslation - currentTranslation) effectiveTaskSpaceInput.head(3) = gainDriftCompensation/dt * (desiredTranslation - currentTranslation)
+ desVelocity.head(3); + desVelocity.head(3);
effectiveTaskSpaceInput.tail(3) = gainDriftCompensation/dt * errorRotationInWorldFrame + desVelocity.tail(3); effectiveTaskSpaceInput.tail(3) = gainDriftCompensation/dt * errorRotationInWorldFrame + desVelocity.tail(3);
std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
// COMPUTE GRADIENTS // COMPUTE GRADIENTS
// define min and max values for the joints of Yumi // 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 ...@@ -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(); Eigen::Matrix<double, 7, 7> m_inverseWeighing = Eigen::Matrix<double, 7, 7> ::Identity();
double m_activationFactorTaskSpace = 1.0; double m_activationFactorTaskSpace = 1.0;
Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * nullSpaceGradient; 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 // 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; jointAnglesDelta << jointVelocities * dt;
// integrate value over one time step
jointAnglesDelta *= dt;
//std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl; //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; std::cout << "delta qs in DEG \n" << jointAnglesDelta * rl::math::RAD2DEG << std::endl;
......
...@@ -64,8 +64,8 @@ computedPose = np.zeros((len(p1[:,0]),6)) ...@@ -64,8 +64,8 @@ computedPose = np.zeros((len(p1[:,0]),6))
error = 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 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 desPose = np.concatenate((pos, phi), axis=0) # note that phi_const is used -> same orientation throughout the movement
desVelocities = np.concatenate((vel, dphi_const), axis=0) # same here desVelocities = np.concatenate((vel, phi_dot), axis=0) # same here
# call the c++ egm function, return joint values and resulting pose # call the c++ egm function, return joint values and resulting pose
result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, 1) result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, 1)
desJointAngles[index,:] = result[0] # computed joint values from IK desJointAngles[index,:] = result[0] # computed joint values from IK
...@@ -113,6 +113,14 @@ plt.legend() ...@@ -113,6 +113,14 @@ plt.legend()
plt.title('view on the x-z plane from the right arm side of yumi') plt.title('view on the x-z plane from the right arm side of yumi')
plt.show() 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) np.save('desJointAngles_left', desJointAngles)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment