Rigid-Body Velocity and Robot Kinematics

Yuzhe Qin

• General Rigid-body Motion:

• 𝑇!×! = 𝑅 𝑝0#×$ 1 , where 𝑅 ∈ 𝑆𝑂 3 , 𝑝 ∈ 𝑅$

• 𝑇 represent position and orientation in a single matrix

• ,𝑝 ≜ 𝑝1 ∈ 𝑅!

• 𝑇 ,𝑝 change reference frame of 𝑝

Review: Homogenous Transformation

• Rotation (axis-angle):• Any rotation in 𝑅! is equivalent to rotation about a fixed axis "𝜔 ∈ 𝑅!

through an positive angle 𝜃

• Rotation: {"𝜔, 𝜃}

• General Rigid-Body Motion (screw motion):• Any 𝑆𝐸(3) motion is equivalent to rotating about axis "𝜔 ∈ 𝑅!

through angle 𝜃 while also translating along axis for 𝑑. This axis pass through point 𝑞 ∈ 𝑅!

• Homogenous Transformation: {"𝜔, 𝜃, 𝑞, 𝑑}

Review: Physical Interpretation

Topics

• Exponential Coordinate of SE(3)

• Rigid-Body Velocity

• Robot Kinematics

• Case study: Hand-Eye Calibration

• Exponential coordinate:• .𝜔𝜃 ∈ 𝑅$

• Skew-symmetric Matrix:• .𝜔 𝜃 ∈ 𝑠𝑜(3)

• Rotation matrix:• 𝑅 = 𝑒[&']) ∈ 𝑆𝑂 3

• Interpretation:• Axis-angle

Recall: the Lie Algebra of SO(3)

• Exponential coordinate:• .𝜔𝜃 ∈ 𝑅$

• Skew-symmetric Matrix:• .𝜔 𝜃 ∈ 𝑠𝑜(3)

• Rotation matrix:• 𝑅 = 𝑒[&']) ∈ 𝑆𝑂 3

• Interpretation:• Axis-angle

Goal: The Lie Algebra of SE(3)

• Exponential coordinate:• 6𝜉𝜃 ∈ 𝑅*

• 𝑠𝑒(3) matrix:• 6𝜉 𝜃 ∈ 𝑠𝑒(3)

• Homogenous transformation matrix:

• T= 𝑒[+,]) ∈ 𝑆𝐸(3)

• Interpretation:• Screw motion

• Consider a point 𝑞 in body frame. At time 𝑡 = 0, the position is 𝑞-. Rotate 𝑞 with unit angular velocity around axis .𝜔:• 𝑣 = "𝜔 × 𝑟

• �̇� 𝑡 = "𝜔 × 𝑞 𝑡 = "𝜔 𝑞(𝑡)

• q t = 𝑒 "# $𝑞%

• Since 𝜃 𝑡 = 𝑡𝑝 𝜃 = 𝑒 &' )𝑝-

• .𝜔 ∈ 𝑠𝑜(3)

Recall: Find so(3) via ODE

𝑣!

• Consider a point 𝑝 in body frame. Rotate 𝑝 with unit angular velocity around fixed axis .𝜔, 𝑞 is any point on this axis, the linear velocity along axis .𝜔 is 𝑣':

• �̇� 𝑡 = "𝜔× 𝑝 𝑡 − 𝑞 + 𝑣# = "𝜔 𝑝 𝑡 − "𝜔 ×𝑞 + 𝑣#

• 𝐴 ≜ "𝜔 − "𝜔 𝑞 + 𝑣#0 0

≜ "𝜔 𝑣0 0

, ?𝑝 𝑡 = 𝑒&$ ?𝑝%

• Since 𝜃 𝑡 = 𝑡?𝑝 𝜃 = 𝑒&' ?𝑝%

• For matrix A, 𝑒&' ∈ 𝑆𝐸(3)

𝐴𝜃 ∈ 𝑠𝑒(3)

Find se(3) via ODE of Screw Motion

• For rotation, define .𝜔 𝜃 ∈ 𝑠𝑜 3

• For homogenous transformation, 6𝜉𝜃 = .𝜔𝑣 𝜃 ∈ 𝑅*:

𝑠𝑒 3 ≜ { 6𝜉 𝜃 = .𝜔 𝑣0 0

𝜃: 𝜔 𝜃 ∈ 𝑠𝑜 3 }

• Recall: .𝜔𝜃 is the exponential coordinate of 3D rotation

• Similarly, 6𝜉𝜃 ∈ 𝑅* is the exponential coordinate of 𝑆𝐸(3)

• 6𝜉 is the direction of motion, which is also called Unit Twist

Exponential Coordinate of SE(3)

• 6𝜉 = .𝜔𝑣 , 𝑒 +, ) ∈ 𝑆𝐸(3)

𝑒 +, ) = 𝐼 + [ 6𝜉] +1 − 𝑐𝑜𝑠𝜃

𝜃.[ 6𝜉].+

𝜃 − 𝑠𝑖𝑛𝜃𝜃$

[ 6𝜉]$

• This formula has different form compared to 𝑒 &' ) for rotation

• Similarly, a log function exists uniquely: 𝑆𝐸 3 → 𝑠𝑒(3)

• Note that 6𝜉 means that the first three value has norm one, in another word .𝜔 is a unit vector, no guarantee for 𝒗

Exponential Mapping of se(3)

• Given 6𝜉/0/ 𝜃 = 𝜋, 0,0,0, 𝜋, 0 1, superscript: {𝑠} frame

1. Find 6𝜉/0/ = 1,0,0,0,1,0 1, 𝜃 = 𝜋

2. Find rotation matrix by .𝜔 = 1,0,0 1

3. Find screw axis .𝜔, 𝑞 by v = −.𝜔×𝑞 + 𝑣'

𝑣' = 0,0,0 1, −.𝜔×𝑞 = 0,1,0 1

q = 0,0,1 1

4. Find origin after transformation

Example: !𝝃𝜽 to SE(3)

• Given 6𝜉/0/ 𝜃 = 𝜋, 0,0,0, 𝜋, 0 1, superscript: {𝑠} frame

1. q = 0,0,1 1, .𝜔 = 1,0,0 1, 𝑣' = 0,0,0 1

2. Recall screw motion

𝑇/0 =100

0−10

0 0

00−1

002

0 1

Same result using exponential mapping

Example: !𝝃𝜽 to SE(3)

Given 𝑆𝐸(3), find screw motion 𝑇-. =−100

0−10

0 0

001

011

0 1

Example: SE(3) to !𝝃𝜽

Given 𝑆𝐸(3), find screw motion

• 𝑇-. =−100

0−10

0 0

001

011

0 1

• Consider rotation only (Inverse Rodrigues): • 𝑡𝑟 𝑅 = −1⇒𝜃 = 𝜋

• "𝜔 = ()((+,!!)

𝑟(!𝑟)!

1 + 𝑟!!=

001

Example: SE(3) to !𝝃𝜽

Given 𝑆𝐸(3), find screw motion

• 𝑇-. =−100

0−10

0 0

001

011

0 1

, .𝜔 =001

• Find screw axis 𝑞 = 0, #., 0

1

• Since v𝜃 = −𝜔×𝑞 + 𝑣'𝜃 =2. , 0,1

3

• Find 6𝜉𝜃 = 0,0, 𝜋, 2., 0,1

1

Example: SE(3) to !𝝃𝜽

• Exponential coordinate:• .𝜔𝜃 ∈ 𝑅$

• Skew-symmetric Matrix:• .𝜔 𝜃 ∈ 𝑠𝑜(3)

• Rotation matrix:• 𝑅 = 𝑒[&']) ∈ 𝑆𝑂 3

• Interpretation:• Axis-angle

Comparison: SO(3) and SE(3)

• Exponential coordinate:• 6𝜉𝜃 ∈ 𝑅*

• 𝑠𝑒(3) matrix:• 6𝜉 𝜃 ∈ 𝑠𝑒(3)

• Homogenous transformation matrix:

• T= 𝑒[+,]) ∈ 𝑆𝐸(3)

• Interpretation:• Screw motion

Topics

• Exponential Coordinate of SE(3)

• Rigid-Body Velocity

• Robot Kinematics

• Case study: Hand-Eye Calibration

𝑧!𝑧"

𝑥!

𝑥" 𝑦!

𝑧"

Question: for moving frame 𝑅(𝑡), find angular velocity 𝜔 at any time 𝑡

Consider a point 𝑞 fixed on a moving frame {𝑏}:

• Fact 1, change reference frame:• 𝑞. 𝑡 = 𝑅./ 𝑡 𝑞/ → �̇�. 𝑡 = �̇�./ 𝑡 𝑞/• �̇�. 𝑡 = �̇�./ 𝑡 𝑅./0 𝑡 𝑅./ 𝑡 𝑞/ = �̇�./ 𝑡 𝑅./0 𝑡 𝑞.(𝑡)

• Fact 2, physical interpretation:• �̇�. 𝑡 = 𝜔 𝑡 𝑞.(𝑡) = 𝜔 𝑡 𝑞.(𝑡)

Angular Velocity from SO(3)

Question: for moving frame 𝑅(𝑡), find angular velocity 𝜔 at any time 𝑡

• Fact 1:• 𝑞. 𝑡 = 𝑅./ 𝑡 𝑞/ → �̇�. 𝑡 = �̇�./ 𝑡 𝑞/• ̇𝑞. 𝑡 = �̇�./ 𝑡 𝑅./1( 𝑡 𝑅./ 𝑡 𝑞/ = �̇�./ 𝑡 𝑅./1( 𝑡 𝑞.(𝑡)

• Fact 2, physical interpretation:• �̇�. 𝑡 = 𝜔 𝑡 𝑞.(𝑡)

• For any 𝑞 and 𝜔 𝜔 𝑡 𝑞/ 𝑡 = �̇�/0 𝑡 𝑅/07# 𝑡 𝑞/(𝑡):

𝜔(𝑡) = �̇�CD 𝑡 𝑅CDEF 𝑡

Angular Velocity from SO(3)

𝜔(𝑡) = �̇�CD 𝑡 𝑅CDEF 𝑡

• Why �̇�/0 𝑡 𝑅/0 𝑡 can represent angular velocity?

• �̇�/0 𝑡 𝑅/0 𝑡 is a skew-symmetric matrix:

• 𝑅 𝑡 𝑅1 𝑡 = 𝐼89:;<=>;<9

�̇� 𝑡 𝑅1 𝑡 + 𝑅 𝑡 �̇�1 𝑡 = 0

• �̇� 𝑡 𝑅1 𝑡 = −𝑅 𝑡 �̇�1 𝑡 = − �̇� 𝑡 𝑅1 𝑡1

• 𝑅 𝑡 �̇�1 𝑡 ∈ 𝑠𝑜(3), which is a skew-symmetric matrix

Angular Velocity of Rigid-Body

Velocity of general motion can be represented as twist:

Similarly, 𝜉 = �̇�(𝑡) 𝑇7#(𝑡), 𝑇 ∈ 𝑆𝐸(3)

𝜉CDC = �̇�CD𝑇CDEF , which is called Spatial Twist

• Recall: exponential coordinate of 𝑆𝐸(3) is 6𝜉𝜃

• 6𝜉 is called unit twist, where the .𝜔 in 6𝜉 is a unit vector

General Velocity of Rigid-Body

• Unit twist is the direction of motion:

• 6𝜉𝜃 can representation 𝑆𝐸(3) motion

• Twist is the velocity of general rigid-body motion:

• 𝜉 contains angular velocity and “linear velocity”

• 𝜉𝑡 can representation 𝑆𝐸(3) motion

What is a Twist?

• Any motion is relative: e.g. the velocity of frame {2} with respect to frame {1}

• Velocity of {2} relative to {1} is 𝑣#. :

How to Record a Velocity

𝑥"𝑦"

𝑧"

𝑥#

𝑦$

𝑧%

𝑣# 𝑥$ 𝑦$

𝑧$𝑣$

𝑣#𝑣$

𝑣#$

• Any motion is relative: e.g. the velocity of frame {2} with respect to frame {1}

• Velocity of {2} relative to {1} is 𝑣#. :

• Question: in which frame we record the velocity?• In order to write down 𝑣(), we a set of 𝑥, 𝑦, 𝑧 basis, here

we have three sets: {𝑠}, {1}, {2}

• If we record this velocity using basis of {s}:• The vector 𝑣() is recorded as 𝑣().

• Similarly, using {1} to record:• The velocity 𝑣() is recorded as 𝑣()(

How to Record a Velocity

𝑥"𝑦"

𝑧"

𝑥#

𝑦$

𝑧%

𝑣# 𝑥$ 𝑦$

𝑧$𝑣$

𝑣#𝑣$

𝑣#$

• Any twist is relative: e.g. the twist of frame {2} with respect to frame {1}, since twist is a generalized velocity

• Twist of {2} relative to {1} is 𝜉#. :

• Thus, 𝜉#./ means record 𝜉#. using basis of {𝑠}

• The superscription of twist 𝜉@ is where the twist is recorded

• This is a reflection of Galilean Transformation Invariance

Record a Twist

• How to change the reference frame of a twist

• Adjoint matrix is used to change the reference frame of twist

• Adjoint matrix: 𝐴𝑑1 ≜ 𝑅 0𝑝 𝑅 𝑅 ∈ 𝑅*×*

• 𝜉#./ = 𝐴𝑑1!" 𝜉#.0 =

𝑅/0𝜔#.0

𝑝!" 𝑅!"𝜔#$" + 𝑅!"𝑣#$"

• Where 𝑇"! =𝑅"! 𝑝"!0 1 , 𝜉()/ =

𝜔()/

𝑣#.0

• Equivalently, 𝜉#./ = 𝑇/0[𝜉#.0 ]𝑇/07#

Adjoint Matrix

• 𝜉/0/ = �̇�/0𝑇/07# , which is called Spatial Twist

• 𝜉/00 = 𝑇/07# �̇�/0, which is called Body Twist

• Spatial Twist: velocities of the point in the body frame that corresponds with the origin of the world frame

Spatial Twist and Body Twist

• Given the motion of rigid-body

• 𝑇GH 𝑡 =

𝑐𝑜𝑠𝜃(𝑡)𝑠𝑖𝑛𝜃(𝑡)

0

−𝑠𝑖𝑛𝜃(𝑡)𝑐𝑜𝑠𝜃(𝑡)

00 0

001

−𝑙.𝑠𝑖𝑛𝜃(𝑡)𝑙# + 𝑙.𝑐𝑜𝑠𝜃(𝑡)

𝑙-0 1

• What is the spatial twist?

• What is the body twist?

Example: General Rigid-Body Velocity

𝑇HG 𝑡 =𝑐𝑜𝑠𝜃(𝑡)𝑠𝑖𝑛𝜃(𝑡)

0

−𝑠𝑖𝑛𝜃(𝑡)𝑐𝑜𝑠𝜃(𝑡)

00 0

001

−𝑙.𝑠𝑖𝑛𝜃(𝑡)𝑙# + 𝑙.𝑐𝑜𝑠𝜃(𝑡)

𝑙-0 1

• [𝜉HGH ] = �̇�𝑇7#, 𝜉HGH = [0,0,1, l#, 0,0]

• 𝜉HGG = 𝑇7#�̇� = 𝐴𝑑1#$%& [𝜉HGH ]

Example: General Rigid-Body Velocity

Topics

• Exponential Coordinate of SE(3)

• Rigid-Body Velocity

• Robot Kinematics

• Case study: Hand-Eye Calibration

Kinematics: • Motion of bodies including spatial relationship of

different objects and their velocity. Kinematics does not consider how to achieve motion via force

Robot Kinematics

Link:• Links are the rigid-body connected in sequence

Joint: • Joints are the movable components of

a robot/object that cause relative motion between adjacent links

Link and Joint

Joint:• Revolute/Hinge/Rotational joint

• Prismatic/Translational joint

Two Common Joint Type

Robot Forward Kinematics:• Calculate the position and orientation of a robot link

(often end-effector) given its joint variables 𝜃= (𝜃#, 𝜃., … . . , 𝜃I)

• Before calculation, we need to assign a frame at each robot link

Forward Kinematics

Robot forward kinematics calculation:• Represent the motion of each joint in 𝑆𝐸(3)

• Simply multiply each matrix

𝑇%( = 𝑅𝑜𝑡(�̂�, 𝜃()𝑐𝑜𝑠𝜃(𝑠𝑖𝑛𝜃(0

𝑠𝑖𝑛𝜃(𝑐𝑜𝑠𝜃(0

0 0

001

000

0 1

, similarly 𝑇(), 𝑇)!, 𝑇!2

𝑇%2 = 𝑇%(𝑇()𝑇)!𝑇!2

Example: Forward Kinematics

Frame assignment in 3D space is not trivial

Can we find a unique way to assign frames?

• Denavit-Hartenberg (D-H) Parameters: • Applying a set of rules which specify the position and

orientation of frames attached to each link of the robot

D-H Parameters

• Denavit-Hartenberg (D-H) Parameters: • Redundancy of link relative pose, no need to use 6-DoF 𝑆𝐸(3)

representation

• Link offset 𝑑3: translate along 𝑧31(• Link length 𝑎3: translate along 𝑥3• Twist angle 𝜃3: rotate along 𝑥3

• No need to understand these parameters in this course

• D-H parameters can represent robot kinematics model

D-H Parameters

Example: D-H Table

• Recall: In algebra course, Jacobian of a function 𝑥 = 𝑓 𝜃can be defined as, where 𝑥 ∈ 𝑅J, 𝜃 ∈ 𝑅I:

J 𝜃 ≜ KLK) 𝜃 = KL'

K)(∈ 𝑅J×I

• Jacobian can relate the derivative of two variable together, if both 𝑥 and 𝜃 is the function of another variable 𝑡

�̇� =𝜕𝑓𝜕𝜃

𝜃𝑑𝜃𝑑𝑡

= 𝐽 𝜃 �̇�

Jacobian

Question: where will robot end-effector move given velocity of each joint?

• Velocity for a 𝑆𝐸(3) pose can be represented as twist 𝜉

• Geometric Jacobian 𝐽(𝜃):

𝜉 = 𝜔𝑣 = 𝐽 𝜃 �̇�, where 𝐽 𝜃 ∈ 𝑅*×I, n is robot DoF

• The i-th column of 𝐽(𝜃) is the twist when the robot is moving about the i-th joint at unit speed ̇𝜃; = 1 while all other joints stay static

Geometric Jacobian

Question: how to compute Geometric Jacobian?

• Using forward kinematics: 𝑇/0 𝜃 = 𝑓(𝜃)

• General velocity: [𝜉/0/ ] = �̇�/0𝑇/07#, where �̇�/0 =K1!"K>

= 𝑔(�̇�)

• Compute each column of geometric Jacobian 𝐽(𝜃):

Let �̇�; = 1 and ̇𝜃M = 0 for 𝑗 ≠ 𝑖

i-th column of 𝐽(𝜃) will be K1!"K)̇

𝑇/07#

Geometric Jacobian Calculation

Inverse Kinematics (IK): • Given the forward kinematics 𝑇(𝜃) and the target pose TOPQRSO ∈ 𝑆𝐸 (3), find solutions 𝜃 that satisfy 𝑇(𝜃)= 𝑇>=:T9>

• Analytical solution of IK for robot with more than 3-DoF is very complex: • For a 6-DoF robot, you will need a several pages to write down

the formula

• If you need analytical solution, just use libraries:• IKFast, IKBT

Inverse Kinematics

Numerical IK, a root finding problem• Inverse kinematics can be viewed as finding roots of a

nonlinear equation with 𝑆𝐸(3) constrain

• Standard root-finding algorithm can be adapted for 𝑇(𝜃) = 𝜉0, e.g. Newton-Raphason method

• The gradient for this method is geometric Jacobian

𝐽 𝜃; = KLK) |)' , Δ𝜃

3 = 𝐽∔ 𝜃3 Δ𝜉,

where Δ𝜉 is the difference of pose in exponential coordinate

𝜃3+( = 𝜃 + Δ𝜃3

Numerical Solution of IK

Question: is it possible to move end-effector to any direction 6𝜉for a robot with 𝐷𝑜𝐹 ≥ 6

• The pseudo inverse 𝐽∔(𝜃) map link twist back to joint velocity

• Kinematic singularity:• A robot configuration where the robot’s end-effector loses the ability

to move in one direction instantaneously

• Mathematically, 𝐽(𝜃) rank deficiency leads to kinematic singularity

• Kinematic singularity does not mean that there exists a configuration that is not accessible

Kinematic Singularity

Topics

• Exponential Coordinate of SE(3)

• Rigid-Body Velocity

• Robot Kinematics

• Case study: Hand-Eye Calibration

• Visual perception comes from camera/lidar, the position and orientation is captured in sensor frame• E.g., point cloud in camera frame

• Moving signal is command in robot frame• E.g., move the robot hand left in robot base frame

• Hand-eye Calibration computes the transformation from camera to robot

Vision-based Robotic Planning

• There are two kinds of problem for hand-eye calibration

• Eye-in-hand (camera mounted on hand):

• Eye-to-hand (camera not fixed with hand):

Settings of Hand-eye Calibration

https://www.torsteinmyhre.name/snippets/robcam_calibration.html

Take eye-in-hand (e.g. camera fixed to hand) as example• Goal: transformation from camera to hand 𝑇VW

• Denote: space frame (robot base) {𝑠}, camera {𝑐},hand {ℎ}, and marker (auxiliary object) {𝑚}

Hand-Eye Transformation Equation

𝑇CV𝑇VW𝑇WX = TYX

Take eye-in-hand (e.g. camera fixed to hand) as example• Assume we can get the pose of marker in camera frame

Hand-Eye Transformation Equation

𝑇CV𝑇VW𝑇WX = TYX

KnownVariant

What we wantConstant

Assume knownVariant

UnknownConstant

To solve the hand-eye transformation equation, we need to prepare multiple pairs of 𝑇/V and 𝑇WJ

• Repeat the following steps for n-times:1. Move the robot hand to a target pose, where camera

can see the marker

2. Capture the 𝑇/V; for i-th pose of hand to base, often calculated by forward kinematics

3. Capture the 𝑇WJ; for i-th pose of marker to camera, calculated by a marker-specific algorithm

Capture Calibration Data

• The pose from marker to spatial frame 𝑇/J is fixed

𝑇.53 𝑇56 𝑇673 = T87 = 𝑇.53+( 𝑇56 𝑇673+(

𝑇.53+(1(𝑇.53 𝑇56= 𝑇56 𝑇673+( 𝑇673

1(

• Now we get a 𝐴𝑋 = 𝑋𝐵 type function with constraints• A = 𝑇.53+(

1(𝑇.53 and 𝐵 = 𝑇673+( 𝑇673

1( are all known

• It is common to use multiple pairs of data for the equation. Actually, at least three pairs are necessary in order for a unique solution.

AX=XB for Hand-Eye Calibration

Horaud et al., “Hand-eye Calibration”, IJRR 1995

Note that the 𝑋 ∈ 𝑆𝐸(3), which is a constrain to this equation

• Two mainstream to solve this equation

1. Determine first rotation and then translation1

2. Determine rotation and translation simultaneously2

• To solve the equation more precisely:• Poses of hand are chosen follow some solver-specific guidelines

• More data

Solving AX=XB

Tsai et al., “A new technique for fully autonomous and efficient 3 D robotics hand/eye calibration”, TRO 1989

Daniilidis, “Hand-Eye Calibration Using Dual Quaternions”, IJRR 1999

• Checkerboard is a most common visual marker in robotics:• Checkerboard pose can be easily solved using standard method

like PnP

• As long as we have method to estimate its pose, anything can be a marker

Markers for Hand-eye Calibration