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