Kinematics – Forward Kinematic

Centre for Robotics Research – School of Natural and Mathematical Sciences – King’s College London Introduction – Forward and Inverse Kinematics

Forward Inverse

Compliant Robotics 2 Peking University, Globex, July 2018 Linear Algebra –Dot Product

The dot product of two vectors A = [A1, A2, ..., An] and B = [B1, B2, ..., Bn]

Compliant Robotics 3 Peking University, Globex, July 2018 Linear Algebra –Dot Product

[1, 1]

[0, 1] [0, 2] [1, 0]

[2, 0]

if A and B are orthogonal

[0, 2] if they are codirectional

Compliant Robotics 4 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

m x p

=Ai ∙ Bj

n x m n x p

Compliant Robotics 5 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

• Example

푒 푓 푎 푏 = ? 푐 푑 푔 ℎ

푒 푓 푎 푏 = ? 푔 ℎ 푐 푑

Compliant Robotics 6 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

Square matrices

AB ≠ BA

Compliant Robotics 7 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

Row vector and column vector- Dimensionality

1 ✕ 3 3 ✕1

Compliant Robotics 8 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

Square matrix and column vector

Compliant Robotics 9 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

If AT = C = x

Then: ABC = xTAx

Quadratic scaler function using matrix representation

Compliant Robotics 10 Peking University, Globex, July 2018 Linear Algebra –Matrix Multiplication

Rectangular matrices

Compliant Robotics 11 Peking University, Globex, July 2018 Linear Algebra

Compliant Robotics 12 Peking University, Globex, July 2018 Rigid body motion

Translation + Rotation

Compliant Robotics 13 Peking University, Globex, July 2018 Rotation Matrix in 2D

y

y’ x’ θ

x

Compliant Robotics 14 Peking University, Globex, July 2018 Rotation Matrix in 2D

y

B a a b y’ p = R b p (-sinθ, cosθ) x’ θ a a a (cosθ, sinθ) R b=[x b y b] x A

Compliant Robotics 15 Peking University, Globex, July 2018 Rotation Matrix in 2D

y

y’ b a -1 a (sinθ, cosθ) p = R b p x’ θ a T a -1 R b = R b (cosθ, -sinθ) x

a T a R b R b = I

Compliant Robotics 16 Peking University, Globex, July 2018 Rotation Matrix in 3D

B

A

Rotation of a rigid object about a point. A be the inertial 3 frame, B the body frame, and xab, yab, zab ∈ R the coordinates of the axes of B relative to A

Compliant Robotics 17 Peking University, Globex, July 2018 Rotation Matrix in 3D

Consider the point q, qb = (xb, yb, zb) be coordinates of q relative to frame B. qa = (xa, ya, za) be coordinates of q relative to frame A.

3 coordinate axes of B, which, in turn, have coordinates xab, yab, zab ∈ R with respect to A

Coordinates of q relative to frame A are given by

Compliant Robotics 18 Peking University, Globex, July 2018 Rotation matrix – Vector representation

Representation of a Vector 푝푥 Respect to 푂 − 푥푦푧 푃 = 푝푦 푝푧

푝′푥 Respect to 푂–푥′푦′푧′ ′ 푃 = 푝′푦

푝′푧 • Since 푃 and 푃′ are representations of the same point P

′ ′ ′ 푃 = 푝′푥푥 + 푝′푦푦 + 푝′푧푧 = 푥′ 푦′ 푧′ 푝′

푃 = 푥′ 푦′ 푧′ 푝′ = 푅푃′

• R represents the transformation matrix of vector coordinates in frame 푂–푥′푦′푧′ • Inverse transformation is

푃′ = 푅푇푃

Compliant Robotics 19 Peking University, Globex, July 2018 Rotation matrix

푅푇 = 푅−1

1 0 0 푇 푅 푅 = 퐼3 = 0 1 0 0 0 1

det(푅) = 1

Compliant Robotics 20 Peking University, Globex, July 2018 Rotation matrix – Elementary rotation

The rotation matrix of frame 푂–푥′푦′푧′ with respect to frame 푂– 푥푦푧

cos 훼 −sin 훼 0 푅푧 훼 = sin 훼 cos 훼 0 0 0 1 cos 훽 0 sin 훽 푅푦 훽 = 0 1 0 −sin 훽 0 cos 훽

1 0 0 푅푥 훾 = 0 cos 훾 −푠𝑖푛 훾 0 푠𝑖푛 훾 cos 훾

Compliant Robotics 21 Peking University, Globex, July 2018 Rotation matrix – Example of a 3D vector rotation 0 푝 = 0 푇 푇 푇 1 푝 = 푅푥 휃푥 푅푦 휃푦 푅푧 휃푧 푝3 푝1 = 푅푥 휃푥 푝

푅푥 휃푥 푝2 = 푅푦 휃푦 푝1

푅푧 휃푧

푝3 = 푅푧 휃푧 푝2

푅푦 휃푦

푝3 = 푅푧 휃푧 푅푦 휃푦 푅푥 휃푥 푝

Compliant Robotics 22 Peking University, Globex, July 2018 Euler Angle

A representation of orientation in terms of three independent parameters constitutes a minimal representation.

T 휃 = 휃푥 휃푦 휃푧

0 표 푝 = 0 , 휃푥 = 휃푦 = 휃푧 = 45 1

푝1 = 푅푥 휃푥 푅푦 휃푦 푅푧 휃푧 푝 ? 푝2 = 푅푧 휃푧 푅푦 휃푦 푅푥 휃푥 푝

Compliant Robotics 23 Peking University, Globex, July 2018 Euler Angle

A representation of orientation in terms of three independent parameters constitutes a minimal representation.

T 휃 = 휃푥 휃푦 휃푧 0 표 푝 = 0 , 휃푥 = 휃푦 = 휃푧 = 45 1

푝1 = 푅푥 휃푥 푅푦 휃푦 푅푧 휃푧 푝 ≠ 푝2 = 푅푧 휃푧 푅푦 휃푦 푅푥 휃푥 푝

푝

푝 푝1 2

Compliant Robotics 24 Peking University, Globex, July 2018 Homogeneous Transformation

• Pose of a rigid body is completely described using position and orientation. 푃 푅 • It can be compactly rewritten as below. • Let’s A is the homogeneous transformation matrix(4 × 4) 푅 푃 퐴 = 0 1 0 0 퐴0 = 푅1 표1 1 0 1

푃표 1 = 퐴0 푃 1 1 1

Compliant Robotics 28 Peking University, Globex, July 2018 Homogeneous Transformation

• Pose of a rigid body is completely described using position and orientation. 푃 푅 • It can be compactly rewritten as below. • Let’s A is the homogeneous transformation matrix(4 × 4) 푅 푃 퐴 = 0 1

A-1

Proof execise: A A-1

푁표푡푒 ∶ 퐴−1 ≠ 퐴푇

Compliant Robotics 29 Peking University, Globex, July 2018 Homogeneous Transformation

• Pose of a rigid body is completely described using position and orientation. 푃 푅 • It can be compactly rewritten as below. • Let’s A is the homogeneous transformation matrix(4 × 4) 0 0 퐴0 = 푅1 표1 1 0 1

푃표 1 = 퐴0 푃 1 1 1 1 0 푃 = 퐴1 푃 1 0 1

1 1 1 0 0 푃 = 푅0 −푅0표1 푃 1 0 1 1

Compliant Robotics 30 Peking University, Globex, July 2018 Homogeneous Transformation

Aac= Aab Abc = C

pbc

Proof exercise

Compliant Robotics 31 Peking University, Globex, July 2018 Homogeneous Transformation 푧′ 1 0 0 P 푧  x    푦′ 0 1 0 Py P A =   푥′ 0 0 1 Pz  푦   0 0 0 1  푥 Translation without rotation

푧 푧′

x'x y'x z'x 0 x' y' z' 0 푦 A =  y y y  x'z y'z z'z 0 푥   Rotation without translation 푥′  0 0 0 1

Compliant Robotics 32 Peking University, Globex, July 2018 Homogeneous Transformation

• Finding the Homogeneous Matrix (an open kinematic chain)

표 표 1 푛−1 푇푛 푞 = 퐴1 푞1 퐴2 푞2 … 퐴푛 푞푛

The coordinate transformation describing the position and orientation of Frame n with respect to Frame 0

Compliant Robotics 33 Peking University, Globex, July 2018 Homogeneous Transformation Programming exercise in class

open exercise1

homogeneousmatrix.m rotationchain_q.m

Compliant Robotics 34 Peking University, Globex, July 2018 Kinematics for manipulators

A manipulator can be schematically represented from a mechanical viewpoint as a kinematic chain of rigid bodies (links) connected by means of revolute or prismatic joints.

Compliant Robotics 35 Peking University, Globex, July 2018 Typical Manipulator Structures

Three link planar arm, Parallelogram Arm, Spherical arm, Depends on combination of revolute Spherical Wrist, joint and prismatic joint Stanford Manipulator, DLR Manipulator, …

Compliant Robotics 36 Peking University, Globex, July 2018 Typical Manipulator Structures

Example of combination of revolute joints

Spherical wrist

Compliant Robotics 37 Peking University, Globex, July 2018 Denavit-Hartenberg Convention(D-H matrix)

• The D–H convention allows the construction of the forward kinematics function by composition of the individual coordinate transformations as 표 표 1 푛−1 푇푛 푞 = 퐴1 푞1 퐴2 푞2 … 퐴푛 푞푛 • It can be applied to any open kinematic chain

D-H convention homogeneous transformation matrix and parameters

ri

ri

Compliant Robotics 38 Peking University, Globex, July 2018 Denavit-Hartenberg Convention(D-H matrix)

ri

Important: Locate the origin 푂푖 at the intersection of 푧푖 with the common normal to axes 푧푖−1 and 푧푖 ri-1

푟𝑖 ∶ 푑𝑖푠푡푎푛푐푒 푏푒푡푤푒푒푛 푂𝑖 푎푛푑푂𝑖′ ′ 푑𝑖: 푑푒푝푡ℎ 푓푟표푚 푂𝑖−1 푡표 푂𝑖 푎푙표푛푔 푧𝑖−1 (푑𝑖푠푡푎푛푐푒 푏푒푡푤푒푒푛 푂𝑖−1 푎푛푑 푤ℎ푒푟푒 푐표푚푚표푛 푛표푟푚푎푙 푟𝑖 𝑖푛푡푒푠푒푐푡 푤𝑖푡ℎ 푧𝑖−1) 훼𝑖: 푎푛푔푙푒 푏푒푡푤푒푒푛 푎푥푒푠 푧𝑖−1 푎푛푑 푧𝑖푎푏표푢푡 푎푥𝑖푠 푥𝑖 휗𝑖: 푎푛푔푙푒 푏푒푡푤푒푒푛 푎푥푒푠 푥𝑖−1 푎푛푑 푥𝑖 푎푏표푢푡 푎푥𝑖푠 푧𝑖−1

• 푟 and 훼푖 are always constant by geometry connection

Compliant Robotics 39 Peking University, Globex, July 2018 DH Homogeneous transformation

Frame i-1 translate by

di along zi-1 and rotate by θi about zi-1

ri

Translate by ri along xi’ and rotate by αi about xi’

ri ri

Compliant Robotics 40 Peking University, Globex, July 2018 Compliant Robotics 41 Peking University, Globex, July 2018 Denavit-Hartenberg Convention(D-H matrix)

Compliant Robotics 42 Peking University, Globex, July 2018 Denavit-Hartenberg Convention

1. Choose base frame by locating the origin on axis 푧0 , and obtain a right-handed frame

2. Locate the origin 푂푖 at the intersection of 푧푖 with the common normal to axes 푧푖−1 and 푧푖

3. Establish 푥푖 axis. Establish or along the common normal between the 푧푖 and 푧푖−1 axes when they are parallel.

4. Establish 푦푖 axis. Assign to complete the right-handed coordinate system. 5. Find the link and joint parameters 6. Using the defined parameters, compute the homogeneous transformation matrices. 푖−1 퐴푖 푞푖 푓표푟 𝑖 = 1, … , 푛. 표 표 1 푛−1 7. Compute homogeneous transformation. 푇푛 푞 = 퐴1 푞1 퐴2 푞2 … 퐴푛 푞푛 표 8. 푇푛 푞 is the position and orientation of the end-effector frame with respect to the base frame

Compliant Robotics 43 Peking University, Globex, July 2018 Denavit-Hartenberg Convention

Remark

ri and 훼푖 are always constant by geometry connection

If joint 𝑖 is revolute, 휃푖 are variables If joint 𝑖 is prismatic, 푑푖 are variables

Compliant Robotics 44 Peking University, Globex, July 2018 Three-linked arm example

Link 풓푖 훼푖 푑푖 휗푖 1 ? 2 ? 3

Compliant Robotics 45 Peking University, Globex, July 2018 Three-linked arm example

Link 퐫푖 훼푖 푑푖 휗푖

1 푎1 0 0 휗1

2 푎2 0 0 휗2

3 푎3 0 0 휗3

Compliant Robotics 46 Peking University, Globex, July 2018 Three-linked arm example

Link 퐫푖 훼푖 푑푖 휗푖

1 푎1 0 0 휗1

2 푎2 0 0 휗2

3 푎3 0 0 휗3

ri ri

Compare to elementary rotation matrix:

R: rotation matrix of z axis P: translation vector

DH arrives the same matrix!

Compliant Robotics 47 Peking University, Globex, July 2018 Three-linked arm example

Link 퐫푖 훼푖 푑푖 휗푖

1 푎1 0 0 휗1

2 푎2 0 0 휗2

3 푎3 0 0 휗3

Compliant Robotics 48 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

1. Choose base frame by locating the origin on axis 푧0 , and obtain a right-handed frame

2. Locate the origin of frame 푂푖 at the intersection of 푧푖 with the common normal to axes 푧 and 푧 parallel 푖−1 푖

Base frame If joint 𝑖 is revolute, axes 푧푖−1 and 푧푖 are parallel

Compliant Robotics 49 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

3. Establish 푥푖 axis. Establish or along the common normal between the 푧푖 and 푧푖−1 axes when they are parallel.

4. Establish 푦푖 axis. Assign to complete the right-handed coordinate system.

Compliant Robotics 50 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

5. Find the link and joint parameter, and form the table of parameters

Link 풓푖 훼푖 푑푖 휗푖

0-1 0 휋/2 0 휗1

1-2 푎2 0 0 휗2

2-3 푎3 0 0 휗3

revolution joint with height =0 so: d =0

Compliant Robotics 51 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

6. Using the defined parameters, compute the homogeneous transformation matrices. 푖−1 퐴푖 푞푖 푓표푟 𝑖 = 1,2,3.

Compliant Robotics 52 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

표 7. Compute 푇푛 푞 . It represents the position and orientation of the end-effector frame with respect to the base frame

0 0 1 2 푇3 푞 = 퐴1퐴2퐴3

푐1푐23 −푐1푠23 푠1 푐1(푎2푐2 + 푎3푐23) 푠1푐23 −푠1푠23 −푐1 푠1(푎2푐2 + 푎3푐23) = 푠 푐 23 23 0 푎2푠2 + 푎3푠23 0 0 0 1

Orientation Position

Compliant Robotics 53 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

Given : 표 표 휗1 0 to 45 → 휗2 0 to 90 표 표 → 휗3(0 to 90 ) → 휗1 (45 to −45 )

푎3 푎2 휗3

휗2

휗1

Compliant Robotics 54 Peking University, Globex, July 2018 Example – Anthropomorphic Arm

Given : 표 표 휗1 0 to 45 → 휗2 0 to 90 표 표 → 휗3(0 to 90 ) → 휗1 (45 to −45 )

Compliant Robotics 55 Peking University, Globex, July 2018 Example –Spherical Arm

1. Choose base frame by locating the origin on axis 푧0 , and obtain a right- handed frame 2. Locate the origin of frame 푂 at the prismatic 푖 intersection of 푧푖 with the common normal to axes 푧푖−1 and 푧푖

Compliant Robotics 56 Peking University, Globex, July 2018 Example –Spherical Arm

5. Find the link and joint parameter, and form the table of parameters

prismatic

Link 풓푖 훼푖 푑푖 휗푖

1 0 -휋/2 0 휗1

2 0 휋/2 푑2 휗2

3 0 0 푑3 0

Compliant Robotics 57 Peking University, Globex, July 2018 Example – Spherical Arm

6. Using the defined parameters, compute the homogeneous transformation matrices. 푖−1 퐴푖 푞푖 푓표푟 𝑖 = 1, … , 푛.

푐1 0 −푠1 0 0 푠1 0 푐1 0 퐴1 휗1 = prismatic 0 −1 0 0 0 0 0 1

푐2 0 푠2 0 1 푠2 0 −푐2 0 퐴2 휗2 = 0 1 0 푑2 0 0 0 1

1 0 0 0 2 0 1 0 0 Translation only 퐴3 푑3 = 0 0 1 푑3 0 0 0 1

Compliant Robotics 58 Peking University, Globex, July 2018 Example – Spherical Arm

0 0 1 2 푇3 푞 = 퐴1퐴2퐴3

푐1푐2 −푠1 푐1푠2 푐1푠2푑3 − 푠1푑2 푠1푐2 푐1 푠1푠2 푠1푠2푑3 + 푐1푑2 prismatic = −푠 푐 2 0 2 푐2푑3 0 0 0 1 Orientation Position

Compliant Robotics 59 Peking University, Globex, July 2018 Example – Spherical Arm

Given : 표 휗1 −90 to − 45 → 푑2 1 to 3 표 표 → 푑3(1 to 3) → 휗2 (0 to 45 ) → 휗1 (−45 to 0 )

prismatic

푑3 휗2 푑2

휗1

Compliant Robotics 60 Peking University, Globex, July 2018 Example – Spherical Arm

Given : 표 휗1 −90 to − 45 → 푑2 1 to 3 표 표 → 푑3(1 to 3) → 휗2 (0 to 45 ) → 휗1 (−45 to 0 )

prismatic

Compliant Robotics 61 Peking University, Globex, July 2018 Programming exercise

DHexecise

complete the spherical arm matlab simulation

Compliant Robotics 62 Peking University, Globex, July 2018 DH Transformation for continuum mechanism

Continuum mechanism • Flexible body • No physical joints • No direct DH Transformation

Flexible catheter

Compliant Robotics 63 Peking University, Globex, July 2018 Continuum Robot Arm

Catheter Kinematics for Intracardiac Navigation, TBME 09

The curvature constancy and the coupling between parameters yield:

θ5 = π / 2 − θ3 θ6 = π − θ2

The constant length of the distal end

(O4O6 ) is denoted by d7

Compliant Robotics 64 Peking University, Globex, July 2018 Summary

• Type of manipulators are defined depending on different combination of revolute and prismatic joints. • Forward kinematic describe pose of a robot by joint and link variables

• Poses of a robot end-effector and joints are completely described in space by its position and orientation

• Poses of a robot end-effector and each joint are compactly rewritten using the homogeneous transformation matrix

• D-H convention to derive forward kinematic using the homogeneous transformation for manipulators

Compliant Robotics 65 Peking University, Globex, July 2018