kinematics forward kinematic - pku.edu.cn

Kinematics – Forward Kinematic

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

Compliant Robotics Peking University, Globex, July 20182

Introduction – Forward and Inverse Kinematics

Inverse

Forward


Linear Algebra –Dot Product

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


Linear Algebra –Dot Product

[1, 0]

[1, 1]

[2, 0]

[0, 2]

[0, 1] [0, 2]

if A and B are orthogonal

if they are codirectional


Linear Algebra –Matrix Multiplication

=Ai ∙ Bj

n x m

m x p

n x p


• Example


𝑎 𝑏𝑐 𝑑

𝑒 𝑓𝑔 ℎ

= ?

𝑎 𝑏𝑐 𝑑

=𝑒 𝑓𝑔 ℎ

?



Square matrices

AB ≠ BA



Row vector and column vector- Dimensionality

3 ✕11✕ 3



Square matrix and column vector



If AT = C = x

Then: ABC = xTAx

Quadratic scaler function using matrix representation



Rectangular matrices


Linear Algebra


Rigid body motion

Translation + Rotation


Rotation Matrix in 2D

x’

y

x

y’

θ



x’

y

x

y’

θ(cosθ, sinθ)

(-sinθ, cosθ)

A

B

Rab=[xa

b yab]

pa= Rab pb



x’

y

x

y’

θ

(cosθ, -sinθ)

(sinθ, cosθ)pb= Rab-1 pa

RabT = Ra

b-1

RabT Ra

b = I



Rotation of a rigid object about a point. A be the inertial

frame, B the body frame, and xab, yab, zab ∈ R3 the

coordinates of the axes of B relative to A

A

B



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.

coordinate axes of B, which, in turn, have coordinates xab, yab, zab ∈ R3 with

respect to A

Coordinates of q relative to frame A are given by


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


Rotation matrix

𝑅𝑇𝑅 = 𝐼3 =1 0 00 1 00 0 1

𝑅𝑇 = 𝑅−1

det(𝑅) = 1


Rotation matrix – Elementary rotation

The rotation matrix of frame 𝑂–𝑥′𝑦′𝑧′

with respect to frame 𝑂– 𝑥𝑦𝑧

𝑅𝑧 𝛼 =cos 𝛼 −sin𝛼 0sin 𝛼 cos 𝛼 00 0 1

𝑅𝑦 𝛽 =cos 𝛽 0 sin 𝛽0 1 0

−sin 𝛽 0 cos 𝛽

𝑅𝑥 𝛾 =1 0 00 cos 𝛾 −𝑠𝑖𝑛 𝛾0 𝑠𝑖𝑛 𝛾 cos 𝛾


Rotation matrix – Example of a 3D vector rotation

𝑝 =001

𝑝1 = 𝑅𝑥 𝜃𝑥 𝑝

𝑅𝑥 𝜃𝑥 𝑝2 = 𝑅𝑦 𝜃𝑦 𝑝1

𝑅𝑦 𝜃𝑦

𝑅𝑧 𝜃𝑧

𝑝3 = 𝑅𝑧 𝜃𝑧 𝑝2

𝑝 = 𝑅𝑥𝑇 𝜃𝑥 𝑅𝑦

𝑇 𝜃𝑦 𝑅𝑧𝑇 𝜃𝑧 𝑝3

𝑝3 = 𝑅𝑧 𝜃𝑧 𝑅𝑦 𝜃𝑦 𝑅𝑥 𝜃𝑥 𝑝


Euler Angle

A representation of orientation in terms of three independent parameters constitutes

a minimal representation.

𝜃 = 𝜃𝑥 𝜃𝑦 𝜃𝑧 T

𝑝1 = 𝑅𝑥 𝜃𝑥 𝑅𝑦 𝜃𝑦 𝑅𝑧 𝜃𝑧 𝑝

𝑝 =001

, 𝜃𝑥 = 𝜃𝑦 = 𝜃𝑧 = 45𝑜

𝑝2 = 𝑅𝑧 𝜃𝑧 𝑅𝑦 𝜃𝑦 𝑅𝑥 𝜃𝑥 𝑝?


Euler Angle

A representation of orientation in terms of three independent parameters

constitutes a minimal representation.

𝜃 = 𝜃𝑥 𝜃𝑦 𝜃𝑧 T

𝑝1 = 𝑅𝑥 𝜃𝑥 𝑅𝑦 𝜃𝑦 𝑅𝑧 𝜃𝑧 𝑝

𝑝 =001

, 𝜃𝑥 = 𝜃𝑦 = 𝜃𝑧 = 45𝑜

𝑝2 = 𝑅𝑧 𝜃𝑧 𝑅𝑦 𝜃𝑦 𝑅𝑥 𝜃𝑥 𝑝≠

𝑝1𝑝2

𝑝


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

𝐴10 = 𝑅1

0 𝑜10

0 1

𝑃𝑜

1= 𝐴1

0 𝑃1

1






𝐴 =𝑅 𝑃0 1

𝑁𝑜𝑡𝑒 ∶ 𝐴−1 ≠ 𝐴𝑇

A-1

Proof execise: A A-1






𝑃1

1= 𝐴0

1 𝑃0

1

𝑃1

1= 𝑅0

1 −𝑅01𝑜1

0

0 1𝑃0

1

𝐴10 = 𝑅1

0 𝑜10

0 1

𝑃𝑜

1= 𝐴1

0 𝑃1

1



Aac= Aab Abc =

Proof exercise

pbc

C



Translation without rotation

P

𝑧

𝑦

𝑥

𝑧′

𝑦′

𝑥′

=

1000

P100

P010

P001

z

y

x

A

𝑧

𝑦

𝑥

𝑧′

𝑥′Rotation without translation

=

1000

0

0

0

zzz

yyy

xxx

'''

'''

'''

zyx

zyx

zyx

A



• Finding the Homogeneous Matrix (an open kinematic chain)

𝑇𝑛𝑜 𝑞 = 𝐴1

𝑜 𝑞1 𝐴21 𝑞2 …𝐴𝑛

𝑛−1 𝑞𝑛

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


Programming exercise in class

open exercise1

homogeneousmatrix.mrotationchain_q.m



Kinematics for manipulators

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


Typical Manipulator Structures

Depends on combination of revolute joint and prismatic joint

Three link planar arm,Parallelogram Arm,

Spherical arm,Spherical Wrist,

Stanford Manipulator,DLR Manipulator,

…


Typical Manipulator Structures

Spherical wrist

Example of combination of revolute joints


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 𝑞𝑛

• It can be applied to any open kinematic chain

D-H convention homogeneous transformation matrix and parameters

ri

ri



𝑟𝑖 ∶ 𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑏𝑒𝑡𝑤𝑒𝑒𝑛 𝑂𝑖 𝑎𝑛𝑑𝑂𝑖′

𝑑𝑖: 𝑑𝑒𝑝𝑡ℎ 𝑓𝑟𝑜𝑚 𝑂𝑖−1 𝑡𝑜 𝑂𝑖′ 𝑎𝑙𝑜𝑛𝑔 𝑧𝑖−1

(𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑏𝑒𝑡𝑤𝑒𝑒𝑛 𝑂𝑖−1 𝑎𝑛𝑑 𝑤ℎ𝑒𝑟𝑒 𝑐𝑜𝑚𝑚𝑜𝑛 𝑛𝑜𝑟𝑚𝑎𝑙 𝑟𝑖 𝑖𝑛𝑡𝑒𝑠𝑒𝑐𝑡 𝑤𝑖𝑡ℎ 𝑧𝑖−1)𝛼𝑖: 𝑎𝑛𝑔𝑙𝑒 𝑏𝑒𝑡𝑤𝑒𝑒𝑛 𝑎𝑥𝑒𝑠 𝑧𝑖−1 𝑎𝑛𝑑 𝑧𝑖𝑎𝑏𝑜𝑢𝑡 𝑎𝑥𝑖𝑠 𝑥𝑖𝜗𝑖: 𝑎𝑛𝑔𝑙𝑒 𝑏𝑒𝑡𝑤𝑒𝑒𝑛 𝑎𝑥𝑒𝑠 𝑥𝑖−1 𝑎𝑛𝑑 𝑥𝑖 𝑎𝑏𝑜𝑢𝑡 𝑎𝑥𝑖𝑠 𝑧𝑖−1

• 𝑟 and 𝛼𝑖 are always constant by geometry connection

ri-1

ri

Important: Locate the origin 𝑂𝑖 at

the intersection of 𝑧𝑖 with the

common normal to axes 𝑧𝑖−1 and 𝑧𝑖


DH Homogeneous transformation

Frame i-1 translate by

di along zi-1 and rotate

by θi about zi-1

Translate by ri along xi’

and rotate by αi about xi’

ri

riri


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 axeswhen 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,… , 𝑛.

7. Compute homogeneous transformation. 𝑇𝑛𝑜 𝑞 = 𝐴1


𝑛−1 𝑞𝑛

8. 𝑇𝑛𝑜 𝑞 is the position and orientation of the end-effector frame with respect to the

base frame


Denavit-Hartenberg Convention

If joint 𝑖 is revolute, 𝜃𝑖 are variablesIf joint 𝑖 is prismatic, 𝑑𝑖 are variables

ri and 𝛼𝑖 are always constant by geometry connection

Remark


Three-linked arm example

Link 𝒓𝑖 𝛼𝑖 𝑑𝑖 𝜗𝑖

1 ?

2 ?

3



Link 𝐫𝑖 𝛼𝑖 𝑑𝑖 𝜗𝑖

1 𝑎1 0 0 𝜗1

2 𝑎2 0 0 𝜗2

3 𝑎3 0 0 𝜗3




1 𝑎1 0 0 𝜗1

2 𝑎2 0 0 𝜗2

3 𝑎3 0 0 𝜗3

riri

Compare to elementary rotation

matrix:

R: rotation matrix of z axis

P: translation vector

DH arrives the same matrix!




1 𝑎1 0 0 𝜗1

2 𝑎2 0 0 𝜗2

3 𝑎3 0 0 𝜗3


Example – Anthropomorphic Arm

1. Choose base frame by locating the originon axis 𝑧0 , and obtain a right-handedframe

2. Locate the origin of frame 𝑂𝑖 at the

intersection of 𝑧𝑖 with the common

normal to axes 𝑧𝑖−1 and 𝑧𝑖

Base frame If joint 𝑖 is revolute, axes 𝑧𝑖−1 and 𝑧𝑖 areparallel

parallel



3. Establish 𝑥𝑖 axis. Establish or along the

common normal between the 𝑧𝑖 and 𝑧𝑖−1axes when they are parallel.

4. Establish 𝑦𝑖 axis. Assign to complete the

right-handed coordinate system.



5. Find the link and joint parameter, and

form the table of parameters


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



6. Using the defined parameters, compute

the homogeneous transformation matrices.

𝐴𝑖𝑖−1 𝑞𝑖 𝑓𝑜𝑟 𝑖 = 1,2,3.



7. Compute 𝑇𝑛𝑜 𝑞 . It represents the

position and orientation of the end-effector

frame with respect to the base frame

𝑇30 𝑞 = 𝐴1

0𝐴21𝐴3

2

=

𝑐1𝑐23𝑠1𝑐23𝑠230

−𝑐1𝑠23−𝑠1𝑠23𝑐230

𝑠1−𝑐100

𝑐1(𝑎2𝑐2 + 𝑎3𝑐23)𝑠1(𝑎2𝑐2 + 𝑎3𝑐23)𝑎2𝑠2 + 𝑎3𝑠23

1

Orientation Position



𝑎2

𝑎3

𝜗1

𝜗2

𝜗3

Given : 𝜗1 0 to 45𝑜 → 𝜗2 0 to 90𝑜

→ 𝜗3(0 to 90𝑜) → 𝜗1 (45 to −45

𝑜)



Given : 𝜗1 0 to 45𝑜 → 𝜗2 0 to 90𝑜

→ 𝜗3(0 to 90𝑜) → 𝜗1 (45 to −45

𝑜)


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

intersection of 𝑧𝑖 with the common

normal to axes 𝑧𝑖−1 and 𝑧𝑖

prismatic


Example –Spherical Arm

prismatic

5. Find the link and joint parameter, and

form the table of parameters


1 0 -𝜋/2 0 𝜗1

2 0 𝜋/2 𝑑2 𝜗2

3 0 0 𝑑3 0


Example – Spherical Arm

prismatic

Translation only

6. Using the defined parameters, compute

the homogeneous transformation matrices.

𝐴𝑖𝑖−1 𝑞𝑖 𝑓𝑜𝑟 𝑖 = 1,… , 𝑛.

𝐴10 𝜗1 =

𝑐1𝑠100

00−10

−𝑠1𝑐100

0001

𝐴21 𝜗2 =

𝑐2𝑠200

0010

𝑠2−𝑐200

00𝑑21

𝐴32 𝑑3 =

1000

0100

0010

00𝑑31



prismatic

𝑇30 𝑞 = 𝐴1

0𝐴21𝐴3

2

=

𝑐1𝑐2𝑠1𝑐2−𝑠20

−𝑠1𝑐100

𝑐1𝑠2𝑠1𝑠2𝑐20

𝑐1𝑠2𝑑3 − 𝑠1𝑑2𝑠1𝑠2𝑑3 + 𝑐1𝑑2

𝑐2𝑑31

Orientation Position



prismatic

Given :

𝜗1 −90 to − 45𝑜 → 𝑑2 1 to 3→ 𝑑3(1 to 3) → 𝜗2 (0 to 45

𝑜) → 𝜗1 (−45 to 0𝑜)

𝑑2

𝑑3

𝜗1

𝜗2



prismatic

Given :

𝜗1 −90 to − 45𝑜 → 𝑑2 1 to 3→ 𝑑3(1 to 3) → 𝜗2 (0 to 45

𝑜) → 𝜗1 (−45 to 0𝑜)


Programming exercise

DHexecise

complete the spherical arm matlab simulation


DH Transformation for continuum mechanism

Continuum mechanism

• Flexible body

• No physical joints

• No direct DH Transformation

Flexible catheter


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


Summary

• Type of manipulators are defined depending on different combination of revolute

and prismatic joints.

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

its position and orientation

• Forward kinematic describe pose of a robot by joint and link variables

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

transformation for manipulators

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

homogeneous transformation matrix

kinematics forward kinematic - pku.edu.cn

Documents