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

Next revision		Previous revision
robotic_manipulators_ik_cpp [2016/08/06 21:20] – created 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. 
 +
 +[[http://eigen.tuxfamily.org/index.php?title=Main_Page|Download the latest stable version 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 (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.1470543645.txt.gz · Last modified: by joaomatos