Prof. Wei Zhang - Ohio State Universityzhang/RoboticsClass/docs/... · The arm may also have an o set at the shoulder (see Figure 6.3). The inverse kinematics problem for PUMA-type

ECE5463: Introduction to Robotics

Lecture Note 8: Inverse Kinematics

Prof. Wei Zhang

Department of Electrical and Computer EngineeringOhio State UniversityColumbus, 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 (θ), θ ∈ Rnand the target homogeneous transform X ∈ SE(3), find solutions θ thatsatisfy

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

• 2-link planar open chain: considering only the end-effectorposition and ignoring its orientation, the forward kinematics is[

xy

]=

[L1 cos θ1 + L2 cos(θ1 + θ2)L1 sin θ1 + L2 sin(θ1 + θ2)

]= f(θ1, θ2)

• Inverse Kinematics Problem: Given (x, y), find(θ1, θ2) = f−1(x, y)

• Inverse Kinematics Solution:{Righty Solution: θ1 = γ − α, θ2 = π − βLefty Solution: θ1 = γ + α, θ2 = β − π

where

γ = atan2(y, x), β = cos−1

(L21 + L2

2 − x2 − y2

2L1L2

)

α = cos−1

(x2 + y2 + L2

1 − L22

2L1

√(x2 + y2)

)

220

Workspace

θ1

L1 θ2

L2

(x, y)

(a) A workspace, and lefty and rightyconfigurations.

θ1

L1

θ2

L2

αγ

βx2 + y2

(b) Geometric solution.

Figure 6.1: Inverse kinematics of a 2R planar open chain.

We also recall the law of cosines,

c2 = a2 + b2 − 2ab cosC,

where a, b, and c are the lengths of the three sides of a triangle and C is theinterior angle of the triangle opposite the side of length c.

Referring to Figure 6.1(b), angle β, restricted to lie in the interval [0, π], canbe determined from the law of cosines,

L21 + L2

2 − 2L1L2 cosβ = x2 + y2,

from which it follows that

β = cos−1

(L2

1 + L22 − x2 − y2

2L1L2

).

Also from the law of cosines,

α = cos−1

(x2 + y2 + L2

1 − L22

2L1

√x2 + y2

).

The angle γ is determined using the two-argument 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

220

Workspace

θ1

L1 θ2

L2

(x, y)

(a) A workspace, and lefty and rightyconfigurations.

θ1

L1

θ2

L2

αγ

βx2 + y2

(b) Geometric solution.

Figure 6.1: Inverse kinematics of a 2R planar open chain.

We also recall the law of cosines,

c2 = a2 + b2 − 2ab cosC,

where a, b, and c are the lengths of the three sides of a triangle and C is theinterior angle of the triangle opposite the side of length c.

Referring to Figure 6.1(b), angle β, restricted to lie in the interval [0, π], canbe determined from the law of cosines,

L21 + L2

2 − 2L1L2 cosβ = x2 + y2,

from which it follows that

β = cos−1

(L2

1 + L22 − x2 − y2

2L1L2

).

Also from the law of cosines,

α = cos−1

(x2 + y2 + L2

1 − L22

2L1

√x2 + y2

).

The angle γ is determined using the two-argument arctangent function, γ =atan2(y, x). With these angles, the righty solution to the inverse kinematics is

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


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

Analytical Inverse Kinematics: PUMA-Type Arm6R arm of PUMA type:

• The first two shoulder joint axesintersect orthogonally at a common point

• Joint axis 3 lies in x0 − y0 plane and isaligned with joint axis 2

• Joint axes 4,5,6 (wrist joints) intersectorthogonally at a common point (thewrist center)

• For PUMA-type arms, the inverseKinematics problem can be decomposedinto inverse position and inverseorientation subproblems

222 6.1. Analytic Inverse Kinematics

0

0

z0

rθ1 px

py

pz

y

x

θ2

θ3a2

a3

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

z0 d1

(a) Elbow armwith offset.

d1

x0

y0

θ1 px

py

αr

φ

d1

y0

py

px x0

r

θ1

(b) Kinematic diagram.

Figure 6.3: A 6R PUMA-type arm with a shoulder offset.

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 havean offset at the shoulder (see Figure 6.3). The inverse kinematics problem forPUMA-type arms can be decoupled into inverse-position and inverse-orientationsubproblems, as we now show.

We first consider the simple case of a zero-offset PUMA-type arm. Referringto Figure 6.2 and expressing all vectors in terms of fixed-frame coordinates,denote the components of the wrist center p ∈ R3 by p = (px, py, pz). Projectingp onto the x0–y0-plane, it can be seen that

θ1 = atan2(py, px).


Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 14

PUMA-Type Arm: Inverse Position Subproblem

• Given desired configuration X = (R, p) ∈ SE(3). Clearly,p = (px, py, pz) depends only on θ1, θ2, θ3. Solving for (θ1, θ2, θ3)based on given (px, py, pz) is the inverse position problem.

• Assume that px, py not both equal to zero. They can be used todetermine two solutions of θ1

• Solutions:


0

0

z0

rθ1 px

py

pz

y

x

θ2

θ3a2

a3


z0 d1


d1

x0

y0

θ1 px

py

αr

φ

d1

y0

py

px x0

r

θ1








0

0

z0

rθ1 px

py

pz

y

x

θ2

θ3a2

a3


z0 d1


d1

x0

y0

θ1 px

py

αr

φ

d1

y0

py

px x0

r

θ1








PUMA-Type Arm: Inverse Position Subproblem

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

cos(θ3) =‖p‖2 − d21 − a22 − a23

2a2a3= D

θ3 = atan2(±√

1−D2, D)


Figure 6.5: Four possible inverse kinematics solutions for the 6R PUMA-type armwith shoulder offset.

and θ2 can be obtained in a similar fashion as

θ2 = atan2

(pz,√r2 − d2

1

)− atan2 (a3s3, a2 + a3c3)

= atan2(pz,√p2x + p2

y − d21

)− atan2 (a3s3, a2 + a3c3) ,

where s3 = sin θ3 and c3 = cos θ3. The two solutions for θ3 correspond tothe well-known elbow-up and elbow-down configurations for the two-link planararm. In general, a PUMA-type arm with an offset will have four solutions tothe inverse position problem, as shown in Figure 6.5; the postures in the upperpanel are lefty solutions (elbow-up and elbow-down), while those in the lowerpanel are righty solutions (elbow-up and elbow-down).

We now solve the inverse orientation problem of finding (θ4, θ5, θ6) given theend-effector orientation. This problem is completely straightforward: havingfound (θ1, θ2, θ3), the forward kinematics can be manipulated into the form

e[S4]θ4e[S5]θ5e[S6]θ6 = e−[S3]θ3e−[S2]θ2e−[S1]θ1XM−1, (6.2)

where the right-hand side is now known, and the ωi-components of S4, S5, and


• The solutions of θ3 corresponds to the “elbow-up” and “elbow-down”configurations for the two-link planar arm.

• Similarly, we can find:

θ2 = atan2

(pz,√p2x + p2y − d21

)− atan2 (a3 sin θ3, a2 + a3 cos θ3)


PUMA-Type Arm: Inverse Orientation Subproblem

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

• The forward kinematics can be written as:

e[S4]θ4e[S5]θ5e[S6]θ6 = e−[S3]θ3e−[S2]θ2e−[S1]θ1XM−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 z0, y0 and x0directions, respectively; Hence, the ωi components of S4,S5,S6 are

ω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 ZY X Euler angles given R ∈ SO(3), whoseanalytical solution can be found in Appendix B.1.1 of the textbook.


Numerical Inverse Kinematics

• Inverse kinematics problem can be viewed as finding roots of a nonlinearequation:

T (θ) = X

• Many numerical methods exist for finding roots of nonlinear equations

• For inverse kinematics problem, the target configuration X ∈ SE(3) is ahomogeneous matrix. We need to modify the standard root finding methods.But the main idea is the same.

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

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

Newton-Raphson Method

• Given f : Rn → Rm, we want to find θd such that xd = f(θd).

• Taylor expansion around initial guess θ0:

xd = f(θd) = f(θ0) +∂f

∂θ

∣∣∣∣θ0

(θd − θ0) + h.o.t.

• Let J(θ0) = ∂f∂θ

∣∣∣θ0

and drop the h.o.t., we can compute ∆θ as

∆θ = J†(θ0)(xd − f(θ0))

- J† denotes the Moore-Penrose pseudoinverse

- For any linear equation: b = Az, the solution z∗ = A†b falls into the followingtwo categories:

1. Az∗ = b

2. ‖Az∗ − b‖ ≤ ‖Az − b‖, ∀z ∈ Rn


Newton-Raphason Method

Algorithm:

• Initialization: Given xd ∈ Rm and aninitial guess θ0 ∈ Rn, set i = 0 and selecttolerance ε > 0.

• Set e = xd − f(θi). While ‖e‖ > ε:

1. Set θi+1 = θi + J†(θi)e2. Increment i.

228 6.2. Numerical Inverse Kinematics

xd − f(θ)

xd − f(θ0)slope = −∂f

∂θ (θ0)

θ0 θ1 θd

∆θ = ∂f∂θ (θ0)

−1

(xd − f(θ0))

θ

Figure 6.7: The first step of the Newton–Raphson method for nonlinear root-findingfor a scalar x and θ. In the first step, the slope −∂f/∂θ is evaluated at the point(θ0, xd−f(θ0)). In the second step, the slope is evaluated at the point (θ1, xd−f(θ1))and eventually the process converges to θd. Note that an initial guess to the left ofthe plateau of xd − f(θ) would be likely to result in convergence to the other root ofxd − 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.

Given an initial guess θ0 which is “close to” a solution θd, the kinematics canbe expressed as the Taylor expansion

xd = f(θd) = f(θ0) +∂f

∂θ

∣∣∣∣θ0︸︷︷︸

J(θ0)

(θd − θ0)︸︷︷︸∆θ

+ h.o.t., (6.3)

where J(θ0) ∈ Rm×n is the coordinate Jacobian evaluated at θ0. Truncatingthe Taylor expansion at first order, we can approximate Equation (6.3) as

J(θ0)∆θ = xd − f(θ0). (6.4)

Assuming that J(θ0) is square (m = n) and invertible, we can solve for ∆θ as

∆θ = J−1(θ0)(xd − f(θ0)

). (6.5)

If the forward kinematics is linear in θ, i.e., the higher-order terms in Equa-tion (6.3) are zero, then the new guess θ1 = θ0 +∆θ exactly satisfies xd = f(θ1).If the forward kinematics is not linear in θ, as is usually the case, the new 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 θd (Figure 6.7).


• If f(θ) is a linear function, the algorithm will converge to solution in one-step

• If f is nonlinear, there may be multiple solutions. The algorithm tends toconverge to the solution that is the ”closest” to the initial guess θ0


From Newton Method to Inverse Kinematics Solution

• Given desired configuration X = Tsd ∈ SE(3), we want to find θd ∈ Rn suchthat Tsb(θd) = Tsd

• At the ith iteration, we want to move towards the desired position:

- In vector case, the direction to move is e = xd − f(θi)

- Meaning: e is the velocity vector which, if followed for unit time, would cause amotion from f(θi) to xd

- Thus, we should look for a body twist Vb which, if followed for unit time, wouldcause a motion from Tsb(θ

i) to the desired configuration Tsd.

- Such a body twist is given by

[Vb] = log Tbd(θi), where Tbd(θ

i) = T−1sb (θi)Tsd

To achieve a desired body twist, we need the joint rate vector:

∆θ = J†b (θi)Vb


Numerical Inverse Kinematics Algorithm

• Algorithm:- Initialization:

• Given: Tsd and initial guess θ0

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

- Set [Vb] = log Tbd(θi). While ‖Vb‖ > ε:

1. Set θi+1 = θi + J†b (θi)Vb

2. Increment i

• An equivalent algorithm can be developed in the space frame, using the spaceJacobian Js and the spatial twist Vs = [AdTsb

]Vb


More Discussions

•


Documents

Prof. Wei Zhang - Ohio State Universityzhang/RoboticsClass/docs/... · The arm may also have an o set at the shoulder (see Figure 6.3). The inverse kinematics problem for PUMA-type