User Tools

Site Tools


robotic_manipulators_ik

This is an old revision of the document!


Inverse Kinematics Solver using Damped Least Squares Method

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:

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:

Which is:

And can be rewritten as:

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

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

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:

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.


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:

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:

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 ,C++ 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

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.

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

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.

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 FIRST PICTURE OF THIS 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 ).

robotic_manipulators_ik.1470451001.txt.gz · Last modified: 2016/08/05 19:36 by joaomatos