DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
===== Inverse Kinematics Solver using Damped Least Squares Method =====

**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 tutorial presents an algorithm to implement the Damped Least Squares method to calculate Inverse Kinematics problems.



----


 Between many algorithm to solve the Inverse Kinematics problem , the damped least squares method avoids many of the problems with the peusoinverse algorithm , and can give a good nummerical solution for the problem.The goal is to find the value of delta theta that minimizes the quantity: 

 {{ ::ikeq1.jpg?direct |}}

 Where Lambda is a non-zero damping constant (chosen to be large enough to make the solution behave well near singularities but not too large to make the convergence rate slow). The previous equation is equivalent to:

 {{ ::ikeq2.jpg?direct |}}

Which is:

 {{ ::ikeq3.jpg?direct |}}

And can be rewritten as:

{{ ::ikeq4.jpg?direct |}}

The inverse kinematics solution is the joint angles, so we need to isolate theta:

{{ ::ikeq5.jpg?direct |}}

 Reorganizing the equation, the final equation to be used on the damped least squares algorithm is:

{{ ::ikeq6.jpg?direct |}}


 As can be seen , the only "problem" here is to find the Jacobian Matrix. After we get the Jacobian , we just need to iterate and find the solution. 
 The DASL serial arm is to perform peg-in-hole tasks , so for this task we can neglect the end-effector orientation , so the "e" vector can be written as:

 {{ ::ikeq7.jpg?direct |}}


 The Jacobian and the End Effector position can be found analyzing the homogeneous transformation matrices of the Serial Arm. This transformations matrices can be found inputting the Denavit-Hartenberg parameters for each link. The next section will explain how to get the Jacobian matrix and what information is inside the Homogeneous transformation matrices.


----
===== Pseudo Algorithm =====

  * [[robotic_manipulators_ik_cpp|Inverse Kinematics Using Damped Least Squares method in Cpp]]
    * Example of the method programmed in Cpp using Visual Studio and Eigen library
  * [[robotic_manipulators_ik_matlab|Inverse Kinematics Using Damped Least Squares method in Matlab]] 
    * Example of the method programmed in Matlab and Simulated by RVC toolbox

 These two codes above were programmed using the same algorithm. The pseudo algorithm used is shown bellow.

  - Calculate A1,A2,A3,A4 and A5 given the initial position.
  - Calculate auxiliary matrices A1.A2 , A1.A2.A3, A1.A2.A3.A4 , A1.A2.A3.A4.A5.
  - Fill Jw getting the elements from the auxiliary matrices.
  - Fill Jv getting the elements from the auxiliary matrices.
  - Fill the actual Jacobian , using Jw(lower part) and Jv(upper part)
  - Get the end effector position on the given angles
  - Calculate "error" vector
  - Get the joint angles as solution of the Damped Least Squares equation
  - Update the joint angles ( new initial position )
  - Calculate error and compare to threshold (should stop iterate or not)
  - if the error (distance from end effector to desired position ) still high , go back to 1.






----
===== Getting the Homogeneous Transformation matrices =====

 One of the benefits of using the Denavit-Hartenberg parameters is that we can get the homogeneous transformation matrices just inputting each link's parameters. The transformation from frame j-1 to frame j is defined by the matrix:

 {{ ::5_manip.jpg?direct |}}

 For example , the DASL serial arm has 5 joints, so we will get A1 ,A2,A3,A4 and A5 , inputting the values that we have in our Denavit - Hartenberg table (A1 uses n=1 row , and so on...) , as shown below:
 
 {{ ::ded6.jpg?direct |}}


 The first step to code your IK solver is to write down the equation to get this 5 matrices. For example , in my code i have (note that i already substituted the alpha values (because this way a lot of elements will be 0 because cos(pi/2)=cos(-pi/2)=0).


Running in Visual Studio ,Cpp and Eigen library (library to deal with matrices).
<Code>

                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;

</Code>


**Informations inside the homogeneous transformation matrix**

 We have good informations on this matrix , the third column is the rotation of the frame j about the frame j-1 , and the fourth column is the displacement of the frame j in relation with the frame j-1

{{ ::ikeq9.jpg?direct |}}


 Now that you coded the 5 transformation matrices, we can start to find the elements of the Jacobian angular and linear velocity.




----

===== Finding the Jacobian Matrix =====

The Jacobian matrix can be found putting two different Jacobian matrices together.Jw is the Jacobian angular velocity and Jv is the Jacobian linear velocity. Jv will be the upper part of the final Jacobian matrix (first three rows and n columns , where n is the number of joints. Jw will be the lower part (last three rows and n columns, where n is the number of joints again.

{{ ::ikeq8.jpg?direct |}}


**Jacobian angular velocity**

 Each column of the Jacobian Angular velocity for revolute joints is defined by:

{{ ::ikeq10.jpg?direct |}}

 If you followed the tutorial about finding the Denavit-Hartenberg parameters , recall that we defined "z" as the axis of rotation of each joint , this way "k" , the rotation vector , will be the "z" vector , (0,0,1) transposed.
{{ ::ikeq11.jpg?direct |}}
 
 Be careful here , the rotation matrix R , is from frame 0 to frame i-1 , but our homogeneous transformation matrices are from frame j-1 to frame j. For example , A1 is from frame 0 to frame 1 , A2 is from frame 1 to frame 2. But to find the Jacobian linear velocity we will need frame 0 to frame 2 , frame 0 to frame 3  , and so on... We can easily find this multiplying the transformation matrices. This way ,R(00) is the k vector itself ,  R(01) come from A1 , R(02) come from A1.A2 , R(03) come from A1.A2.A3 , R(04) come from A1.A2.A3.A4 . 

 Note here that the R matrix will multiply the "k" vector , (0,0,1) transposed , this way, the information we need is in the third column of the transformation matrix ( third column and first third rows, the last row does not contain any information , is just a neutral row ). **THIS IS SIGNED IN RED IN THE LAST PICTURE OF LAST SECTION**

 So, the next step of the IK code is to find the result from the homogeneous transformation matrices multiplication. Just create new matrices , for example:

<Code>
                A12 = A1*A2;
		A123 = A1*A2*A3;
		A1234 = A1*A2*A3*A4;
		A12345 = A1*A2*A3*A4*A5;

</Code>

 Now we just need to get the third column and first three rows of each of these multiplication results to create our Jacobian Linear velocity. For example : ( Using the Eigen library , the first element is a(0,0) , this way , the third column is actually a(..,2) , and the first three rows are rows 0 , 1 and 2.

<Code>

//Finding the five columns of Jacobian angular velocity

		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);

</Code>
 
 All set , we just found the lower part of the Jacobian matrix , (last three rows and n columns ).

 **Jacobian Linear Velocity**

 The upper part is a little bit more complicated to be found than the Jacobian angular velocity. Each column of Jv is defined by:

 {{ ::ikeq12.jpg?direct |}}

 Note here that we have a cross product. The first element of the cross product is the same element from the correspondent column in Jw ( we already have it calculated ). The second element of the cross product is the distance between the position of the end effector in relation with the base and the distance between the frame (i-1) in relation with the base.

 As the referential is the base , we need to use the multiplication of the homogeneous matrices to get these elements ( A1, A1.A2 , A1.A2.A3 , A1.A2.A3.A4 ). These elements are located in the first three rows and fourth column of the homogeneous transformation matrices. THIS IS SIGNED IN BLUE IN THE MATRIX PICTURE.

 The end effector position can be found in the multiplication A1.A2.A3.A4.A5 . O(00) is 0 because is the distance from frame 0 to frame 0. 0(01)can be found in A1, O(02) can be found in A1.A2 , O(03) can be found in A1.A2.A3 , O(04) can be found in A1.A2.A3.A4.

 For example (is convenient to define a vector with the difference - the second term of the cross product ) to organize better the code:
  
 <Code>
                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);

		rc1 = Oeff - O00;
		rc2 = Oeff - O01;
		rc3 = Oeff - O02;
		rc4 = Oeff - O03;
		rc5 = Oeff - O04;

</Code>

 Now we just have to do the cross product, and create the Jv matrix. The eigen library only does cross product with vectors , so before doing the cross product we need to convert these matrices to vectors. See bellow:

<Code>

               // Columns from Jw (first element of the cross product)
		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 the matrices into vectors to do 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);
 
                //Cross product.
		jv1 = jwc1v.cross(rc1v);
		jv2 = jwc2v.cross(rc2v);
		jv3 = jwc3v.cross(rc3v);
		jv4 = jwc4v.cross(rc4v);
		jv5 = jwc5v.cross(rc5v);

</Code>

 Now with the cross product ready , just need to fill the Jv matrix and the upper part of the Jacobian matrix is ready:

<Code>

// FIll Jv

		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);
</Code>

 Finally we already have Jw and Jv all calculated , now just need to fill the actual Jacobian matrix.

<Code>
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);
</Code>


----

===== Applying the Damped Least Squares method =====

 Until now we calculated the Jacobian Matrix to be used in the equation from the damped least squares method. However , we still need to get the "e" vector that describes the displacement from the end effector position to the desired position. This is easily found because we can get the end effector position in the fourth column and first three rows of thetransformation matrix A1.A2.A3.A4.A5

<Code>
                //Get end effector x,y,z position
                float x = A12345(0, 3);
		float y = A12345(1, 3);
		float z = A12345(2, 3);


               // Create the vector "e". xt,yt and zt are the target coordinates
		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;
</Code>

We will also need the transpose of the Jacobian , an identity matrix 6x6 and calculate the product that is present in the damped least squares method. Doing it step by step:

<Code>
                //Creating the Transpose of the Jacobian
                MatrixXf Jt(5, 6);
		Jt = Jacobian.transpose();
                
                //Defining lambda
		int lambda = 1;
		lambda = lambda*lambda;

                //Creating an 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;

		
               //Calculating the product of matrices present in the DLS method
		MatrixXf cof1(6, 6);
		cof1 = (Jacobian*Jt);

		MatrixXf cof2(6, 6);
		cof2 = (lambda*Id);

		MatrixXf cof(6, 6);
		cof = cof1 + cof2;

		MatrixXf invcof(6, 6);
		invcof = cof.inverse();

                //Creating the matrix to hold the joint angles solution
		MatrixXf thetavector(5, 1);

                //Actual Damped Least Squares method formula
		thetavector = Jt*invcof*evector;

</Code>

 The last line of this code is the formula calculation:

{{ ::ikeq6.jpg?direct |}}

 The solution of each iteration will be hold into this "thetavector" variable. The way that the algorithm works is in each iteration , the "e" vector will get lower , and setting a threshold ( for example stop when the error is less than 1cm ) the algorithm will stop to make this "e" lower and will give the joint angles to reach the desired end effector pose.

 If the algorithm took 100 iterations to find the solution - find the joint angles that makes the "e" vector get bellow a defined threshold - we will have 100 set of joint angles that will move the end effector from a initial to the desired position. This way , is useful to save all these set of joint angles so you can send all this angles to your servo-motor and make a gradual movement (and not send only go to final position).

 Storing the theta values (we will have 5 angles given by the solution , one for each joint ) :

<Code>
                //Get the angles from the solution
		t1 = thetavector(0, 0);
		t2 = thetavector(1, 0);
		t3 = thetavector(2, 0);
		t4 = thetavector(3, 0);
		t5 = thetavector(4, 0);
		
                 //Convert from degree to radians
		t1 = t1*pi / 180;
		t2 = t2*pi / 180;
		t3 = t3*pi / 180;
		t4 = t4*pi / 180;
		t5 = t5*pi / 180;


</Code>
 
 For the next iteration , the initial position will not be the same , we need to update the initial position from the last position plus the delta theta given by the solution.

 Note that in my code i put a restriction on some joint angles , in order to keep the end effector in the vertical position (to perform the peg-in-hole in a horizontal hole , the end effector orientation must be perpendicular to the hole , so needs to be vertical all the time ).

<Code>
                //Updating the initial position for the next iteration
                th1 = th1 + t1;
		th2 = th2 + t2;
		th3 = th3 + t3;
		th5 = th5 + t5;
                
                //Restriction on the joint angles to keep the end effector vertical
		th4 = -th2 - th3;
		

</Code>
 
 Now we want to store the commands that will be send to the dynamixel servos. Each dynamixel has its own resolution , for example , 1 goal position command = 0.088 degrees or 0.29 depending on the model. This way , the command send to the dynamixel to move to a desired angle is not given by angles , and yes given by "goal position ". Another example , if we want to move a dynamixel with resolution 0.088 degrees in 50 degrees, we need to send to the servo the goal position of 50/0.088 = 568. See each command bellow:

<Code> 
                //Store each goal position to be send to the servo
                //goal position = angles in degrees * resolution * convert to degrees
                // Each servo (numbered as 1 to 5 ) will have its own vector of goal positions
                
                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);

</Code>

 Finally , the last step of the algorithm is to calculate the "e" vector - the position error between the actual position of the end effector and the target position:

<Code>
                //Get errors from the "e" vector
		ev1 = evector(0, 0);
		ev2 = evector(1, 0);
		ev3 = evector(2, 0);
 
                //values to be compared with a threshold to see if needs to iterate another time or not
		ev1 = abs(ev1);
		ev2 = abs(ev2);
		ev3 = abs(ev3);
</Code>
 
 This is the last step , so your IK solver using Damped Least Squares method should be running OK!.