Carnegie Mellon

An Introduction to Robot Kinematics

Renata Melamud Carnegie Mellon An Example - The PUMA 560

2 3

4 1 There are two more joints on the end effector (the gripper) The PUMA 560 has SIX revolute joints A revolute joint has ONE degree of freedom ( 1 DOF) that is defined by its angle Carnegie Mellon Other basic joints

Revolute Joint 1 DOF ( Variable - Υ)

Prismatic Joint 1 DOF (linear) (Variables - d)

Spherical Joint

3 DOF ( Variables - Υ1, Υ2, Υ3) Carnegie Mellon We are interested in two kinematics topics

Forward Kinematics (angles to position) What you are given: The length of each link The angle of each joint

What you can find: The position of any point (i.e. it’s (x, y, z) coordinates

Inverse Kinematics (position to angles) What you are given: The length of each link The position of some point on the robot

What you can find: The angles of each joint needed to obtain that position Carnegie Mellon Quick Math Review Dot Product: ⎡ax ⎤ ⎢ ⎥ Geometric Representation: ⎣ay ⎦ ⎡b ⎤ =• BABA cos θ A x ⎢b ⎥ θ ⎣ y ⎦ Matrix Representation: B

⎡a x ⎤ ⎡b x ⎤ BA =• ⎢ ⎥ • ⎢ ⎥ += baba yyxx ⎣a y ⎦ ⎣b y ⎦

Unit Vector Vector in the direction of a chosen vector but whose magnitude is 1. B uB = B B

uB Carnegie Mellon Quick Matrix Review

Matrix Multiplication:

An (m x n) matrix A and an (n x p) matrix B, can be multiplied since the number of columns of A is equal to the number of rows of B. Non-Commutative Multiplication AB is NOT equal to BA

⎡ ba ⎤ ⎡ fe ⎤ ⎡( + ) ( + bhafbgae )⎤ ⎢ ⎥ ∗ ⎢ ⎥ = ⎢ ⎥ ⎣ dc ⎦ ⎣ hg ⎦ ⎣()()+ + dhcfdgce ⎦

Matrix Addition: ⎡ ba ⎤ ⎡ fe ⎤ ⎡( ) ( ++ fbea )⎤ ⎢ ⎥ + ⎢ ⎥ = ⎢ ⎥ ⎣ dc ⎦ ⎣ hg ⎦ ⎣()()++ hdgc ⎦ Carnegie Mellon Basic Transformations Moving Between Coordinate Frames

Translation Along the X-Axis Y O

(VN,VO)

O XY N VO V V

P N X VN

Px

Px = distance between the XY and NO coordinate planes ⎡V X ⎤ ⎡V N ⎤ ⎡P ⎤ Notation: XY NO P = x V = ⎢ Y ⎥ V = ⎢ O ⎥ ⎢ ⎥ ⎣V ⎦ ⎣V ⎦ ⎣ 0 ⎦ Carnegie Mellon Writing V XY in terms of V NO

Y O

O XY N VO V V P Υ N X VN

⎡ + VP N ⎤ V XY X VP NO = ⎢ O ⎥ += ⎣ V ⎦ Carnegie Mellon

O Translation along the X-Axis and Y-Axis

Y

O N VO V

XY N V VN PXY

XY ⎡Px ⎤ X P = ⎢ ⎥ ⎣PY ⎦

N XY NO ⎡ X + VP ⎤ VPV =+= ⎢ O ⎥ ⎣ Y + VP ⎦ Carnegie Mellon Using Basis Vectors Basis vectors are unit vectors that point along a coordinate axis

O

n Unit vector along the N-Axis o Unit vector along the N-Axis O N VO V V NO Magnitude of the VNO vector o n VN N

N ⎡ NO ⎤ ⎡ NO ⎤ NO NO ⎡V ⎤ V cosθ V cosθ ⎡ • nV ⎤ V = ⎢ O ⎥ = ⎢ NO ⎥ = ⎢ NO ⎥ = ⎢ NO ⎥ ⎣V ⎦ ⎣⎢ V sinθ⎦⎥ ⎣⎢ − θ)cos(90V ⎦⎥ ⎣ • oV ⎦ Carnegie Mellon Y Rotation (around the Z-Axis)

Z X Y O

VO V N

VY

VN Υ X VX

Υ = Angle of rotation between the XY and NO coordinate axis

X N XY ⎡V ⎤ NO ⎡V ⎤ V = ⎢ Y ⎥ V = ⎢ O ⎥ ⎣V ⎦ ⎣V ⎦ Carnegie Mellon x Y Unit vector along X-Axis O V Can be considered with respect to the XY coordinates or NO coordinates O V V N V XY = V NO VY α Υ VN X VX X = VV XY cos α = V NO α NO •= xVcos

X N O (Substituting for VNO using the N and O •∗+∗= x)oVn(VV components of the vector) X N O ()( •+•= )oxVnxVV = V N θ) + V(cos O (cos( θ + 90)) = V N θ) − V(cos O (sin θ) Carnegie Mellon

Similarly…. Y = VV NO α = Vsin NO cos(90 α) NO •=− yV Y N O •∗+∗= y)oVn(VV Y N O •+•= )oy(V)ny(VV = N (cos(90V θ)) +− V O (cos θ) = V N θ) + V(sin O (cos θ) So…. X X N O ⎡ ⎤ = VV θ) − V(cos (sin θ) XY V V = ⎢ Y ⎥ Y = VV N θ) + V(sin O (cos θ) ⎣V ⎦ Written in Matrix Form

X N ⎡V ⎤ ⎡cosθ − sinθ⎤⎡V ⎤ Rotation Matrix about the z-axis V XY = = ⎢ Y ⎥ ⎢ ⎥⎢ O ⎥ ⎣V ⎦ ⎣sinθ cosθ ⎦⎣V ⎦ Carnegie Mellon Y1 O (VN,VO) Y0 VNO N

VXY Υ 1 P X

Translation along P followed by rotation by θ X0

⎡V X ⎤ ⎡P ⎤ ⎡cos θ − sin θ⎤ ⎡V N ⎤ V XY = = x + ⎢ Y ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ O ⎥ ⎣V ⎦ ⎣Py ⎦ ⎣sin θ cos θ ⎦ ⎣V ⎦

(Note : Px, Py are relative to the original coordinate frame. Translation followed by rotation is different than rotation followed by translation.)

In other words, knowing the coordinates of a point (VN,VO) in some coordinate frame (NO) you can find the position of that point relative to your original coordinate frame (X0Y0). Carnegie Mellon

HOMOGENEOUS REPRESENTATION Putting it all into a Matrix

⎡V X ⎤ ⎡P ⎤ ⎡cos θ − sin θ⎤ ⎡V N ⎤ What we found by doing a V XY = = x + ⎢ Y ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ O ⎥ translation and a rotation ⎣V ⎦ ⎣Py ⎦ ⎣sin θ cos θ ⎦ ⎣V ⎦

X N ⎡V ⎤ ⎡Px ⎤ ⎡cos θ − θ 0sin ⎤ ⎡V ⎤ ⎢ ⎥ ⎢ ⎥ Padding with 0’s and 1’s = V Y = ⎢P ⎥ + ⎢sin θ θ 0cos ⎥ V O ⎢ ⎥ ⎢ y ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ 1 ⎦ ⎣⎢ 1 ⎦⎥ ⎣⎢ 100 ⎦⎥ ⎣ 1 ⎦

X N ⎡V ⎤ ⎡cos θ − θ Psin x ⎤ ⎡V ⎤ ⎢ ⎥ ⎢ ⎥ = V Y = ⎢sin θ θ Pcos ⎥ V O Simplifying into a matrix form ⎢ ⎥ ⎢ y ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ 1 ⎦ ⎣⎢ 100 ⎦⎥ ⎣ 1 ⎦

⎡cos θ − θ Psin x ⎤ ⎢ ⎥ H = sin θ θ Pcos y Homogenous Matrix for a Translation in ⎢ ⎥ XY plane, followed by a Rotation around ⎣⎢ 100 ⎦⎥ the z-axis Carnegie Mellon Rotation Matrices in 3D – OK,lets return from homogenous repn ⎡cos θ − θ 0sin ⎤ Rotation around the Z-Axis R = ⎢sin θ θ 0cos ⎥ z ⎢ ⎥ ⎣⎢ 100 ⎦⎥

⎡ θ 0cos sin θ ⎤ R = ⎢ 010 ⎥ Rotation around the Y-Axis y ⎢ ⎥ ⎣⎢− θ 0sin cos θ⎦⎥

⎡ 001 ⎤ R = ⎢0 cos θ − sin θ⎥ Rotation around the X-Axis z ⎢ ⎥ ⎣⎢0 sin θ cos θ ⎦⎥ Carnegie Mellon Homogeneous Matrices in 3D H is a 4x4 matrix that can describe a translation, rotation, or both in one matrix O

⎡ P001 x ⎤ Y ⎢ ⎥ P010 y N ⎢ ⎥ P H = ⎢ P100 z ⎥ X A ⎢ ⎥ 1000 Translation without rotation ⎣ ⎦ Z 0aon Y ⎡ xxx ⎤ ⎢ 0aon ⎥ O ⎢ yyy ⎥ N H = ⎢ zzz 0aon ⎥ ⎢ ⎥ X ⎣ 1000 ⎦ Rotation part: Z Rotation without translation Could be rotation around z-axis, A x-axis, y-axis or a combination of the three. Carnegie Mellon Homogeneous Continued….

⎡V N ⎤ ⎢ ⎥ VO The (n,o,a) position of a point relative to the XY = HV ⎢ ⎥ current coordinate frame you are in. ⎢V A ⎥ ⎢ ⎥ ⎣⎢ 1 ⎦⎥

N ⎡ Paon xxxx ⎤⎡V ⎤ ⎢ ⎥ X N O A ⎢ Paon ⎥ O +++= PVaVoVnV XY ⎢ yyyy ⎥⎢V ⎥ x x x x V = A ⎢ Paon zzzz ⎥⎢V ⎥ ⎢ ⎥⎢ ⎥ ⎣ 1000 ⎦⎣⎢ 1 ⎦⎥

The rotation and translation part can be combined into a single homogeneous matrix IF and ONLY IF both are relative to the same coordinate frame. Carnegie Mellon Finding the Homogeneous Matrix EX. J

Y N ⎡WN ⎤ ⎢ O ⎥ I ⎢W ⎥ ⎢ A ⎥ ⎣W ⎦ T P X K A O Z I N X ⎡ W ⎤ ⎡W ⎤ ⎡W ⎤ Point relative to the Point relative to the ⎢ ⎥ Point relative to the ⎢ J ⎥ ⎢ O ⎥ WY ⎢W ⎥ I-J-K frame ⎢W ⎥ N-O-A frame ⎢ ⎥ X-Y-Z frame K A ⎢ Z ⎥ ⎢W ⎥ ⎢W ⎥ ⎣W ⎦ ⎣ ⎦ ⎣ ⎦

I N I N ⎡ W ⎤ ⎡ Paon iiii ⎤⎡W ⎤ ⎡ W ⎤ ⎡Pi ⎤ ⎡ aon iii ⎤⎡W ⎤ ⎢ J ⎥ ⎢ ⎥⎢ O ⎥ ⎢ J ⎥ ⎢ ⎥ ⎢ ⎥⎢ O ⎥ W Paon jjjj W W = Pj + aon jjj W ⎢ ⎥ = ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ K A K A ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢W ⎥ ⎢P ⎥ ⎢ aon ⎥⎢W ⎥ W Paon kkkk W ⎣ ⎦ ⎣ k ⎦ ⎣ kkk ⎦⎣ ⎦ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣⎢ 1 ⎦⎥ ⎣ 1000 ⎦⎣⎢ 1 ⎦⎥ Carnegie Mellon J

⎡W N ⎤ Y N ⎢ O ⎥ I ⎢W ⎥ ⎢ A ⎥ ⎣W ⎦ T P X K A O Z ⎡W X ⎤ ⎡ Tkji ⎤⎡ WI ⎤ ⎡W X ⎤ T kji ⎡WI ⎤ xxxx ⎡ x ⎤ ⎡ xxx ⎤ ⎢ Y ⎥ ⎢ ⎥⎢ J ⎥ ⎢ ⎥ ⎢ ⎥ W Tkji yyyy W WY = ⎢T ⎥ + ⎢ kji ⎥ WJ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ y ⎥ ⎢ yyy ⎥⎢ ⎥ ⎢WZ ⎥ ⎢ Tkji ⎥⎢WK ⎥ ⎢ Z ⎥ ⎢ ⎥ ⎢ ⎥⎢ k ⎥ ⎢ ⎥ zzzz ⎢ ⎥ ⎣W ⎦ ⎣Tz ⎦ ⎣ kji zzz ⎦⎣W ⎦ ⎢ ⎥ ⎣⎢ 1 ⎦⎥ ⎣ 1000 ⎦⎣⎢ 1 ⎦⎥

X N ⎡W ⎤ ⎡ Tkji xxxx ⎤⎡ Paon iiii ⎤⎡W ⎤ ⎡ WI ⎤ ⎢ Y ⎥ ⎢ ⎥⎢ ⎥⎢ O ⎥ ⎢ J ⎥ W Tkji Paon W Substituting for ⎢W ⎥ ⎢ ⎥ = ⎢ yyyy ⎥⎢ jjjj ⎥⎢ ⎥ K Z A ⎢W ⎥ ⎢W ⎥ ⎢ Tkji zzzz ⎥⎢ Paon kkkk ⎥⎢W ⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎣⎢ 1 ⎦⎥ ⎣ 1000 ⎦⎣ 1000 ⎦⎣⎢ 1 ⎦⎥ Carnegie Mellon

X N ⎡W ⎤ ⎡W ⎤ ⎡ Tkji ⎤⎡ Paon ⎤ ⎢ ⎥ ⎢ ⎥ xxxx iiii W Y W O ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ Tkji yyyy Paon jjjj = H H = ⎢ ⎥⎢ ⎥ ⎢W Z ⎥ ⎢W A ⎥ ⎢ Tkji zzzz ⎥⎢ Paon kkkk ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣⎢ 1 ⎦⎥ ⎣⎢ 1 ⎦⎥ ⎣ 1000 ⎦⎣ 1000 ⎦ Product of the two matrices Notice that H can also be written as:

⎡ T001 x ⎤⎡ xxx 0kji ⎤⎡ P001 i ⎤⎡ iii 0aon ⎤ ⎢ T010 ⎥⎢ 0kji ⎥⎢ P010 ⎥⎢ 0aon ⎥ H = ⎢ y ⎥⎢ yyy ⎥⎢ j ⎥⎢ jjj ⎥ ⎢ T100 z ⎥⎢ zzz 0kji ⎥⎢ P100 k ⎥⎢ kkk 0aon ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥ ⎣ 1000 ⎦⎣ 1000 ⎦⎣ 1000 ⎦⎣ 1000 ⎦

H = (Translation relative to the XYZ frame) * (Rotation relative to the XYZ frame) * (Translation relative to the IJK frame) * (Rotation relative to the IJK frame) Carnegie Mellon The Homogeneous Matrix is a concatenation of numerous translations and rotations J

I ⎡W N ⎤ Y N ⎢ O ⎥ ⎢W ⎥ ⎢ A ⎥ ⎣W ⎦ T P X A K O Z One more variation on finding H:

H = (Rotate so that the X-axis is aligned with T)

* ( Translate along the new t-axis by || T || (magnitude of T)) * ( Rotate so that the t-axis is aligned with P) * ( Translate along the p-axis by || P || ) * ( Rotate so that the p-axis is aligned with the O-axis)

This method might seem a bit confusing, but it’s actually an easier way to solve our problem given the information we have. Here is an example… Carnegie Mellon

F o r w a r d K i n e m a t i c s Carnegie Mellon

The Situation: You have a robotic arm that

starts out aligned with the xo-axis. You tell the first link to move by Υ1 and the second link to move by Υ2.

The Quest: What is the position of the end of the robotic arm?

Solution: 1. Geometric Approach This might be the easiest solution for the simple situation. However, notice that the angles are measured relative to the direction of the previous link. (The first link is the exception. The angle is measured relative to it’s initial position.) For robots with more links and whose arm extends into 3 dimensions the geometry gets much more tedious.

2. Algebraic Approach Involves coordinate transformations. Carnegie Mellon Example Problem: You are have a three link arm that starts out aligned in the x-axis.

Each link has lengths l1, l2, l3, respectively. You tell the first one to move by

Υ1 , and so on as the diagram suggests. Find the Homogeneous matrix to get the position of the yellow dot in the X0Y0 frame.

Y3

Υ3

Y2 23X3 Υ2 X2

H = Rz(Υ1 ) * Tx1(l1) * Rz(Υ2 ) * Tx2(l2) * Rz(Υ3 )

0 1 1 Y i.e. Rotating by Υ1 will put you in the X Y frame. 1 1 1 Translate in the along the X axis by l1. X 2 2 Rotating by Υ2 will put you in the X Y frame. 1 Υ Y 1 3 3 X0 and so on until you are in the X Y frame.

The position of the yellow dot relative to the X3Y3 frame is

(l1, 0). Multiplying H by that position vector will give you the coordinates of the yellow point relative the the X0Y0 frame. Carnegie Mellon

Slight variation on the last solution: Make the yellow dot the origin of a new coordinate X4Y4 frame

Y3

Y4 Υ3

Y2 23X3 Υ2 X2 X4

H = Rz(Υ1 ) * Tx1(l1) * Rz(Υ2 ) * Tx2(l2) * Rz(Υ3 ) * Tx3(l3) Y0 1 0 0 4 4

1 This takes you from the X Y frame to the X Y frame.

X

1 Υ Y 1 4 4 X0 The position of the yellow dot relative to the X Y frame is (0,0).

⎡ X ⎤ ⎡ 0 ⎤ ⎢ Y ⎥ ⎢ 0 ⎥ ⎢ ⎥ = H ⎢ ⎥ Notice that multiplying by the (0,0,0,1) vector will ⎢ Z ⎥ ⎢ 0 ⎥ equal the last column of the H matrix. ⎢ ⎥ ⎢ ⎥ ⎣ 1 ⎦ ⎣ 1 ⎦ Carnegie Mellon

More on Forward Kinematics… Denavit - Hartenberg Parameters Carnegie Mellon Denavit-Hartenberg Notation

Z(i - 1)

Y(i -1) Y i Z i

X i a i a(i - 1 ) d i X(i -1)

Υ i α( i - 1)

IDEA: Each joint is assigned a coordinate frame. Using the Denavit- Hartenberg notation, you need 4 parameters to describe how a frame (i) relates to a previous frame ( i -1 ).

THE PARAMETERS/VARIABLES: α, a , d, Υ Carnegie Mellon The Parameters

You can

Z(i - 1) align the two axis Y Y (i -1) i Z i just using

X i a the 4 a i (i - 1 ) d i parameters X(i -1)

Υ i α( i - 1)

1) a(i-1) Technical Definition: a(i-1) is the length of the perpendicular between the joint axes. The joint axes is the axes around which revolution takes place which are the

Z(i-1) and Z(i) axes. These two axes can be viewed as lines in space. The common perpendicular is the shortest line between the two axis-lines and is perpendicular to both axis-lines. Carnegie Mellon a(i-1) cont... Visual Approach - “A way to visualize the link parameter a(i-1) is to imagine an expanding cylinder whose axis is the Z(i-1) axis - when the cylinder just touches the joint axis i the radius of the cylinder is equal to a(i-1).” (Manipulator Kinematics)

It’s Usually on the Diagram Approach - If the diagram already specifies the various coordinate frames, then the common perpendicular is usually the X(i-1) axis. So a(i-1) is just the displacement along the X(i-1) to move from the (i-1) frame to the i frame.

If the link is prismatic, then a(i-1) is a variable, not a parameter.

Z(i - 1) Y Y (i -1) i Z i

X i a a i (i - 1 ) d i X(i -1)

Υ i α( i - 1) Carnegie Mellon

2) α(i-1) Technical Definition: Amount of rotation around the common perpendicular so that the joint axes are parallel. i.e. How much you have to rotate around the X(i-1) axis so that the Z(i-1) is pointing in the same direction as the Zi axis. Positive rotation follows the right hand rule.

3) d(i-1) Technical Definition: The displacement Z(i - 1) along the Zi axis needed to align the a(i-1) Y Y common perpendicular to the ai common (i -1) Z i perpendicular. i X i a i a(i - 1 ) d X i In other words, displacement along the (i -1) Υ i Zi to align the X(i-1) and Xi axes. α( i - 1)

4) Υi Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi axis. Carnegie Mellon The Denavit-Hartenberg Matrix

⎡ cosθi − sinθi 0 a −1)(i ⎤ ⎢sinθ cosα cosθ cosα − sinα − sinα d ⎥ ⎢ −1)(ii −1)(ii −1)(i − i1)(i ⎥

⎢sinθ sinα −1)(ii cosθ sinα −1)(ii cosα −1)(i cosα − di1)(i ⎥ ⎢ ⎥ ⎣ 0 0 0 1 ⎦

Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a transformation matrix from one coordinate frame to the next. Using a series of D-H Matrix multiplications and the D-H Parameter table, the final result is a transformation matrix from some frame to your initial frame.

Z(i -

Put the transformation here 1) Y(i - Y Z 1) i i X a a(i - d X(i - i i 1 ) i 1) Υ α( i i -1) Carnegie Mellon 3 Revolute Joints Z Y2 2

Z0 Z1

X2 d2

X0 X1

Y0 Y1 Denavit-Hartenberg Link Parameter Table a0 a1

i α (i-1) a (i-1) di θ i Notice that the table has two uses: 1) To describe the robot with its 0 0 0 0 θ 0 variables and parameters.

2) To describe some state of the 1 0 a0 0 θ 1 robot by having a numerical values for the variables. 2 -90 a1 d2 θ 2

Carnegie Mellon Y Z 2 2 i α(i-1) a(i-1) di θi Z0 Z1

0 0 0 0 θ0 X2 d2

1 0 a0 0 θ X0 X1 1

Y0 Y1 2 -90 a1 d2 θ2

a a 0 1

⎡V X 2 ⎤

⎢ Y2 ⎥ 0 1 ZYX V = T)T)(T)((T V 000 = T ⎢ ⎥ 10 2 ⎢V Z 2 ⎥ ⎢ ⎥ ⎣⎢ 1 ⎦⎥ Note: T is the D-H matrix with (i-1) = 0 and i = 1. Carnegie Mellon

i α(i-1) a(i-1) di θi ⎡cosθ0 − θ0 00sin ⎤ ⎢ ⎥ sinθ0 θ0 00cos 0 0 0 0 θ0 T = ⎢ ⎥ 0 ⎢ 0100 ⎥ ⎢ ⎥ 1 0 a0 0 θ1 ⎣ 1000 ⎦

This is just a rotation around the Z0 axis 2 -90 a1 d2 θ2

⎡cosθ1 − θ1 a0sin 0 ⎤ ⎡ cosθ2 − θ2 a0sin 1 ⎤ ⎢sinθ θ 00cos ⎥ ⎢ d100 ⎥ 0 ⎢ 1 1 ⎥ 1 ⎢ 2 ⎥ 1T = 2T = ⎢ 0000 ⎥ ⎢ sinθ2 −− θ2 00cos ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ 1000 ⎦ ⎣ 1000 ⎦

This is a translation by a1 and then d2 This is a translation by a0 followed by a followed by a rotation around the X2 and rotation around the Z1 axis Z2 axis 0 1 = 10 2T)T)(T)((T Carnegie Mellon

Next Example Carnegie Mellon

Puma Carnegie Mellon Alternative PUMA 560 Carnegie Mellon

I n v e r s e K i n e m a t i c s

From Position to Angles Carnegie Mellon A Simple Example

Revolute and Finding Υ: Prismatic Joints y Combined θ = arctan( ) x More Specifically: (x , y) y θ = (2arctan ) arctan2() specifies that it’s in the x first quadrant

Y S Υ1 Finding S:

X += 22 )y(xS Carnegie Mellon Inverse Kinematics of a Two Link Manipulator

(x , y) Given: l1, l2 ,x , y

Find: Υ , Υ Υ2 1 2

l2 Redundancy: A unique solution to this problem does not exist. Notice, that using the “givens” two solutions are possible. l1 Υ1 Sometimes no solution is possible.

(x , y) l2

l2 l1

l 1 The Geometric Solution Carnegie Mellon Using the Law of Cosines: (x , y) l 222 2 θ2 −+= cos2 Cabbac 22 2 2 )( 1 2 −+=+ llllyx 21 180cos(2 − θ2 )

180cos( θ2 ) −=− θ2 )cos( 22 2 2 l1 1 −−+ llyx 2 α θ2 )cos( = 2 ll 21 θ 1 ⎛ 22 2 −−+ llyx 2 ⎞ θ = arccos⎜ 1 2 ⎟ 2 ⎜ 2 ll ⎟ Using the Law of Cosines: ⎝ 21 ⎠ B sinsin C Redundant since θ2 could be in the = first or fourth quadrant. b c

sinθ1 sin(180 − θ2 ) θ2 )sin( = = Redundancy caused since θ2 has two possible 22 22 l2 + yx + yx values −= θαθ ⎛ ⎞ 1 1 ⎛ y ⎞ ⎜ l θ22 )sin( ⎟ θ1 = 2arctan ⎜ ⎟ − arcsin ⎛ y ⎞ ⎝ x ⎠ ⎜ + yx 22 ⎟ α = 2arctan ⎜ ⎟ ⎝ ⎠ ⎝ x ⎠ The Algebraic Solution Carnegie Mellon (x , y)

l2 θ2

c 1 = cos θ 1

c + 21 cos( θ += θ 12 ) += ll ccx(1) l1 + 21211

+= ll sinsy(2) + 21211 θ1

yx)2((1) 2222 =+=+ 2 2 2 2 2 2 2 2 ()1 1 += ll 2 +21 + ll + ()121121 1 ++ ll 2 +21 + ll +21121 )(sins2)(sins)(cc2)(cc 2 2 1 2 ++= llll ()+ + +21121121 )(sins)(cc2 2 2 Only Unknown 1 2 ++= llll c2 221 Note: + − ⎛ yx 22 2 −−+ ll 2 ⎞ − = + bababa ))(sin(sin))(cos(cos)cos( ∴θ = arccos⎜ 1 2 ⎟ 2 ⎜ ⎟ + = + abbaba ))(sin(cos))(sin(cos)sin( ⎝ 2 ll 21 ⎠ − − Carnegie Mellon Note: += ll ccx + 21211 + − ccc ccc −+= sslll 21221211 − = + bababa ))(sin(sin))(cos(cos)cos( )()c(c −+= slsll )()c(c + + 2212211 − = − abbaba ))(sin(cos))(sin(cos)sin(

+= ll sinsy + 21211 We know what θ2 is from the previous cscss ++= lll cscss 12221211 slide. We need to solve for θ1 . Now we have two equations and two )c(s)s(c = l ++ ll 2211221 )c(s)s(c unknowns (sin θ1 and cos θ1 ) + sls 221 )(x c 1 = + ll 221 )c( Substituting for c and simplifying + sls 221 )(x 1 y = l ++ ll 221122 )c(s)s( many times + ll 221 )c( 1 Notice this is the law of cosines = (sx 2 2 +++ llllsl )c2 ()1122 2 221 and can be replaced by x2+ y2 + ll 221 )c(

221 −+ x)c(y slll 22 s1 = θ = cs ),(2arctan + yx 22 1 11