Inverse Kinematics

ECE5463: Introduction to Robotics Lecture Note 8: Inverse Kinematics

Prof. Wei Zhang

Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA

Spring 2018

Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 1 / 14 Outline

Inverse Kinematics Problem •

Analytical Solution for PUMA-Type Arm •

Numerical Inverse Kinematics •

Outline Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 2 / 14 Inverse Kinematics Problem

Inverse Kinematics Problem: Given the forward kinematics T (θ), θ n • R and the target homogeneous transform X SE(3), ﬁnd solutions θ that∈ satisfy ∈ T (θ) = X

Multiple solutions may exist; they are challenging to characterize in general •

This lecture will focus on: • - Simple illustrating example

- Analytical solution for PUMA-type arm

- Numerical solution using the Newton-Raphson method

Inverse Kinematics Problem Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 3 / 14 Example: 2-Link Planar Open Chain 220 2-link planar open chain: considering only the end-eﬀector • position and ignoring its orientation, the forward kinematics(x, isy) L2

L1 θ2 x L1 cos θ1 + L2 cos(θ1 + θ2) = = f(θ1, θ2) L2 2 2 y L1 sin θ1 + L2 sin(θ1 + θ2) θ1 x + y β θ2 γ Inverse Kinematics Problem: Given (x, y), ﬁnd α • −1 L1 (θ1, θ2) = f (x, y) θ Workspace 220 1 Inverse Kinematics Solution: • (a) A workspace, and lefty and righty (b) Geometric solution. ( conﬁgurations. θ γ − α, θ π − β (x, y) Righty Solution: 1 = 2 = L2 Lefty Solution: θ = γ + α, θ Figure= β − 6.1:πInverse kinematics of a 2R planar open chain. 1 2 L1 θ2

L2 2 2 where We also recall the law of cosines, θ1 x + y β θ2 L2 + L2 − x2 − y2 c2 = a2 + b2 2ab cos C, γ γ = atan2(y, x), β = cos−1 1 2 − α where a,2bL,1 andL2c are the lengths of the three sides of a triangle and C is the L1 interior angle of the triangle opposite the side of lengthWorkspacec. θ1 2 2 Referring2 2 to! Figure 6.1(b), angle β, restricted to lie in the interval [0, π], can −1 x + y + L1 − L2 α = cos be determined from the law of cosines, (a) A workspace, and lefty and righty (b) Geometric solution. p 2 2 2L1 (x + y ) conﬁgurations. L2 + L2 2L L cos β = x2 + y2, 1 2 − 1 2 Figure 6.1: Inverse kinematics of a 2R planar open chain. from which it follows that 2 2 2 2 1 L1 +WeL2 alsox recally the law of cosines, β = cos− − − . 2L1L2 Inverse Kinematics Problem Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU)c2 = a2 4+ /b 142 2ab cos C, Also from the law of cosines, − where a, b, and c are the lengths of the three sides of a triangle and C is the 2 2 2 2 1 xinterior+ y + angleL1 ofL the2 triangle opposite the side of length c. α = cos− − . Referring2 to2 Figure 6.1(b), angle β, restricted to lie in the interval [0, π], can 2L1 x + y ! be determined from the law of cosines, The angle γ is determined using the two-argumentp arctangent function, γ = L2 + L2 2L L cos β = x2 + y2, atan2(y, x). With these angles, the righty solution to the inverse1 2 kinematics− 1 2 is from which it follows that θ1 = γ α, θ2 = π β − − 2 2 2 2 1 L1 + L2 x y β = cos− − − . 2L L May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org 1 2 Also from the law of cosines,

2 2 2 2 1 x + y + L1 L2 α = cos− − . 2 2 2L1 x + y ! The angle γ is determined using the two-argumentp arctangent function, γ = atan2(y, x). With these angles, the righty solution to the inverse kinematics is

θ = γ α, θ = π β 1 − 2 −

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org Analytical Inverse Kinematics: PUMA-Type Arm 6R arm of PUMA type: The ﬁrst two shoulder joint axes • intersect orthogonally at a common point

Joint axis 3 lies in xˆ yˆ plane and is • 0 0 aligned with joint axis− 2

Joint axes 4,5,6 (wrist joints) intersect • 222 6.1. Analytic Inverse Kinematics orthogonally at a common point (the

wrist center) ˆz0 a3 pz

a2 θ3

For PUMA-type arms, the inverse θ2 ˆy0 • θ r Kinematics problem can be decomposed 1 px into inverse position and inverse py ˆx0

orientation subproblems Figure 6.2: Inverse position kinematics of a 6R PUMA-type arm.

ˆz0 d1

ˆy0 ˆy0 py Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 14 py r r α φ d1 θ1 p px θ1 x ˆx0 ˆx0 d1

(a) Elbow arm (b) Kinematic diagram. with oﬀset.

Figure 6.3: A 6R PUMA-type arm with a shoulder oﬀset.

that these joint axes are aligned in the ˆz0-, ˆy0-, and ˆx0-directions, respectively. The lengths of links 2 and 3 are a2 and a3, respectively. The arm may also have an offset at the shoulder (see Figure 6.3). The inverse kinematics problem for PUMA-type arms can be decoupled into inverse-position and inverse-orientation subproblems, as we now show. We first consider the simple case of a zero-offset PUMA-type arm. Referring to Figure 6.2 and expressing all vectors in terms of fixed-frame coordinates, 3 denote the components of the wrist center p R by p = (px, py, pz). Projecting ∈ p onto the ˆx0–ˆy0-plane, it can be seen that

θ1 = atan2(py, px).

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org 222 6.1. Analytic Inverse Kinematics

ˆz0 a3 pz

a2 θ3

θ2 ˆy0 θ r PUMA-Type Arm: Inverse Position Subproblem 1 px py ˆx0 Given desired configuration X = (R, p) SE(3). Clearly, • 222∈ 6.1. Analytic Inverse KinematicsFigure 6.2: Inverse position kinematics of a 6R PUMA-type arm. p = (px, py, pz) depends only on θ1, θ2, θ3. Solving for (θ1, θ2, θ3) based on given is the inverse position problem. ˆz0 d1 (px, py, pz) ˆz0 a3 pz ˆy0 θ3 ˆy0 a2 py Assume that p , p not both equal to zero. They can be used to x y py • θ2 ˆy0 r determine two solutions of θ1 r r α θ1 px φ d1 py θ1 ˆx0 p px θ1 x ˆx0 ˆx0 Solutions: d1 • Figure 6.2: Inverse position kinematics of a 6R PUMA-type arm. (a) Elbow arm (b) Kinematic diagram. with offset. ˆz0 d1 Figure 6.3: A 6R PUMA-type arm with a shoulder offset. ˆy0 ˆy0 py that these joint axes are aligned in the ˆz0-, ˆy0-, and ˆx0-directions, respectively. py The lengths of links 2 and 3 are a2 and a3, respectively. The arm may also have anr offset at the shoulder (see Figure 6.3). The inverse kinematics problem for r PUMA-typeα arms can be decoupled into inverse-position and inverse-orientation subproblems,φ as we now show. d1 θWe1 first consider the simple case of a zero-offset PUMA-type arm. Referring p px θ1 x ˆx0 to Figure 6.2ˆx0 and expressing all vectors in terms of fixed-frame coordinates, d1 3 denote the components of the wrist center p R by p = (px, py, pz). Projecting ∈ p onto the ˆx0–ˆy0-plane, it can be seen that (a) Elbow arm (b) Kinematic diagram. with offset. Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 6 / 14 θ1 = atan2(py, px). Figure 6.3: A 6R PUMA-type arm with a shoulder offset.

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

θ1 = atan2(py, px).

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org PUMA-Type Arm: Inverse Position Subproblem224 6.1. Analytic Inverse Kinematics

Determining θ and θ is inverse kinematics problem for • 2 3 a planar two-link chain

2 2 2 2 p d1 a2 a3 cos(θ3) = k k − − − = D 2a2a3 p 2 θ3 = atan2 1 D ,D ± − Figure 6.5: Four possible inverse kinematics solutions for the 6R PUMA-type arm with shoulder oﬀset.

and θ2 can be obtained in a similar fashion as

θ = atan2 p , r2 d2 atan2 (a s , a + a c ) 2 z − 1 − 3 3 2 3 3 q = atan2 p , p2 + p2 d2 atan2 (a s , a + a c ) , z x y − 1 − 3 3 2 3 3 q where s3 = sin θ3 and c3 = cos θ3. The two solutions for θ3 correspond to the well-known elbow-up and elbow-down configurations for the two-link planar The solutions of θ3 corresponds to the “elbow-up” andarm. “elbow-down” In general, a PUMA-type arm with an offset will have four solutions to • the inverse position problem, as shown in Figure 6.5; the postures in the upper configurations for the two-link planar arm. panel are lefty solutions (elbow-up and elbow-down), while those in the lower panel are righty solutions (elbow-up and elbow-down). We now solve the inverse orientation problem of finding (θ4, θ5, θ6) given the end-effector orientation. This problem is completely straightforward: having found (θ1, θ2, θ3), the forward kinematics can be manipulated into the form

Similarly, we can ﬁnd: [ 4]θ4 [ 5]θ5 [ 6]θ6 [ 3]θ3 [ 2]θ2 [ 1]θ1 1 • e S e S e S = e− S e− S e− S XM − , (6.2) where the right-hand side is now known, and the ωi-components of 4, 5, and q S S 2 2 2 May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org θ2 = atan2 pz, p + p d atan2 (a3 sin θ3, a2 + a3 cos θ3) x y − 1 −

Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 7 / 14 PUMA-Type Arm: Inverse Orientation Subproblem

Now we have found (θ , θ , θ ), we can determine (θ , θ , θ ) given the end-eﬀector • 1 2 3 4 5 6 orientation

The forward kinematics can be written as: • e[S4]θ4 e[S5]θ5 e[S6]θ6 = e−[S3]θ3 e−[S2]θ2 e−[S1]θ1 XM −1

where the right-hand side is now known (denoted by R˜)

For simplicity, we assume joint axes of 4,5,6 are aligned in the zˆ , yˆ and xˆ • 0 0 0 directions, respectively; Hence, the ωi components of 4, 5, 6 are S S S

ω4 = (0, 0, 1), ω4 = (0, 1, 0), ω6 = (1, 0, 0)

Therefore, the wrist angles can be determined as the solution to •

Rot(ˆz, θ4)Rot(ˆy, θ5)Rot(ˆx, θ6) = R˜

This corresponds to solving for ZYX Euler angles given R˜ SO(3), whose • analytical solution can be found in Appendix B.1.1 of the textbook.∈

Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 8 / 14 Numerical Inverse Kinematics

Inverse kinematics problem can be viewed as ﬁnding roots of a nonlinear • equation: T (θ) = X

Many numerical methods exist for ﬁnding roots of nonlinear equations •

For inverse kinematics problem, the target conﬁguration X SE(3) is a • homogeneous matrix. We need to modify the standard root∈ ﬁnding methods. But the main idea is the same.

We ﬁrst recall the standard Newton-Raphson method for solving x = f(θ), • where θ Rn and x Rm. Then we will discuss how to modify the method to numerically∈ solve the∈ inverse kinematics problem.

Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 14 Newton-Raphson Method

n m Given f : R R , we want to ﬁnd θd such that xd = f(θd). • → Taylor expansion around initial guess θ0: •

0 ∂f 0 xd = f(θd) = f(θ ) + (θd θ ) + h.o.t. ∂θ 0 − θ

0 ∂f Let J(θ ) = ∂θ and drop the h.o.t., we can compute ∆θ as • θ0

0 0 ∆θ = J †(θ )(x f(θ )) d −

- J † denotes the Moore-Penrose pseudoinverse - For any linear equation: b = Az, the solution z∗ = A†b falls into the following two categories: 1. Az∗ = b ∗ 2. kAz − bk ≤ kAz − bk, ∀z ∈ Rn

Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 10 / 14 Newton-Raphason Method 228 6.2. Numerical Inverse Kinematics Algorithm: x f(θ) d − m Initialization: Given xd R and an • ∈ 0 n ∂f initial guess θ , set i = 0 and select slope= (θ0) R xd f(θ0 ) − ∂θ tolerance > 0.∈ − i Set e = xd f(θ ). While e > : θ0 θ1 θd θ • − k k 1 i+1 i † i ∆θ = ∂f (θ ) − (x f(θ )) 1. Set θ = θ + J (θ )e ∂θ 0 d − 0 2. Increment i. Figure 6.7: The first step of the Newton–Raphson method for nonlinear root-finding for a scalar x and θ. In the first step, the slope ∂f/∂θ is evaluated at the point 0 0 − 1 1 (θ , xd f(θ )). In the second step, the slope is evaluated at the point (θ , xd f(θ )) − − and eventually the process converges to θd. Note that an initial guess to the left of the plateau of xd f(θ) would be likely to result in convergence to the other root of − xd f(θ), and an initial guess at or near the plateau would result in a large initial − ∆θ and the iterative process might not converge at all. If f(θ) is a linear function, the algorithm| will| converge to solution in one-step • 0 Given an initial guess θ which is “close to” a solution θd, the kinematics can be expressed as the Taylor expansion

0 ∂f 0 xd = f(θd) = f(θ ) + (θd θ ) + h.o.t., (6.3) ∂θ 0 − θ If f is nonlinear, there may be multiple solutions. The algorithm tends∆θ to J(θ 0) • | {z0 } converge to the solution that is the ”closest”0 to them n initial guess| {z } 0 where J(θ ) R × is the coordinate Jacobianθ evaluated at θ . Truncating the Taylor expansion∈ at ﬁrst order, we can approximate Equation (6.3) as

J(θ0)∆θ = x f(θ0). (6.4) d − Assuming that J(θ0) is square (m = n) and invertible, we can solve for ∆θ as

1 0 0 ∆θ = J − (θ ) x f(θ ) . (6.5) d − If the forward kinematics is linear in θ, i.e., the higher-order terms in Equa- 1 0 1 tion (6.3) are zero, then the new guess θ = θ +∆θ exactly satisﬁes xd = f(θ ). Numerical Inverse Kinematics Lecture 8 (ECE5463If Sp18) the forward kinematics is not linear in θ,Wei as is Zhang(OSU) usually the case, the 11 new/ 14 guess θ1 should still be closer to the root than θ0, and the process is then repeated, producing a sequence θ0, θ1, θ2,... converging to θ (Figure 6.7). { } d

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org From Newton Method to Inverse Kinematics Solution

n Given desired conﬁguration X = Tsd SE(3), we want to ﬁnd θd R such • ∈ ∈ that Tsb(θd) = Tsd At the ith iteration, we want to move towards the desired position: • i - In vector case, the direction to move is e = xd f(θ ) − - Meaning: e is the velocity vector which, if followed for unit time, would cause a i motion from f(θ ) to xd

- Thus, we should look for a body twist b which, if followed for unit time, would i V cause a motion from Tsb(θ ) to the desired conﬁguration Tsd. - Such a body twist is given by

i i −1 i [ b] = log Tbd(θ ), where Tbd(θ ) = T (θ )Tsd V sb To achieve a desired body twist, we need the joint rate vector:

† i ∆θ = J (θ ) b b V

Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 12 / 14 Numerical Inverse Kinematics Algorithm

Algorithm: • - Initialization: 0 • Given: Tsd and initial guess θ

• Set i = 0 and select a small error tolerance > 0

i - Set [ b] = log Tbd(θ ). While b > : V kV k

i+1 i † i 1. Set θ = θ + Jb (θ )Vb

2. Increment i

An equivalent algorithm can be developed in the space frame, using the space • Jacobian J and the spatial twist = [Ad ] s Vs Tsb Vb

Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 13 / 14 More Discussions

•

Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 14 / 14