DASL Wiki

Book Creator
Add this page to your book

Book Creator
Remove this page from your book

This is an old revision of the document!

Inverse Kinematics Using Damped Least Squares method in C++

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]] 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 —- ===== 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>