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

Source: Internet
Author: User
Tags cos sin

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

Related Article

Contact Us

The content source of this page is from Internet, which doesn't represent Alibaba Cloud's opinion; products and services mentioned on that page don't have any relationship with Alibaba Cloud. If the content of the page makes you feel confusing, please write us an email, we will handle the problem within 5 days after receiving your email.

If you find any instances of plagiarism from the community, please send an email to: info-contact@alibabacloud.com and provide relevant evidence. A staff member will contact you within 5 working days.

A Free Trial That Lets You Build Big!

Start building with 50+ products and up to 12 months usage for Elastic Compute Service

  • Sales Support

    1 on 1 presale consultation

  • After-Sales Support

    24/7 Technical Support 6 Free Tickets per Quarter Faster Response

  • Alibaba Cloud offers highly flexible support services tailored to meet your exact needs.