rigid-body velocity and robot kinematics€¦ · question: is it possible to move end-effector to...
TRANSCRIPT
![Page 1: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/1.jpg)
Rigid-Body Velocity and Robot Kinematics
Yuzhe Qin
![Page 2: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/2.jpg)
• General Rigid-body Motion:
• 𝑇!×! = 𝑅 𝑝0#×$ 1 , where 𝑅 ∈ 𝑆𝑂 3 , 𝑝 ∈ 𝑅$
• 𝑇 represent position and orientation in a single matrix
• ,𝑝 ≜ 𝑝1 ∈ 𝑅!
• 𝑇 ,𝑝 change reference frame of 𝑝
Review: Homogenous Transformation
![Page 3: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/3.jpg)
• 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
![Page 4: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/4.jpg)
Topics
• Exponential Coordinate of SE(3)
• Rigid-Body Velocity
• Robot Kinematics
• Case study: Hand-Eye Calibration
![Page 5: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/5.jpg)
• Exponential coordinate:• .𝜔𝜃 ∈ 𝑅$
• Skew-symmetric Matrix:• .𝜔 𝜃 ∈ 𝑠𝑜(3)
• Rotation matrix:• 𝑅 = 𝑒[&']) ∈ 𝑆𝑂 3
• Interpretation:• Axis-angle
Recall: the Lie Algebra of SO(3)
![Page 6: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/6.jpg)
• 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
![Page 7: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/7.jpg)
• 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
![Page 8: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/8.jpg)
𝑣!
• 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
![Page 9: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/9.jpg)
• 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)
![Page 10: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/10.jpg)
• 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)
![Page 11: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/11.jpg)
• 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)
![Page 12: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/12.jpg)
• 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)
![Page 13: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/13.jpg)
Given 𝑆𝐸(3), find screw motion 𝑇-. =−100
0−10
0 0
001
011
0 1
Example: SE(3) to !𝝃𝜽
![Page 14: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/14.jpg)
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 !𝝃𝜽
![Page 15: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/15.jpg)
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 !𝝃𝜽
![Page 16: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/16.jpg)
• 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
![Page 17: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/17.jpg)
Topics
• Exponential Coordinate of SE(3)
• Rigid-Body Velocity
• Robot Kinematics
• Case study: Hand-Eye Calibration
![Page 18: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/18.jpg)
𝑧!𝑧"
𝑥!
𝑥" 𝑦!
𝑧"
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)
![Page 19: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/19.jpg)
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)
![Page 20: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/20.jpg)
𝜔(𝑡) = �̇�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
![Page 21: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/21.jpg)
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
![Page 22: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/22.jpg)
• 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?
![Page 23: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/23.jpg)
• 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
𝑥"𝑦"
𝑧"
𝑥#
𝑦$
𝑧%
𝑣# 𝑥$ 𝑦$
𝑧$𝑣$
𝑣#𝑣$
𝑣#$
![Page 24: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/24.jpg)
• 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
𝑥"𝑦"
𝑧"
𝑥#
𝑦$
𝑧%
𝑣# 𝑥$ 𝑦$
𝑧$𝑣$
𝑣#𝑣$
𝑣#$
![Page 25: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/25.jpg)
• 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
![Page 26: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/26.jpg)
• 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
![Page 27: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/27.jpg)
• 𝜉/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
![Page 28: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/28.jpg)
• 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
![Page 29: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/29.jpg)
𝑇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
![Page 30: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/30.jpg)
Topics
• Exponential Coordinate of SE(3)
• Rigid-Body Velocity
• Robot Kinematics
• Case study: Hand-Eye Calibration
![Page 31: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/31.jpg)
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
![Page 32: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/32.jpg)
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
![Page 33: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/33.jpg)
Joint:• Revolute/Hinge/Rotational joint
• Prismatic/Translational joint
Two Common Joint Type
![Page 34: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/34.jpg)
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
![Page 35: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/35.jpg)
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
![Page 36: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/36.jpg)
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
![Page 37: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/37.jpg)
• 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
![Page 38: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/38.jpg)
Example: D-H Table
![Page 39: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/39.jpg)
• 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
![Page 40: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/40.jpg)
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
![Page 41: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/41.jpg)
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
![Page 42: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/42.jpg)
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
![Page 43: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/43.jpg)
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
![Page 44: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/44.jpg)
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
![Page 45: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/45.jpg)
Topics
• Exponential Coordinate of SE(3)
• Rigid-Body Velocity
• Robot Kinematics
• Case study: Hand-Eye Calibration
![Page 46: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/46.jpg)
• 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
![Page 47: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/47.jpg)
• 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
![Page 48: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/48.jpg)
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
![Page 49: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/49.jpg)
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
![Page 50: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/50.jpg)
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
![Page 51: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/51.jpg)
• 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
![Page 52: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/52.jpg)
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
![Page 53: Rigid-Body Velocity and Robot Kinematics€¦ · Question: is it possible to move end-effector to any direction 76 for a robot with c2d≥6 •The pseudo inverse ]∔(0)map link twist](https://reader034.vdocuments.us/reader034/viewer/2022042621/5f527bb042705b0f1913c0e9/html5/thumbnails/53.jpg)
• 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