DASL Wiki

User Tools

Site Tools

Trace:
robotic_manipulators_ik_cpp

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
robotic_manipulators_ik_cpp [2016/08/06 22:06] – joaomatosrobotic_manipulators_ik_cpp [2016/08/08 19:12] (current) – joaomatos
Line 1: Line 1:
 ===== Inverse Kinematics Using Damped Least Squares method in C++ ===== ===== Inverse Kinematics Using Damped Least Squares method in C++ =====
 +
 +**Author:** Joao Matos Email: <jcunha@id.uff.br> <!-- replace with your email address -->
 +\\
 +**Date:** Last modified on 6/8/2016
 +\\
 +**Keywords:** Inverse Kinematics , Damped Least Squares, Serial Arm
 +\\
  
  This application was made using Visual Studio 2015 and C++. Is necessary to include the Eigen library. To include the Eigen library just go to your solution properties , under C++ general properties , on the additional include directory include the folder where you saved the Eigen file.   This application was made using Visual Studio 2015 and C++. Is necessary to include the Eigen library. To include the Eigen library just go to your solution properties , under C++ general properties , on the additional include directory include the folder where you saved the Eigen file. 
Line 7: Line 14:
 {{::ikdls.rar| My source file is here.}} {{::ikdls.rar| My source file is here.}}
  
 +
 + To run this application just include the source given above or copy and paste the code bellow in your application
 +
 +
 +{{ ::ikcmd.jpg?direct |}}
  
 ---- ----
-===== Code with commentaries =====+===== Code with commentaries (copy and paste in your Visual Studio) ===== 
 + 
 +<Code:c++ linenums:1> 
 + 
 +#include <iostream> 
 +#include <Eigen/Dense> 
 +#include <cmath> 
 +#include <Windows.h> 
 +#include <sstream> 
 +#include <string> 
 + 
 + 
 + 
 +using namespace Eigen; 
 + 
 + 
 +int main() 
 +{ 
 + 
 + const float pi = 3.14159265; 
 + const float L1 = -11, L2 = 15, L3 = 10, L4 = -21, L5 = -8; 
 + float th1, th2, th3, th4, th5; 
 + float xt, yt, zt; 
 + int method; 
 + 
 + 
 + // Ask for the desired position (target position) 
 + std::cout << "Enter the Target x position " << std::endl; 
 + std::cin >> xt; 
 + 
 + std::cout << "Enter the Target y position " << std::endl; 
 + std::cin >> yt; 
 + 
 + std::cout << "Enter the Target z position " << std::endl; 
 + std::cin >> zt; 
 + 
 + // Ask the method to be used  
 + //Orientation control = keep the end effector in the vertical position 
 + //No Orientation control  
 + std::cout << "Enter 1 to run the IK with orientation control " << std::endl; 
 + std::cout << "Enter 2 to run the IK without orientation control " << std::endl; 
 + std::cout << "If you choose orientation Control and get an error " << std::endl; 
 + std::cout << "The desired position cant be reached keeping the end effector in the vertical " << std::endl; 
 + 
 + 
 + std::cin >> method; 
 + 
 + MatrixXf A1(4, 4); 
 + MatrixXf A2(4, 4); 
 + MatrixXf A3(4, 4); 
 + MatrixXf A4(4, 4); 
 + MatrixXf A5(4, 4); 
 + 
 + MatrixXf A12(4, 4); 
 + MatrixXf A123(4, 4); 
 + MatrixXf A1234(4, 4); 
 + MatrixXf A12345(4, 4); 
 + 
 + MatrixXf Jw(3, 5); 
 + MatrixXf Jv(3, 5); 
 + MatrixXf Jacobian(6, 5); 
 + 
 + MatrixXf Oeff(3, 1); 
 + MatrixXf O00(3, 1); 
 + MatrixXf O01(3, 1); 
 + MatrixXf O02(3, 1); 
 + MatrixXf O03(3, 1); 
 + MatrixXf O04(3, 1); 
 + 
 + 
 + MatrixXf rc1(3, 1); 
 + MatrixXf rc2(3, 1); 
 + MatrixXf rc3(3, 1); 
 + MatrixXf rc4(3, 1); 
 + MatrixXf rc5(3, 1); 
 + 
 + MatrixXf jwc1(3, 1); 
 + MatrixXf jwc2(3, 1); 
 + MatrixXf jwc3(3, 1); 
 + MatrixXf jwc4(3, 1); 
 + MatrixXf jwc5(3, 1); 
 + 
 + MatrixXf jv1(3, 1); 
 + MatrixXf jv2(3, 1); 
 + MatrixXf jv3(3, 1); 
 + MatrixXf jv4(3, 1); 
 + MatrixXf jv5(3, 1); 
 + 
 + MatrixXf thv1, thv2, thv3, thv4, thv5; 
 + MatrixXf gpcmd1(1, 1002), gpcmd2(1, 1002), gpcmd3(1, 1002), gpcmd4(1, 1002), gpcmd5(1, 1002); 
 + 
 + MatrixXf dgp1(1, 1002); 
 + MatrixXf dgp2(1, 1002); 
 + MatrixXf dgp3(1, 1002); 
 + MatrixXf dgp4(1, 1002); 
 + MatrixXf dgp5(1, 1002); 
 + 
 + 
 + // initial angles 
 + th1 = 0; 
 + th2 = 0; 
 + th3 = 0; 
 + th4 = 0; 
 + th5 = 0; 
 + 
 + float ev1 = 30; 
 + float ev2 = 30; 
 + float ev3 = 30; 
 + float i = 0; 
 + float k = 0; 
 + 
 + float gpi1 = 2292; 
 + float gpi2 = 2086; 
 + float gpi3 = 2007; 
 + float gpi4 = 538; 
 + float gpi5 = 509; 
 + 
 + 
 + if (method == 1) 
 + { 
 + bool status = true; 
 + while (status) 
 + { 
 +  
 + //Error threshold , to stop the iteration and accept a solution 
 + if (ev1 < 1 & ev2 < 1 & ev3 < 1) 
 + { 
 + status = false; 
 + } 
 + 
 + //If the solution is not converging , should stop the program 
 + if (i > 1000) 
 + { 
 + status = false; 
 + std::cout << "The IK took more than 1000 iteration to find the solution " << std::endl; 
 + std::cout << "The Arm can't reach the point with accuracy " << std::endl; 
 + std::cout << "Close the CMD window to exit " << std::endl; 
 + Sleep(1000000000); 
 + 
 + } 
 + 
 + 
 + //Calculting A1 
 + float a1p11 = cos(th1); 
 + float a1p12 = 0; 
 + float a1p13 = -sin(th1); 
 + float a1p14 = 0; 
 + 
 + float a1p21 = sin(th1); 
 + float a1p22 = 0; 
 + float a1p23 = cos(th1); 
 + float a1p24 = 0; 
 + 
 + float a1p31 = 0; 
 + float a1p32 = -1; 
 + float a1p33 = 0; 
 + float a1p34 = L1; 
 + 
 + float a1p41 = 0; 
 + float a1p42 = 0; 
 + float a1p43 = 0; 
 + float a1p44 = 1; 
 + 
 + A1 << a1p11, a1p12, a1p13, a1p14, 
 + a1p21, a1p22, a1p23, a1p24, 
 + a1p31, a1p32, a1p33, a1p34, 
 + a1p41, a1p42, a1p43, a1p44; 
 +  
 + 
 + //Calculting A2 
 + float a2p11 = cos(th2); 
 + float a2p12 = -sin(th2); 
 + float a2p13 = 0; 
 + float a2p14 = L2*cos(th2); 
 + 
 + float a2p21 = sin(th2); 
 + float a2p22 = cos(th2); 
 + float a2p23 = 0; 
 + float a2p24 = L2*sin(th2); 
 + 
 + float a2p31 = 0; 
 + float a2p32 = 0; 
 + float a2p33 = 1; 
 + float a2p34 = 0; 
 + 
 + float a2p41 = 0; 
 + float a2p42 = 0; 
 + float a2p43 = 0; 
 + float a2p44 = 1; 
 + 
 + A2 << a2p11, a2p12, a2p13, a2p14, 
 + a2p21, a2p22, a2p23, a2p24, 
 + a2p31, a2p32, a2p33, a2p34, 
 + a2p41, a2p42, a2p43, a2p44; 
 + 
 +  
 + 
 + //Calculting A3 
 + float a3p11 = cos(th3); 
 + float a3p12 = -sin(th3); 
 + float a3p13 = 0; 
 + float a3p14 = L3*cos(th3); 
 + 
 + float a3p21 = sin(th3); 
 + float a3p22 = cos(th3); 
 + float a3p23 = 0; 
 + float a3p24 = L3*sin(th3); 
 + 
 + float a3p31 = 0; 
 + float a3p32 = 0; 
 + float a3p33 = 1; 
 + float a3p34 = 0; 
 + 
 + float a3p41 = 0; 
 + float a3p42 = 0; 
 + float a3p43 = 0; 
 + float a3p44 = 1; 
 + 
 + A3 << a3p11, a3p12, a3p13, a3p14, 
 + a3p21, a3p22, a3p23, a3p24, 
 + a3p31, a3p32, a3p33, a3p34, 
 + a3p41, a3p42, a3p43, a3p44; 
 + 
 +  
 + 
 + //Calculting A4 
 + 
 + float a4p11 = cos(th4); 
 + float a4p12 = 0; 
 + float a4p13 = sin(th4); 
 + float a4p14 = 0; 
 + 
 + float a4p21 = sin(th4); 
 + float a4p22 = 0; 
 + float a4p23 = -cos(th4); 
 + float a4p24 = 0; 
 + 
 + float a4p31 = 0; 
 + float a4p32 = 1; 
 + float a4p33 = 0; 
 + float a4p34 = 0; 
 + 
 + float a4p41 = 0; 
 + float a4p42 = 0; 
 + float a4p43 = 0; 
 + float a4p44 = 1; 
 + 
 + A4 << a4p11, a4p12, a4p13, a4p14, 
 + a4p21, a4p22, a4p23, a4p24, 
 + a4p31, a4p32, a4p33, a4p34, 
 + a4p41, a4p42, a4p43, a4p44; 
 + 
 +  
 + //Calculting A5 
 + 
 + float a5p11 = cos(th5); 
 + float a5p12 = -sin(th5); 
 + float a5p13 = 0; 
 + float a5p14 = 0; 
 + 
 + float a5p21 = sin(th5); 
 + float a5p22 = cos(th5); 
 + float a5p23 = 0; 
 + float a5p24 = 0; 
 + 
 + float a5p31 = 0; 
 + float a5p32 = 0; 
 + float a5p33 = 1; 
 + float a5p34 = L4 + L5; 
 + 
 + float a5p41 = 0; 
 + float a5p42 = 0; 
 + float a5p43 = 0; 
 + float a5p44 = 1; 
 + 
 + 
 + A5 << a5p11, a5p12, a5p13, a5p14, 
 + a5p21, a5p22, a5p23, a5p24, 
 + a5p31, a5p32, a5p33, a5p34, 
 + a5p41, a5p42, a5p43, a5p44; 
 + 
 +  
 + 
 + 
 + 
 + //Calculating auxiliary matrices 
 + //A12 A123 A1234 A12345 
 + 
 + A12 = A1*A2; 
 + A123 = A1*A2*A3; 
 + A1234 = A1*A2*A3*A4; 
 + A12345 = A1*A2*A3*A4*A5; 
 + 
 + 
 + //Calculating Jacobian angular velocity (Jw) 
 + Jw(0, 0) = 0; 
 + Jw(1, 0) = 0; 
 + Jw(2, 0) = 1; 
 + 
 + Jw(0, 1) = A1(0, 2); 
 + Jw(1, 1) = A1(1, 2); 
 + Jw(2, 1) = A1(2, 2); 
 + 
 + Jw(0, 2) = A12(0, 2); 
 + Jw(1, 2) = A12(1, 2); 
 + Jw(2, 2) = A12(2, 2); 
 + 
 + Jw(0, 3) = A123(0, 2); 
 + Jw(1, 3) = A123(1, 2); 
 + Jw(2, 3) = A123(2, 2); 
 + 
 + Jw(0, 4) = A1234(0, 2); 
 + Jw(1, 4) = A1234(1, 2); 
 + Jw(2, 4) = A1234(2, 2); 
 + 
 +  
 + //Calculating Jacobian linear velocity 
 + 
 + //First calculate the second element of the cross product 
 + 
 + Oeff(0, 0) = A12345(0, 3); 
 + Oeff(1, 0) = A12345(1, 3); 
 + Oeff(2, 0) = A12345(2, 3); 
 + 
 + O00(0, 0) = 0; 
 + O00(1, 0) = 0; 
 + O00(2, 0) = 0; 
 + 
 + O01(0, 0) = A1(0, 3); 
 + O01(1, 0) = A1(1, 3); 
 + O01(2, 0) = A1(2, 3); 
 + 
 + O02(0, 0) = A12(0, 3); 
 + O02(1, 0) = A12(1, 3); 
 + O02(2, 0) = A12(2, 3); 
 + 
 + O03(0, 0) = A123(0, 3); 
 + O03(1, 0) = A123(1, 3); 
 + O03(2, 0) = A123(2, 3); 
 + 
 + O04(0, 0) = A1234(0, 3); 
 + O04(1, 0) = A1234(1, 3); 
 + O04(2, 0) = A1234(2, 3); 
 + 
 + //The second element is the difference between end effector position and frame i-1 position 
 + rc1 = Oeff - O00; 
 + rc2 = Oeff - O01; 
 + rc3 = Oeff - O02; 
 + rc4 = Oeff - O03; 
 + rc5 = Oeff - O04; 
 + 
 + 
 +  
 + // First element of the cross product 
 + // Is the same as the Jw  
 + jwc1(0, 0) = Jw(0, 0); 
 + jwc1(1, 0) = Jw(1, 0); 
 + jwc1(2, 0) = Jw(2, 0); 
 + 
 + jwc2(0, 0) = Jw(0, 1); 
 + jwc2(1, 0) = Jw(1, 1); 
 + jwc2(2, 0) = Jw(2, 1); 
 + 
 + jwc3(0, 0) = Jw(0, 2); 
 + jwc3(1, 0) = Jw(1, 2); 
 + jwc3(2, 0) = Jw(2, 2); 
 + 
 + jwc4(0, 0) = Jw(0, 3); 
 + jwc4(1, 0) = Jw(1, 3); 
 + jwc4(2, 0) = Jw(2, 3); 
 + 
 + jwc5(0, 0) = Jw(0, 4); 
 + jwc5(1, 0) = Jw(1, 4); 
 + jwc5(2, 0) = Jw(2, 4); 
 + 
 + 
 + //Converting to vector in order to perform the cross product 
 + float jwc1vp1 = jwc1(0, 0); 
 + float jwc1vp2 = jwc1(1, 0); 
 + float jwc1vp3 = jwc1(2, 0); 
 + Vector3f jwc1v(jwc1vp1, jwc1vp2, jwc1vp3); 
 + 
 + float rc1vp1 = rc1(0, 0); 
 + float rc1vp2 = rc1(1, 0); 
 + float rc1vp3 = rc1(2, 0); 
 + Vector3f rc1v(rc1vp1, rc1vp2, rc1vp3); 
 + 
 + float jwc2vp1 = jwc2(0, 0); 
 + float jwc2vp2 = jwc2(1, 0); 
 + float jwc2vp3 = jwc2(2, 0); 
 + Vector3f jwc2v(jwc2vp1, jwc2vp2, jwc2vp3); 
 + 
 + float rc2vp1 = rc2(0, 0); 
 + float rc2vp2 = rc2(1, 0); 
 + float rc2vp3 = rc2(2, 0); 
 + Vector3f rc2v(rc2vp1, rc2vp2, rc2vp3); 
 + 
 + float jwc3vp1 = jwc3(0, 0); 
 + float jwc3vp2 = jwc3(1, 0); 
 + float jwc3vp3 = jwc3(2, 0); 
 + Vector3f jwc3v(jwc3vp1, jwc3vp2, jwc3vp3); 
 + 
 + float rc3vp1 = rc3(0, 0); 
 + float rc3vp2 = rc3(1, 0); 
 + float rc3vp3 = rc3(2, 0); 
 + Vector3f rc3v(rc3vp1, rc3vp2, rc3vp3); 
 + 
 + float jwc4vp1 = jwc4(0, 0); 
 + float jwc4vp2 = jwc4(1, 0); 
 + float jwc4vp3 = jwc4(2, 0); 
 + Vector3f jwc4v(jwc4vp1, jwc4vp2, jwc4vp3); 
 + 
 + float rc4vp1 = rc4(0, 0); 
 + float rc4vp2 = rc4(1, 0); 
 + float rc4vp3 = rc4(2, 0); 
 + Vector3f rc4v(rc4vp1, rc4vp2, rc4vp3); 
 + 
 + float jwc5vp1 = jwc5(0, 0); 
 + float jwc5vp2 = jwc5(1, 0); 
 + float jwc5vp3 = jwc5(2, 0); 
 + Vector3f jwc5v(jwc5vp1, jwc5vp2, jwc5vp3); 
 + 
 + float rc5vp1 = rc5(0, 0); 
 + float rc5vp2 = rc5(1, 0); 
 + float rc5vp3 = rc5(2, 0); 
 + Vector3f rc5v(rc5vp1, rc5vp2, rc5vp3); 
 + 
 + //Do the cross product 
 + jv1 = jwc1v.cross(rc1v); 
 + jv2 = jwc2v.cross(rc2v); 
 + jv3 = jwc3v.cross(rc3v); 
 + jv4 = jwc4v.cross(rc4v); 
 + jv5 = jwc5v.cross(rc5v); 
 + 
 +  
 + // FIll Jv with the cross products calculated before 
 + Jv(0, 0) = jv1(0, 0); 
 + Jv(1, 0) = jv1(1, 0); 
 + Jv(2, 0) = jv1(2, 0); 
 + 
 + Jv(0, 1) = jv2(0, 0); 
 + Jv(1, 1) = jv2(1, 0); 
 + Jv(2, 1) = jv2(2, 0); 
 + 
 + Jv(0, 2) = jv3(0, 0); 
 + Jv(1, 2) = jv3(1, 0); 
 + Jv(2, 2) = jv3(2, 0); 
 + 
 + Jv(0, 3) = jv4(0, 0); 
 + Jv(1, 3) = jv4(1, 0); 
 + Jv(2, 3) = jv4(2, 0); 
 + 
 + Jv(0, 4) = jv5(0, 0); 
 + Jv(1, 4) = jv5(1, 0); 
 + Jv(2, 4) = jv5(2, 0); 
 + 
 + 
 + 
 + // Fill Jacobian  
 + // Jv is the upper part ( first three rows and n columns (n=number of joints) 
 + // Jw is the lower part ( last  three rows and n columns (n=number of joints) 
 + 
 + Jacobian(0, 0) = Jv(0, 0); 
 + Jacobian(1, 0) = Jv(1, 0); 
 + Jacobian(2, 0) = Jv(2, 0); 
 + Jacobian(3, 0) = Jw(0, 0); 
 + Jacobian(4, 0) = Jw(1, 0); 
 + Jacobian(5, 0) = Jw(2, 0); 
 + 
 + Jacobian(0, 1) = Jv(0, 1); 
 + Jacobian(1, 1) = Jv(1, 1); 
 + Jacobian(2, 1) = Jv(2, 1); 
 + Jacobian(3, 1) = Jw(0, 1); 
 + Jacobian(4, 1) = Jw(1, 1); 
 + Jacobian(5, 1) = Jw(2, 1); 
 + 
 + Jacobian(0, 2) = Jv(0, 2); 
 + Jacobian(1, 2) = Jv(1, 2); 
 + Jacobian(2, 2) = Jv(2, 2); 
 + Jacobian(3, 2) = Jw(0, 2); 
 + Jacobian(4, 2) = Jw(1, 2); 
 + Jacobian(5, 2) = Jw(2, 2); 
 + 
 + Jacobian(0, 3) = Jv(0, 3); 
 + Jacobian(1, 3) = Jv(1, 3); 
 + Jacobian(2, 3) = Jv(2, 3); 
 + Jacobian(3, 3) = Jw(0, 3); 
 + Jacobian(4, 3) = Jw(1, 3); 
 + Jacobian(5, 3) = Jw(2, 3); 
 + 
 + Jacobian(0, 4) = Jv(0, 4); 
 + Jacobian(1, 4) = Jv(1, 4); 
 + Jacobian(2, 4) = Jv(2, 4); 
 + Jacobian(3, 4) = Jw(0, 4); 
 + Jacobian(4, 4) = Jw(1, 4); 
 + Jacobian(5, 4) = Jw(2, 4); 
 + 
 + 
 + //Get end effector position 
 + float x = A12345(0, 3); 
 + float y = A12345(1, 3); 
 + float z = A12345(2, 3); 
 + 
 + 
 + //Calculate the "e" vector 
 + //Distance between end effector position and desired position 
 + MatrixXf evector(6, 1); 
 + evector(0, 0) = xt - x; 
 + evector(1, 0) = yt - y; 
 + evector(2, 0) = zt - z; 
 + evector(3, 0) = 0; 
 + evector(4, 0) = 0; 
 + evector(5, 0) = 0; 
 + 
 + // Calculate tranpose of Jacobian 
 + MatrixXf Jt(5, 6); 
 + Jt = Jacobian.transpose(); 
 + //Initial lambda constant to be used on the damped least squares equation 
 + int lambida = 1; 
 + lambida = lambida*lambida; 
 + 
 + // Create identity 6x6 
 + MatrixXf Id(6, 6); 
 + Id << 1, 0, 0, 0, 0, 0, 
 + 0, 1, 0, 0, 0, 0, 
 + 0, 0, 1, 0, 0, 0, 
 + 0, 0, 0, 1, 0, 0, 
 + 0, 0, 0, 0, 1, 0, 
 + 0, 0, 0, 0, 0, 1; 
 + 
 + 
 + //Calculate the coefficients of the damped least squares equation 
 + MatrixXf cof1(6, 6); 
 + cof1 = (Jacobian*Jt); 
 + 
 + MatrixXf cof2(6, 6); 
 + cof2 = (lambida*Id); 
 + 
 + MatrixXf cof(6, 6); 
 + cof = cof1 + cof2; 
 + 
 + MatrixXf invcof(6, 6); 
 + invcof = cof.inverse(); 
 + 
 + // Create the vector to hold the damped least squares solution 
 + MatrixXf thetavector(5, 1); 
 + 
 + //Apply damped least squares equation 
 + thetavector = Jt*invcof*evector; 
 + 
 + //Create variables to store the solution of this iteration 
 + float t1, t2, t3, t4, t5; 
 + 
 + t1 = thetavector(0, 0); 
 + t2 = thetavector(1, 0); 
 + t3 = thetavector(2, 0); 
 + t4 = thetavector(3, 0); 
 + t5 = thetavector(4, 0); 
 + 
 + //Convert to radians 
 + t1 = t1*pi / 180; 
 + t2 = t2*pi / 180; 
 + t3 = t3*pi / 180; 
 + t4 = t4*pi / 180; 
 + t5 = t5*pi / 180; 
 + 
 +  
 + // Update new angles to be used on the next iteration 
 + th1 = th1 + t1; 
 + th2 = th2 + t2; 
 + th5 = th5 + t5; 
 + th3 = 0;   // Physical limitation of DASL arm , is better to not move this joint 
 + //th3 = th3 + t3;   WE ARE USING POSITION CONTROL , SO WE  
 + //th4 = th4 + t4;   MUST IMPOSE SOME RESTRICTION ON THE ANGLES IN ORDER TO KEEP THE END EFFECTOR VERTICAL 
 +  
 + 
 + //Angles restrictions 
 + th4 = -th2 - th3; 
 +  
 + 
 + // Store the command to be send to the dynamixel 
 + //Dynamixel servo take goal position and not an "move to angle " input 
 + //Depending on the Dynamixel model , the resolution can change. 
 + dgp1(0, i) = th1*(1 / 0.088)*(180 / pi); 
 + dgp2(0, i) = th2*(1 / 0.088)*(180 / pi); 
 + dgp3(0, i) = th3*(1 / 0.088)*(180 / pi); 
 + dgp4(0, i) = th4*(1 / 0.29)*(180 / pi); 
 + dgp5(0, i) = th5*(1 / 0.29)*(180 / pi); 
 + 
 + 
 + //Get errors to compare to a threshold and see if the solution was found 
 + ev1 = evector(0, 0); 
 + ev2 = evector(1, 0); 
 + ev3 = evector(2, 0); 
 + ev1 = abs(ev1); 
 + ev2 = abs(ev2); 
 + ev3 = abs(ev3); 
 + 
 + //update counters  
 + k = i; 
 + i = i + 1; 
 + 
 + } 
 + 
 + //After finding the solution , check what is the final end effector position 
 + float xeff = A12345(0, 3); 
 + float yeff = A12345(1, 3); 
 + float zeff = A12345(2, 3); 
 + 
 + //Copy all the commands to be send to the dynamixel  
 + // As the dynamixels start in a initial goal position 
 + // we just need to add / subtract the goals given by each iteration 
 + for (int cc = 0; cc <= k; cc++) 
 + 
 + { 
 + gpcmd1(0, cc) = gpi1 - dgp1(0, cc); 
 + gpcmd2(0, cc) = gpi2 - dgp2(0, cc); 
 + gpcmd3(0, cc) = gpi3 - dgp3(0, cc); 
 + gpcmd4(0, cc) = gpi4 - dgp4(0, cc); 
 + gpcmd5(0, cc) = gpi5 - dgp5(0, cc); 
 +  
 + } 
 + 
 + 
 + } // end of while loop 
 + 
 +  
 +  
 + if (method == 2) 
 + { 
 + bool status = true; 
 + while (status) 
 + { 
 + 
 + //Error threshold , to stop the iteration and accept a solution 
 + if (ev1 < 1 & ev2 < 1 & ev3 < 1) 
 + { 
 + status = false; 
 + } 
 + 
 + //If the solution is not converging , should stop the program 
 + if (i > 1000) 
 + { 
 + status = false; 
 + std::cout << "The IK took more than 1000 iteration to find the solution " << std::endl; 
 + std::cout << "The Arm can't reach the point with accuracy " << std::endl; 
 + std::cout << "Close the CMD window to exit " << std::endl; 
 + Sleep(1000000000); 
 + 
 + } 
 + 
 + 
 + //Calculting A1 
 + float a1p11 = cos(th1); 
 + float a1p12 = 0; 
 + float a1p13 = -sin(th1); 
 + float a1p14 = 0; 
 + 
 + float a1p21 = sin(th1); 
 + float a1p22 = 0; 
 + float a1p23 = cos(th1); 
 + float a1p24 = 0; 
 + 
 + float a1p31 = 0; 
 + float a1p32 = -1; 
 + float a1p33 = 0; 
 + float a1p34 = L1; 
 + 
 + float a1p41 = 0; 
 + float a1p42 = 0; 
 + float a1p43 = 0; 
 + float a1p44 = 1; 
 + 
 + A1 << a1p11, a1p12, a1p13, a1p14, 
 + a1p21, a1p22, a1p23, a1p24, 
 + a1p31, a1p32, a1p33, a1p34, 
 + a1p41, a1p42, a1p43, a1p44; 
 + 
 + 
 + //Calculting A2 
 + float a2p11 = cos(th2); 
 + float a2p12 = -sin(th2); 
 + float a2p13 = 0; 
 + float a2p14 = L2*cos(th2); 
 + 
 + float a2p21 = sin(th2); 
 + float a2p22 = cos(th2); 
 + float a2p23 = 0; 
 + float a2p24 = L2*sin(th2); 
 + 
 + float a2p31 = 0; 
 + float a2p32 = 0; 
 + float a2p33 = 1; 
 + float a2p34 = 0; 
 + 
 + float a2p41 = 0; 
 + float a2p42 = 0; 
 + float a2p43 = 0; 
 + float a2p44 = 1; 
 + 
 + A2 << a2p11, a2p12, a2p13, a2p14, 
 + a2p21, a2p22, a2p23, a2p24, 
 + a2p31, a2p32, a2p33, a2p34, 
 + a2p41, a2p42, a2p43, a2p44; 
 + 
 + 
 + 
 + //Calculting A3 
 + float a3p11 = cos(th3); 
 + float a3p12 = -sin(th3); 
 + float a3p13 = 0; 
 + float a3p14 = L3*cos(th3); 
 + 
 + float a3p21 = sin(th3); 
 + float a3p22 = cos(th3); 
 + float a3p23 = 0; 
 + float a3p24 = L3*sin(th3); 
 + 
 + float a3p31 = 0; 
 + float a3p32 = 0; 
 + float a3p33 = 1; 
 + float a3p34 = 0; 
 + 
 + float a3p41 = 0; 
 + float a3p42 = 0; 
 + float a3p43 = 0; 
 + float a3p44 = 1; 
 + 
 + A3 << a3p11, a3p12, a3p13, a3p14, 
 + a3p21, a3p22, a3p23, a3p24, 
 + a3p31, a3p32, a3p33, a3p34, 
 + a3p41, a3p42, a3p43, a3p44; 
 + 
 + 
 + 
 + //Calculting A4 
 + 
 + float a4p11 = cos(th4); 
 + float a4p12 = 0; 
 + float a4p13 = sin(th4); 
 + float a4p14 = 0; 
 + 
 + float a4p21 = sin(th4); 
 + float a4p22 = 0; 
 + float a4p23 = -cos(th4); 
 + float a4p24 = 0; 
 + 
 + float a4p31 = 0; 
 + float a4p32 = 1; 
 + float a4p33 = 0; 
 + float a4p34 = 0; 
 + 
 + float a4p41 = 0; 
 + float a4p42 = 0; 
 + float a4p43 = 0; 
 + float a4p44 = 1; 
 + 
 + A4 << a4p11, a4p12, a4p13, a4p14, 
 + a4p21, a4p22, a4p23, a4p24, 
 + a4p31, a4p32, a4p33, a4p34, 
 + a4p41, a4p42, a4p43, a4p44; 
 + 
 + 
 + //Calculting A5 
 + 
 + float a5p11 = cos(th5); 
 + float a5p12 = -sin(th5); 
 + float a5p13 = 0; 
 + float a5p14 = 0; 
 + 
 + float a5p21 = sin(th5); 
 + float a5p22 = cos(th5); 
 + float a5p23 = 0; 
 + float a5p24 = 0; 
 + 
 + float a5p31 = 0; 
 + float a5p32 = 0; 
 + float a5p33 = 1; 
 + float a5p34 = L4 + L5; 
 + 
 + float a5p41 = 0; 
 + float a5p42 = 0; 
 + float a5p43 = 0; 
 + float a5p44 = 1; 
 + 
 + 
 + A5 << a5p11, a5p12, a5p13, a5p14, 
 + a5p21, a5p22, a5p23, a5p24, 
 + a5p31, a5p32, a5p33, a5p34, 
 + a5p41, a5p42, a5p43, a5p44; 
 + 
 + 
 + 
 + 
 + 
 + //Calculating auxiliary matrices 
 + //A12 A123 A1234 A12345 
 + 
 + A12 = A1*A2; 
 + A123 = A1*A2*A3; 
 + A1234 = A1*A2*A3*A4; 
 + A12345 = A1*A2*A3*A4*A5; 
 + 
 + 
 + //Calculating Jacobian angular velocity (Jw) 
 + Jw(0, 0) = 0; 
 + Jw(1, 0) = 0; 
 + Jw(2, 0) = 1; 
 + 
 + Jw(0, 1) = A1(0, 2); 
 + Jw(1, 1) = A1(1, 2); 
 + Jw(2, 1) = A1(2, 2); 
 + 
 + Jw(0, 2) = A12(0, 2); 
 + Jw(1, 2) = A12(1, 2); 
 + Jw(2, 2) = A12(2, 2); 
 + 
 + Jw(0, 3) = A123(0, 2); 
 + Jw(1, 3) = A123(1, 2); 
 + Jw(2, 3) = A123(2, 2); 
 + 
 + Jw(0, 4) = A1234(0, 2); 
 + Jw(1, 4) = A1234(1, 2); 
 + Jw(2, 4) = A1234(2, 2); 
 + 
 + 
 + //Calculating Jacobian linear velocity 
 + 
 + //First calculate the second element of the cross product 
 + 
 + Oeff(0, 0) = A12345(0, 3); 
 + Oeff(1, 0) = A12345(1, 3); 
 + Oeff(2, 0) = A12345(2, 3); 
 + 
 + O00(0, 0) = 0; 
 + O00(1, 0) = 0; 
 + O00(2, 0) = 0; 
 + 
 + O01(0, 0) = A1(0, 3); 
 + O01(1, 0) = A1(1, 3); 
 + O01(2, 0) = A1(2, 3); 
 + 
 + O02(0, 0) = A12(0, 3); 
 + O02(1, 0) = A12(1, 3); 
 + O02(2, 0) = A12(2, 3); 
 + 
 + O03(0, 0) = A123(0, 3); 
 + O03(1, 0) = A123(1, 3); 
 + O03(2, 0) = A123(2, 3); 
 + 
 + O04(0, 0) = A1234(0, 3); 
 + O04(1, 0) = A1234(1, 3); 
 + O04(2, 0) = A1234(2, 3); 
 + 
 + //The second element is the difference between end effector position and frame i-1 position 
 + rc1 = Oeff - O00; 
 + rc2 = Oeff - O01; 
 + rc3 = Oeff - O02; 
 + rc4 = Oeff - O03; 
 + rc5 = Oeff - O04; 
 + 
 + 
 + 
 + // First element of the cross product 
 + // Is the same as the Jw  
 + jwc1(0, 0) = Jw(0, 0); 
 + jwc1(1, 0) = Jw(1, 0); 
 + jwc1(2, 0) = Jw(2, 0); 
 + 
 + jwc2(0, 0) = Jw(0, 1); 
 + jwc2(1, 0) = Jw(1, 1); 
 + jwc2(2, 0) = Jw(2, 1); 
 + 
 + jwc3(0, 0) = Jw(0, 2); 
 + jwc3(1, 0) = Jw(1, 2); 
 + jwc3(2, 0) = Jw(2, 2); 
 + 
 + jwc4(0, 0) = Jw(0, 3); 
 + jwc4(1, 0) = Jw(1, 3); 
 + jwc4(2, 0) = Jw(2, 3); 
 + 
 + jwc5(0, 0) = Jw(0, 4); 
 + jwc5(1, 0) = Jw(1, 4); 
 + jwc5(2, 0) = Jw(2, 4); 
 + 
 + 
 + //Converting to vector in order to perform the cross product 
 + float jwc1vp1 = jwc1(0, 0); 
 + float jwc1vp2 = jwc1(1, 0); 
 + float jwc1vp3 = jwc1(2, 0); 
 + Vector3f jwc1v(jwc1vp1, jwc1vp2, jwc1vp3); 
 + 
 + float rc1vp1 = rc1(0, 0); 
 + float rc1vp2 = rc1(1, 0); 
 + float rc1vp3 = rc1(2, 0); 
 + Vector3f rc1v(rc1vp1, rc1vp2, rc1vp3); 
 + 
 + float jwc2vp1 = jwc2(0, 0); 
 + float jwc2vp2 = jwc2(1, 0); 
 + float jwc2vp3 = jwc2(2, 0); 
 + Vector3f jwc2v(jwc2vp1, jwc2vp2, jwc2vp3); 
 + 
 + float rc2vp1 = rc2(0, 0); 
 + float rc2vp2 = rc2(1, 0); 
 + float rc2vp3 = rc2(2, 0); 
 + Vector3f rc2v(rc2vp1, rc2vp2, rc2vp3); 
 + 
 + float jwc3vp1 = jwc3(0, 0); 
 + float jwc3vp2 = jwc3(1, 0); 
 + float jwc3vp3 = jwc3(2, 0); 
 + Vector3f jwc3v(jwc3vp1, jwc3vp2, jwc3vp3); 
 + 
 + float rc3vp1 = rc3(0, 0); 
 + float rc3vp2 = rc3(1, 0); 
 + float rc3vp3 = rc3(2, 0); 
 + Vector3f rc3v(rc3vp1, rc3vp2, rc3vp3); 
 + 
 + float jwc4vp1 = jwc4(0, 0); 
 + float jwc4vp2 = jwc4(1, 0); 
 + float jwc4vp3 = jwc4(2, 0); 
 + Vector3f jwc4v(jwc4vp1, jwc4vp2, jwc4vp3); 
 + 
 + float rc4vp1 = rc4(0, 0); 
 + float rc4vp2 = rc4(1, 0); 
 + float rc4vp3 = rc4(2, 0); 
 + Vector3f rc4v(rc4vp1, rc4vp2, rc4vp3); 
 + 
 + float jwc5vp1 = jwc5(0, 0); 
 + float jwc5vp2 = jwc5(1, 0); 
 + float jwc5vp3 = jwc5(2, 0); 
 + Vector3f jwc5v(jwc5vp1, jwc5vp2, jwc5vp3); 
 + 
 + float rc5vp1 = rc5(0, 0); 
 + float rc5vp2 = rc5(1, 0); 
 + float rc5vp3 = rc5(2, 0); 
 + Vector3f rc5v(rc5vp1, rc5vp2, rc5vp3); 
 + 
 + //Do the cross product 
 + jv1 = jwc1v.cross(rc1v); 
 + jv2 = jwc2v.cross(rc2v); 
 + jv3 = jwc3v.cross(rc3v); 
 + jv4 = jwc4v.cross(rc4v); 
 + jv5 = jwc5v.cross(rc5v); 
 + 
 + 
 + // FIll Jv with the cross products calculated before 
 + Jv(0, 0) = jv1(0, 0); 
 + Jv(1, 0) = jv1(1, 0); 
 + Jv(2, 0) = jv1(2, 0); 
 + 
 + Jv(0, 1) = jv2(0, 0); 
 + Jv(1, 1) = jv2(1, 0); 
 + Jv(2, 1) = jv2(2, 0); 
 + 
 + Jv(0, 2) = jv3(0, 0); 
 + Jv(1, 2) = jv3(1, 0); 
 + Jv(2, 2) = jv3(2, 0); 
 + 
 + Jv(0, 3) = jv4(0, 0); 
 + Jv(1, 3) = jv4(1, 0); 
 + Jv(2, 3) = jv4(2, 0); 
 + 
 + Jv(0, 4) = jv5(0, 0); 
 + Jv(1, 4) = jv5(1, 0); 
 + Jv(2, 4) = jv5(2, 0); 
 + 
 + 
 + 
 + // Fill Jacobian  
 + // Jv is the upper part ( first three rows and n columns (n=number of joints) 
 + // Jw is the lower part ( last  three rows and n columns (n=number of joints) 
 + 
 + Jacobian(0, 0) = Jv(0, 0); 
 + Jacobian(1, 0) = Jv(1, 0); 
 + Jacobian(2, 0) = Jv(2, 0); 
 + Jacobian(3, 0) = Jw(0, 0); 
 + Jacobian(4, 0) = Jw(1, 0); 
 + Jacobian(5, 0) = Jw(2, 0); 
 + 
 + Jacobian(0, 1) = Jv(0, 1); 
 + Jacobian(1, 1) = Jv(1, 1); 
 + Jacobian(2, 1) = Jv(2, 1); 
 + Jacobian(3, 1) = Jw(0, 1); 
 + Jacobian(4, 1) = Jw(1, 1); 
 + Jacobian(5, 1) = Jw(2, 1); 
 + 
 + Jacobian(0, 2) = Jv(0, 2); 
 + Jacobian(1, 2) = Jv(1, 2); 
 + Jacobian(2, 2) = Jv(2, 2); 
 + Jacobian(3, 2) = Jw(0, 2); 
 + Jacobian(4, 2) = Jw(1, 2); 
 + Jacobian(5, 2) = Jw(2, 2); 
 + 
 + Jacobian(0, 3) = Jv(0, 3); 
 + Jacobian(1, 3) = Jv(1, 3); 
 + Jacobian(2, 3) = Jv(2, 3); 
 + Jacobian(3, 3) = Jw(0, 3); 
 + Jacobian(4, 3) = Jw(1, 3); 
 + Jacobian(5, 3) = Jw(2, 3); 
 + 
 + Jacobian(0, 4) = Jv(0, 4); 
 + Jacobian(1, 4) = Jv(1, 4); 
 + Jacobian(2, 4) = Jv(2, 4); 
 + Jacobian(3, 4) = Jw(0, 4); 
 + Jacobian(4, 4) = Jw(1, 4); 
 + Jacobian(5, 4) = Jw(2, 4); 
 + 
 + 
 + //Get end effector position 
 + float x = A12345(0, 3); 
 + float y = A12345(1, 3); 
 + float z = A12345(2, 3); 
 + 
 + 
 + //Calculate the "e" vector 
 + //Distance between end effector position and desired position 
 + MatrixXf evector(6, 1); 
 + evector(0, 0) = xt - x; 
 + evector(1, 0) = yt - y; 
 + evector(2, 0) = zt - z; 
 + evector(3, 0) = 0; 
 + evector(4, 0) = 0; 
 + evector(5, 0) = 0; 
 + 
 + // Calculate tranpose of Jacobian 
 + MatrixXf Jt(5, 6); 
 + Jt = Jacobian.transpose(); 
 + //Initial lambda constant to be used on the damped least squares equation 
 + int lambida = 1; 
 + lambida = lambida*lambida; 
 + 
 + // Create identity 6x6 
 + MatrixXf Id(6, 6); 
 + Id << 1, 0, 0, 0, 0, 0, 
 + 0, 1, 0, 0, 0, 0, 
 + 0, 0, 1, 0, 0, 0, 
 + 0, 0, 0, 1, 0, 0, 
 + 0, 0, 0, 0, 1, 0, 
 + 0, 0, 0, 0, 0, 1; 
 + 
 + 
 + //Calculate the coefficients of the damped least squares equation 
 + MatrixXf cof1(6, 6); 
 + cof1 = (Jacobian*Jt); 
 + 
 + MatrixXf cof2(6, 6); 
 + cof2 = (lambida*Id); 
 + 
 + MatrixXf cof(6, 6); 
 + cof = cof1 + cof2; 
 + 
 + MatrixXf invcof(6, 6); 
 + invcof = cof.inverse(); 
 + 
 + // Create the vector to hold the damped least squares solution 
 + MatrixXf thetavector(5, 1); 
 + 
 + //Apply damped least squares equation 
 + thetavector = Jt*invcof*evector; 
 + 
 + //Create variables to store the solution of this iteration 
 + float t1, t2, t3, t4, t5; 
 + 
 + t1 = thetavector(0, 0); 
 + t2 = thetavector(1, 0); 
 + t3 = thetavector(2, 0); 
 + t4 = thetavector(3, 0); 
 + t5 = thetavector(4, 0); 
 + 
 + //Convert to radians 
 + t1 = t1*pi / 180; 
 + t2 = t2*pi / 180; 
 + t3 = t3*pi / 180; 
 + t4 = t4*pi / 180; 
 + t5 = t5*pi / 180; 
 + 
 + 
 + // Update new angles to be used on the next iteration  
 + th1 = th1 + t1; 
 + th2 = th2 + t2; 
 + th3 = 0;  // Physical limitation of DASL arm , is better to not move this joint 
 + th4 = th4 + t4; 
 + th5 = th5 + t5; 
 +  
 +  
 + 
 + // Store the command to be send to the dynamixel 
 + //Dynamixel servo take goal position and not an "move to angle " input 
 + //Depending on the Dynamixel model , the resolution can change. 
 + dgp1(0, i) = th1*(1 / 0.088)*(180 / pi); 
 + dgp2(0, i) = th2*(1 / 0.088)*(180 / pi); 
 + dgp3(0, i) = th3*(1 / 0.088)*(180 / pi); 
 + dgp4(0, i) = th4*(1 / 0.29)*(180 / pi); 
 + dgp5(0, i) = th5*(1 / 0.29)*(180 / pi); 
 + 
 + 
 + //Get errors to compare to a threshold and see if the solution was found 
 + ev1 = evector(0, 0); 
 + ev2 = evector(1, 0); 
 + ev3 = evector(2, 0); 
 + ev1 = abs(ev1); 
 + ev2 = abs(ev2); 
 + ev3 = abs(ev3); 
 + 
 + //update counters  
 + k = i; 
 + i = i + 1; 
 + 
 + } 
 + 
 + 
 + 
 + 
 + } // end of while loop 
 + 
 + 
 +   //After finding the solution , check what is the final end effector position 
 + float xeff = A12345(0, 3); 
 + float yeff = A12345(1, 3); 
 + float zeff = A12345(2, 3); 
 + 
 + //Copy all the commands to be send to the dynamixel  
 + // As the dynamixels start in a initial goal position 
 + // we just need to add / subtract the goals given by each iteration 
 + for (int cc = 0; cc <= k; cc++) 
 + 
 + { 
 + gpcmd1(0, cc) = gpi1 - dgp1(0, cc); 
 + gpcmd2(0, cc) = gpi2 - dgp2(0, cc); 
 + gpcmd3(0, cc) = gpi3 - dgp3(0, cc); 
 + gpcmd4(0, cc) = gpi4 - dgp4(0, cc); 
 + gpcmd5(0, cc) = gpi5 - dgp5(0, cc); 
 + 
 + } 
 + 
 +  //Write results on the CMD  
 + std::cout << " -------------------------" << std::endl; 
 + std::cout << " IK Algorithm found the solution ! " << std::endl; 
 + std::cout << " Took   " << i << "  iterations " << std::endl; 
 + std::cout << "Theta 1 :" << th1 * 180 / pi << std::endl; 
 + std::cout << "Theta 2 :" << th2 * 180 / pi << std::endl; 
 + std::cout << "Theta 3 :" << th3 * 180 / pi << std::endl; 
 + std::cout << "Theta 4 :" << th4 * 180 / pi << std::endl; 
 + std::cout << "Theta 5: " << th5 * 180 / pi << std::endl; 
 + 
 + std::cout << " The end effector position is " << std::endl; 
 + std::cout << "x=  " << xeff << "y=  " << yeff << "z=  " << zeff; 
 + std::cout << std::endl; 
 + std::cout << std::endl; 
 + std::cout << " The position entered was " << std::endl; 
 + std::cout << "x=  " << xt << "y=  " << yt << "z=  " << zt; 
 + std::cout << std::endl; 
 + std::cout << std::endl; 
 + 
 + std::cout << " The initial goals are " << std::endl; 
 + std::cout << gpi1 << std::endl; 
 + std::cout << gpi2 << std::endl; 
 + std::cout << gpi3 << std::endl; 
 + std::cout << gpi4 << std::endl; 
 + std::cout << gpi5 << std::endl; 
 + std::cout << std::endl; 
 + 
 + 
 + std::cout << " The final goals are " << std::endl; 
 + std::cout << gpcmd1(0, k) << std::endl; 
 + std::cout << gpcmd2(0, k) << std::endl; 
 + std::cout << gpcmd3(0, k) << std::endl; 
 + std::cout << gpcmd4(0, k) << std::endl; 
 + std::cout << gpcmd5(0, k) << std::endl; 
 + std::cout << std::endl; 
 + std::cout << "The path vector has " << k << " set of joint angles " << std::endl; 
 + 
 +  
 + return 0; 
 + 
 +} 
 + 
 + 
 + 
 + 
 + 
 + 
 + 
 +</Code>
robotic_manipulators_ik_cpp.1470546382.txt.gz · Last modified: by joaomatos