User Tools

Site Tools


2_link_kinematics

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision
Previous revision
2_link_kinematics [2018/09/10 21:44] – [Graphical Simulation] ntorresreyes2_link_kinematics [2018/09/27 01:12] (current) – [Graphical Simulation] ntorresreyes
Line 10: Line 10:
 <!-- Add a representative photo of your tutorial below.  Make it centered in the page --> <!-- Add a representative photo of your tutorial below.  Make it centered in the page -->
  
-{{ torres:tutorials:2_link_tutorial_1.png?200 |}}+{{ torres:tutorials:2_link_tutorial_1.png?400 |}}
  
 \\ \\
Line 30: Line 30:
 The rest of this tutorial is presented as follows: The rest of this tutorial is presented as follows:
   * Background And Theory   * Background And Theory
-  * Numerical Simulation+  * Analytical Solution 
 +  * Numerical Solution
   * Graphical Simulation   * Graphical Simulation
   * Final Words   * Final Words
Line 37: Line 38:
  
 This section provides a short background of the problem and basic theory used. This section provides a short background of the problem and basic theory used.
 +\\
 +\\
 +** Homogeneous Transformations **
 +\\
 +\\
 +{{:torres:tutorials:2_link_tutorial_9.png?450|}}
 +\\
 +\\
 +In this case, we will only consider 2-dimensional homogeneous transformations. First, we must introduce homogeneous coordinates:
 +\\
 +\\
 +Vector v = [x y w]<sup>T</sup> , where scale factor w = 1
 +\\
 +\\
 +These coordinates are useful in applying rotations and translations and make transformations simpler than in Cartesian coordinates.
 +We can apply an elementary rotation about the z-axis (normal to the page) with matrix:
 +\\
 +\\
 +[R<sub>z</sub>] = [<sup>A</sup>x<sub>B</sub> <sup>A</sup>y<sub>B</sub> <sup>A</sup>z<sub>B</sub>]
 +\\
 +\\
 +Where <sup>A</sup>x,y,z<sub>B</sub> are the unit vectors along the x,y,z-axis of frame B w.r.t frame A. This results in:
 +\\
 +\\
 +{{:torres:tutorials:2_link_tutorial_10.png?300|}}
 +\\
 +\\
 +A translation vector **p** = p<sub>x</sub>i + p<sub>y</sub>j translates the origin of B from the origin of A. The given translation matrix **P** is given by:
 +\\
 +\\
 +{{:torres:tutorials:2_link_tutorial_11.png?200|}}
 +\\
 +\\
 +Next, the translation matrix can be multiplied by the rotation matrix to obtain the complete homogeneous transformation matrix:
 +\\
 +\\
 +{{:torres:tutorials:2_link_tutorial_12.png?nolink&300|}}
 +\\
 +\\
 +To obtain the coordinates for a given point **p** in frame B relative to frame A, the transformation matrix is multiplied by the vector of **p** in frame B. For example, in Matlab:
 +\\
 +\\
 +<code>
 +theta = 30*(pi/180);
 +Px = 2; Py = 3;
 +PframeB = [1; 4; 1];
 +T = [cos(theta) -sin(theta) Px; ...
 +     sin(theta)  cos(theta) Py; ...
 +     0 0 1];
 +
 +PframeA = T*PframeB
 +
 +PframeA = 
 +   0.8660
 +   6.9641
 +   1.0000
 +</code>
 +\\
 +\\
 +In the above example, frame B is translated in the (x,y) direction of frame A by (2,3) and rotated counter-clockwise by 30 degrees along the z-axis. The **p** vector in frame B has coordinates (1, 4) and the same vector in frame A has coordinates (0.8660, 6.9641).
 +\\
 +\\
 +To perform a combinations of rotations, translations, and/or transformations, the order of multiplication matters. For example, Rot<sub>z</sub>(30)*Trans(2,3), means that frame B is rotated by 30 degrees about z-axis of frame A and then translated by (2,3) w.r.t frame B. On the other hand, Trans(2,3)*Rot<sub>z</sub>[30] means frame B is translated by (2,3) w.r.t frame A, and then rotated by 30 degrees about z-axis of frame B.
 +\\
 +\\
 +If the transformation is about the new reference frame, then it is post-multiplied. If the transformation is about the old reference frame, then it is pre-multiplied. 
 +\\
 +\\
 +For 3D rotations and translations, the following equations apply:
 +\\
 +\\
 +{{:torres:tutorials:2_link_tutorial_13.png?nolink&300|}}
 \\ \\
 \\ \\
Line 42: Line 115:
 \\ \\
 \\ \\
-{{:torres:tutorials:2_link_tutorial_5.png?400|}}+{{:torres:tutorials:2_link_tutorial_14.png?nolink&400|}}
 \\  \\ 
 The image above shows a diagram of the 2-link robotic arm that will be used in this example.  The image above shows a diagram of the 2-link robotic arm that will be used in this example. 
-The forward kinematics are used to calculate the joint angles. This is done using planar transformations to go from the origin plane to the end-effector plane using the equation below. 
-{{:torres:2_link_tutorial_6.png?200|}} 
 \\ \\
-Each matrix "A" is obtained using the Denavit-Hartenberg parameters.+\\ 
 +The forward kinematics are used to calculate the joint angles. This is done using planar transformations to go from the origin plane to the end-effector plane. This can be done with arbitrary translations and rotations, but a more practical approach is to use the Denavit-Hartenberg parameters. Obtaining the parameters is discussed more below. First, we will focus on the specific translations and rotations. 
 +\\ 
 +\\ 
 +Each relative transformation can be described by a matrix **A**: 
 +\\ 
 +\\ 
 +<sup>n-1</sup>**A**<sub>n</sub> = Rot(z<sub>n</sub>, theta<sub>n</sub>)*Trans(0,0,d<sub>n</sub>)*Trans(a<sub>n</sub>,0,0)*Rot(x<sub>n</sub>, alpha<sub>n</sub>) , where n the number of links 
 +\\ 
 +\\ 
 +Using Matlab, the relative transformation **A** can be calculated easily using symbolic variables (can also be done by hand): 
 +\\ 
 +\\ 
 +<code> 
 +>>syms theta d a alpha 
 +>>Rotz = [cos(theta) -sin(theta) 0 0;... 
 +          sin(theta)  cos(theta) 0 0;... 
 +          0 0 1 0;... 
 +          0 0 0 1]; 
 +>>Trans_d = [1 0 0 0;... 
 +             0 1 0 0;... 
 +             0 0 1 d;... 
 +             0 0 0 1]; 
 +>>Trans_a = [1 0 0 a;... 
 +             0 1 0 0;... 
 +             0 0 1 0;... 
 +             0 0 0 1]; 
 +>>Rotx = [1 0 0 0;... 
 +          0 cos(alpha) -sin(alpha) 0;... 
 +          0 sin(alpha) cos(alpha) 0;... 
 +          0 0 0 1]; 
 +>> A = Rotz*Trans_d*Trans_a*Rotx 
 +  
 +A = 
 +  
 +[ cos(theta), -cos(alpha)*sin(theta),  sin(alpha)*sin(theta), a*cos(theta)] 
 +[ sin(theta),  cos(alpha)*cos(theta), -sin(alpha)*cos(theta), a*sin(theta)] 
 +[          0,             sin(alpha),             cos(alpha),            d] 
 +[          0,                      0,                      0,            1] 
 + 
 +</code> 
 +\\ 
 +\\ 
 +Once each **A** matrix is obtained, the complete transformation **T** from frame {0} to frame {N} can be found through: 
 +\\ 
 +\\ 
 +<sup>0</sup>**T**<sub>n</sub> = <sup>0</sup>**A**<sub>1</sub> * <sup>1</sup>**A**<sub>2</sub>
 \\ \\
 \\ \\
Line 54: Line 171:
 \\ \\
 \\ \\
-The DH-parameters (theta_j, d_j, a_j, alpha_j) are obtained from a set of rules that can be easily referenced online. \\ +The DH-parameters (theta_j, d_j, a_j, alpha_j) are obtained from a set of rules that can be easily referenced [[https://www.google.com/search?q=dh+parameters&ie=utf-8&oe=utf-8&client=firefox-b-1 | online]]Also, [[https://www.youtube.com/watch?v=rA9tm0gTln8&feature=youtu.be |this video ]] is helpful in visualizing each parameter. 
-*The first parameter, the joint angle, is labeled on the diagram and is the angle from the previous x-axis to the new x-axis about the old z-axis. +\\ 
 +*The first parameter, the Joint Angle, is labeled on the diagram and is the angle from the previous x-axis to the new x-axis about the old z-axis. 
 \\ *The Link Offset is the distance from the origin of the previous frame of reference to the new x-axis along the old z-axis.  \\ *The Link Offset is the distance from the origin of the previous frame of reference to the new x-axis along the old z-axis. 
 \\ *The link length is the distance between the old z-axis and the new z-axis along the new x-axis. \\ *The link length is the distance between the old z-axis and the new z-axis along the new x-axis.
Line 62: Line 180:
 {{:torres:tutorials:2_link_tutorial_7.png?300|}} {{:torres:tutorials:2_link_tutorial_7.png?300|}}
 \\ \\
-The parameters obtained can be substituted into the "A" matrix below for each transformation (with j = 1,2):+The parameters obtained can be substituted into the **A** that we calculated earlier for each transformation (with j = 1,2):
 \\ \\
 {{:torres:tutorials:2_link_tutorial_3b.png?400|}} {{:torres:tutorials:2_link_tutorial_3b.png?400|}}
 \\ \\
 +==== Analytical Solution - Geometric ==== 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_15.png?nolink&450|}} 
 +\\ 
 +\\ 
 +First, a triangle can be made from the two link arm with the above parameters specified.  
 +\\ 
 +\\ 
 +Recalling the cosine rule, A<sup>2</sup> = B<sup>2</sup> + C<sup>2</sup> - 2BCcos(a), the first equation can be found by: 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_16.png?nolink&500|}} 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_17.png?nolink&450|}} 
 +\\ 
 +\\ 
 +From the above equation and diagram, we can gather that: 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_18.png?nolink&300|}} 
 +\\ 
 +\\ 
 +Finally, we have an analytical solution for each joint angle: 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_19.png?nolink&300|}}
 ==== Numerical Simulation - MatLab ==== ==== Numerical Simulation - MatLab ====
- 
 \\ \\
----- +\\ 
-clear; clc; format compact; +The following code uses the equations obtained in the analytical solution to calculate the joint angles for the 2-link mechanism. 
 +\\ 
 +\\ 
 +<code> 
 +clear; clc; format compact;
  
-L1 = 1\\ +L1 = 2%link 1 length 
-L2 = 1;+L2 = 3; %link 2 length 
 +xPos = 2; %x position of end-effector 
 +yPos = 1; %y position of end-effector
  
-L(1) Link([0 0 L1 0]); \\ += [xPos yPos]; %end-effector position vector
-L(2) = Link([0 0 L2 0]);+
  
-TwoLink_arm SerialLink(L, 'name', '2Link Arm'); +theta2 acos((xPos^2 + yPos^2 L1^2 L2^2)/(2*L1*L2)); 
----- +theta1 atan2(yPos,xPos) - atan2((L2*sin(theta2)),(L1 + L2*cos(theta2)));
-The code above in Matlab creates a planar 2-link robotic arm class with arbitrary link lengths. From this, many functions can be used to do simulations and numerical analysis. For example, forward and inverse kinematics can be easily calculated. The code below translates the end-effector in the x-axis only and creates a transformation matrix. The 'ikine' function uses inverse kinematics to obtain the joint angles needed for the end-effector to achieve the desired position. The 'fkine' function takes in arbitrary joint angles and will return the position of the end-effector. +
-\\ +
----- +
-T = transl(1,0,0\\ +
-T = +
-                 1 +
-                 0 +
-                 0 +
-                 1 +
->> q TwoLink_arm.ikine(T,'mask',[1 1 0 0 0 0],'q0',[1 0]\\ +
-q = +
-    1.0472   -2.0944 +
->> p = TwoLink_arm.fkine(q\\+
  
-=  +JointAngles [theta1 theta2] %joint angle vector 
-    0.5000    0.8660                 1 +</code>
-   -0.8660    0.5000         0 9.309e-13 +
-                                 0 +
-                                 1+
  
 +In this case, the joint angles obtained for the first and second links are -1.1071 and 2.3005 radians respectively (or -63.4 and 131.8 degrees). The benefits of the analytical solution are rapid calculation times and precise answers.
 \\ \\
----- 
-The 'TwoLink_arm.plot(q)' function along with the joint angles can be used to plot the position of the robotic arm with the given angles, shown in the image below with the end-effector at (1,0). 
 \\ \\
 +As a sanity check, we can go back to the forward kinematics and apply the necessary transformations to calculate the position of the end-effector based on the joint angles obtained. For the above example:
 \\ \\
-{{:torres:tutorials:2_link_tutorial_8.png?400|}}+\\ 
 +<code> 
 +>>Rotz1 = [cos(theta1) -sin(theta1) 0 0;... 
 +          sin(theta1)  cos(theta1) 0 0;... 
 +          0 0 1 0;... 
 +          0 0 0 1]; 
 +>>Trans1 = [1 0 0 L1;... 
 +           0 1 0 0;... 
 +           0 0 1 0;... 
 +           0 0 0 1]; 
 +>>Rotz2 = [cos(theta2) -sin(theta2) 0 0;... 
 +          sin(theta2)  cos(theta2) 0 0;... 
 +          0 0 1 0;... 
 +          0 0 0 1]; 
 +>>Trans2 = [1 0 0 L2;... 
 +           0 1 0 0;... 
 +           0 0 1 0;... 
 +           0 0 0 1]; 
 + 
 +>>T = Rotz1*Trans1*Rotz2*Trans2 
 +T = 
 +    0.3685   -0.9296            2.0000 
 +    0.9296    0.3685            1.0000 
 +                    1.0000         0 
 +                            1.0000   
 +</code>   
 +The final transformation vector shows that the coordinates of the end of the 2nd link are indeed (2,1), which is what we wanted. 
 +\\ 
 +\\ 
 +We can also use the forward kinematics to plot a simple graph that shows the 2-link arm positions. With the following code the intermediate and final transformation matrices can be obtained. 
 +\\ 
 +\\ 
 +<code> 
 +>> T_01 = Rotz1*Trans1 
 +T_01 = 
 +    0.4472    0.8944            0.8944 
 +   -0.8944    0.4472           -1.7889 
 +                    1.0000         0 
 +                            1.0000 
 +>> T_02 = T_01*Rotz2*Trans2 
 +T_02 = 
 +    0.3685   -0.9296            2.0000 
 +    0.9296    0.3685            1.0000 
 +                    1.0000         0 
 +                            1.0000 
 +>> P = [0;0;0;1]; 
 +>> P1 = T_01*P; P2 = T_02*P; 
 +>> plot([0 P1(1) P2(1)],[0 P1(2) P2(2)]); grid on; 
 +</code> 
 +The resulting plot will look like the following: 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_21.png?nolink&450|}}
 ==== Graphical Simulation ==== ==== Graphical Simulation ====
 \\ \\
-TBA+Matlab can be used to produce a graphical simulation of the 2-link arm mechanism for a given angle. Many of these simulations are based on the [[http://petercorke.com/wordpress/toolboxes/robotics-toolbox|Robotics Toolbox]] created by Peter Corke for Matlab. The link contains information on how to download the toolbox and install into Matlab. The [[https://robotacademy.net.au/masterclass/inverse-kinematics-and-robot-motion/?lesson=256|Robot Academy]] also has resources on using the Robotics Toolbox for inverse kinematics. 
 +\\ 
 +\\ 
 +The following code can be used to model a planar 2-link arm. Many of the functions behind the arm use the same theory and math covered previously in the tutorial. 
 +\\ 
 +\\ 
 +<code> 
 +>>mdl_planar2 
 +>>p2.plot(qz) 
 +</code> 
 +Which results in the following image: 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_22.png?nolink&450|}} 
 +\\ 
 +\\ 
 +Next, a translation can be made and inverse kinematics applied: 
 +\\ 
 +\\ 
 +<code> 
 +>> T = transl(1.5,0.5,0) 
 +T = 
 +    1.0000                    1.5000 
 +            1.0000            0.5000 
 +                    1.0000         0 
 +                            1.0000 
 +>> q = p2.ikine(T,'mask',[1 1 0 0 0 0]) 
 +q = 
 +   -0.3373    1.3181 
 +>> p2.plot(q) 
 +</code> 
 +Which will plot the arm with the joint angles that will result in the end-effector having a position of (1.5, 0.5) 
 +\\ 
 +\\ 
 +{{:torres:tutorials:2_link_tutorial_23.png?nolink&450|}} 
 +\\
 \\ \\
 +By changing the T matrix with different values of transl(x,y,0), and using the inverse kinematic function, one can obtain the joint angle values for any possible position and simulate them graphically in the easiest way possible.
 ==== Final Words ==== ==== Final Words ====
  
 \\ \\
-TBA+Hopefully, the information presented in this tutorial is enough for the reader to gain a good understanding of the background, theory, and practical knowledge required to apply this to a 2-link planar robot arm and possibly a 3-link planar arm.
 \\ \\
 \\ \\
2_link_kinematics.1536641063.txt.gz · Last modified: 2018/09/10 21:44 by ntorresreyes