Ros system Moveit play with Arms Robot Series (vi)--d-h inverse kinematics Solver (c + +)

Note: This blog post all source code is:Git Repo.

1. Source code is written in Ubuntu14.04 + Indigo environment.

First, the transformation matrix

After a blog introduction, we've got Rob an arm of the d-h parameter table, as shown in the table above, we want to convert these parameters to the transformation matrix of the adjacent coordinate system, D and h two-bit old-timers have deduced a general formula, general equation 1, where cθi = cos (θ i),sθi = sin (θi). This is a 4x4 matrix, it represents the position of the adjacent two coordinate system and the attitude of the two dimensions of the conversion relationship, specific instructions see the previous blog post.

Figure 1

To apply the formula in Figure 1, we get the transformation matrix T1 of the coordinate system {1} relative to the coordinate system {0} as follows:

Similarly, we get the coordinate system {2} transformation matrix T2relative to the coordinate system {1}, {3} with respect to the transformation matrix T2for {3}, {4} With respect to the transformation matrix T3for {4}, {5} With respect to the transformation matrix T of {4} 5, {6} With respect to the conversion matrix T6of {5}, respectively, as follows:

At this point, we get the transformation matrices for all neighboring coordinate systems, and since they are concatenated, we can get the transformation matrix of the coordinate system {6} relative to the coordinate system {0} T2:

Style 1

Where (S1 refers to θ1,s2 refers to θ2, etc.):

　　 Nx =-cos (S5) * (COS (S4) * (sin (S1) *sin (S3)-cos (s1) *cos (S2) *cos (S3)) + cos (s1) *sin (S2) *sin (S4))-Sin (S5) * (COS (S3) *sin (S1 ) + cos (s1) *cos (S2) *sin (S3));
　　 Ny = cos (s5) * (COS (S4) * (cos (s1) *sin (S3) + cos (s2) *cos (S3) *sin (S1))-Sin (S1) *sin (S2) *sin (S4)) + sin (S5) * (cos (s1) *cos (S3) -cos (S2) *sin (S1) *sin (S3));
　　 Nz = cos (s5) * (cos (S2) *sin (S4) + cos (S3) *cos (S4) *sin (S2))-Sin (S2) *sin (S3) *sin (S5);
　　 ox = sin (S5) * (COS (S4) * (sin (S1) *sin (S3)-cos (s1) *cos (S2) *cos (S3)) + cos (s1) *sin (S2) *sin (S4))-cos (S5) * (COS (S3) *sin (S1) + cos (s1) *cos (S2) *sin (S3));
　　 oy = cos (s5) * (cos (s1) *cos (S3)-cos (S2) *sin (S1) *sin (S3))-Sin (S5) * (COS (S4) * (cos (s1) *sin (S3) + cos (s2) *cos (S3) *sin (S1)) -Sin (S1) *sin (S2) *sin (S4));
　　 oz =-Sin (S5) * (cos (S2) *sin (S4) + cos (S3) *cos (S4) *sin (S2))-cos (S5) *sin (S2) *sin (S3);
　　 ax = sin (S4) * (sin (S1) *sin (S3)-cos (s1) *cos (S2) *cos (S3))-cos (S1) *cos (S4) *sin (S2);
　　 ay =-sin (S4) * (cos (s1) *sin (S3) + cos (s2) *cos (S3) *sin (S1))-cos (S4) *sin (S1) *sin (S2);
　　 az = cos (s2) *cos (S4)-cos (S3) *sin (S2) *sin (S4);
Px = 40*cos (S1) *cos (S2)-(764*cos (S1) *sin (S2))/5;

Py = 40*cos (s2) *sin (S1)-(764*sin (S1) *sin (S2))/5;

Pz = (764*cos (S2))/5 + 40*sin (S2);

Because of the time, temporarily do not write down the specific process, in fact, all the source code inside, you need to go to the beginning of the article code warehouse download code to see.

<--not yet finished--

　

Ros system Moveit play with Arms Robot Series (vi)--d-h inverse kinematics Solver (c + +)