Intro to Robotics, In-Class Handout –Lecture 25: Inverse Velocity
Overview:
𝑞 → 𝑆𝐸 (3) Forward kinematics (easy after learning rotations, geometry, DH)
Must generate:
𝑆𝐸(3) → 𝑞 Inverse kinematics (hard), multiple or no solutions
Solve by:
• projecting onto lower dimensional subspaces & solve geometric problems
• Also use kinematic decoupling with a spherical wrist
𝑣
𝑞̇ → 𝜉 forward velocity kinematics 𝜉 = 𝐽𝑞̇ 𝜉 = [ ], needed skew symmetric matrices
𝜔
𝑣 = 𝐽𝑣 𝑞̇
𝜔 = 𝐽𝜔 𝑞̇
Example with single revolute-joint robot
,𝑥 = 𝑟1 cos(𝜃1 ), 𝑦 = 𝑟1 sin(𝜃1 ), 𝜃1 = atan2(𝑥, 𝑦)
𝑥̇ −𝑟1 sin (𝜃)
𝑦̇ 𝑟1 cos (𝜃)
𝑧̇ 0 𝑥̇ 𝑥̇ 𝑦̇
̇ = 𝜃̇, 𝜃̇ = −𝑟 sin (𝜃) iff −𝑟 sin (𝜃) = 𝑟 cos (𝜃)
𝑟𝑜𝑙𝑙 0 1 1 1
𝑝𝑖𝑡𝑐ℎ ̇ 0
[ 𝑦𝑎𝑤 ̇ ] [ 1 ]
Draw possible xy velocities:
When can we find solutions to 𝜉 = 𝐽𝑞̇ ? IFF rank[ 𝐽(𝑞)] = rank[ 𝐽(𝑞)| 𝜉]
(the rank of the augmented matrix [ 𝐽(𝑞)| 𝜉] is the same as the rank of the Jacobian 𝐽(𝑞), \xi is
in the range space of the Jacobian
What if 𝑛 > 6? (Hard to draw, so lets do a planar case)
Draw a 3-rotational link planar robot with 𝑟1 = 2, 𝑟2 = 1, 𝑟3 = 1
−𝑟1 𝑠1 − 𝑟2 𝑠12 − 𝑟3 𝑠123 −𝑟2 𝑠12 − 𝑟3 𝑠123 −𝑟3 𝑠123 −2𝑠1 − 𝑠12 − 𝑠123 −𝑠12 − 𝑠123 −𝑠123
𝑟1 𝑠1 + 𝑟2 𝑐12 + 𝑟3 𝑐123 𝑟2 𝑐12 + 𝑟3 𝑐123 𝑟3 𝑐123 2𝑠1 + 𝑐12 + 𝑐123 𝑐12 + 𝑐123 𝑐123
0 0 0 0 0 0 =
𝐽= =
0 0 0 0 0 0
0 0 0 0 0 0
[ 1 1 1 ] [ 1 1 1 ]
Page 1 of 6
Where do we lose DOF (where are the singularities)? Whenever all 𝜃𝑖 = 𝑘𝑖 𝜋, 𝑖 > 1
1
−√2 −√2 −
𝜋 𝜋 √2
𝐽𝑣 ([0, , ]) =
4 2 1
2 0 −
[ √2]
Draw it. What are the instantaneous velocities?
1/√2
How can we get 𝜉 = [ ], i.e. what is 𝑞̇ ?
1/√2
0
One way is: choose 𝜃̇ = [ 0 ]
−1
The norm is 1
Better way is to calculate the “pseudo inverse” 𝜃̇ = 𝐽𝑣𝑇 (𝐽𝑣 𝐽𝑣𝑇 )−1 𝜉 =
0.19074356983054613
[−0.46049571322036414]
−0.4604957132203642
Because norm is 0.81353
How did we get that? “Right Pseudo Inverse of 𝑱”
If 𝐽 ∈ ℝ𝑚×𝑛 if 𝑚 < 𝑛 and rank[ 𝐽(𝑞)] = 𝑚, then (𝐽𝐽T )−1 exists.
Page 2 of 6
−1
Why “right” psuedo inverse? right because 𝐽𝐽 + = 𝐼, 𝐽+ = 𝐽𝑇 (𝐽 𝐽𝑇 )
𝑞̇ = 𝐽+ 𝜉 + (𝐼 − 𝐽+ 𝐽)𝑏, 𝑏 ∈ ℝ𝑛
Null space of J:
All vectors in (𝐼 − 𝐽+ 𝐽)
To minimize joint velocity, pick 𝑏 = 0.
Right Pseudo Inverse of 𝑱
When 𝐽 ∈ ℝ𝑚×𝑛 , if 𝑚 < 𝑛, and rank of 𝐽 = 𝑚,
then (𝐽𝐽𝑇 )−1 exists.
Then 𝐽𝐽𝑇 ∈ ℝ𝑚×𝑚 and has rank 𝑚
𝐼 = (𝐽𝐽𝑇 )(𝐽𝐽𝑇 )−1 = 𝐽𝐽 +
𝐽 = 𝑈𝛴𝑉 𝑇
𝜎1
𝜎2 0
𝛴=
⋱
[ 𝜎𝑚 ]
𝜎1 ≥ 𝜎2 ≥ ⋯ ≥ 𝜎𝑚 ≥ 0
Cool trick with singular value decomposition
𝐽 = 𝑈𝛴𝑉 𝑇
𝐽+ = 𝑉𝛴 + 𝑈 𝑇
𝑇
𝜎1−1
𝜎2−1 0
𝛴+ =
⋱
−1
[ 𝜎𝑚 ]
Page 3 of 6
Manipulability
Set of inputs with unit norm: ‖𝑞̇ ‖2 = 𝑞̇ 12 + 𝑞̇ 22 + ⋯ + 𝑞̇ 𝑛2 ≤ 1
Minimum norm solution: 𝑞̇ = 𝐽+ 𝜉
‖𝑞̇ ‖2 = 𝑞̇ 𝑇 𝑞̇ =(𝐽 + 𝜉)𝑇 𝐽+ 𝜉 = 𝜉 𝑇 (𝐽𝐽𝑇 )−1 𝜉 ≤ 1
𝐽 = 𝑈𝛴𝑉 𝑇
‖𝑞̇ ‖2 = 𝜉 𝑇 (𝐽𝐽𝑇 )−1 𝜉 = (𝑈 𝑇 𝜉)𝑇 𝛴𝑚
−2 (𝑈 𝑇
𝜉) ≤ 1
−2
𝜎1
−2 𝜎2−2
𝛴𝑚 =
⋱
−2
[ 𝜎𝑚 ]
Substitute 𝑤 = 𝑈 𝑇 𝜉
𝑤𝑖2
(𝑤)𝑇 𝛴𝑚
−2
𝑤
=∑ 2 ≤1
𝜎𝑖
This is the equation for an axis-aligned ellipse in the coordinate system obtained by
rotating by U T
Axes of ellipsoid: 𝜎𝑖 𝑢𝑖
Volume of ellipsoid: 𝐾𝜎1 𝜎2 ⋯ 𝜎𝑚
4
𝐾 is a constant depends on dimension 𝑚 of ellipsoid. 𝑚 = 2, 𝐾 = 𝜋, 𝑚 = 3, 𝐾 = , …
3𝜋
𝜇 is the Manipulability measure
𝜇 = 0 iff 𝑟𝑎𝑛𝑘(𝐽) < 𝑚 (𝐽 is not full rank)
‖𝛥𝑞̇ ‖
error in measured velocity 𝛥𝜉, then (𝜎1 )−1 ≤ ≤ (𝜎𝑚 )−1
‖𝛥𝜉‖
If a designer starts the study of the kinematics of a robot, it is essential to analyze
the manipulability, one of the most important parameters of functionality of a robotic
manipulator; in fact, this term has a major impact on design since it facilitates the definition of
kinematics performance indicators which allow optimizing the size of the robot.
Page 4 of 6
Intro to Robotics, In-Class Handout – prep for Lecture 26: Motion Planning
Three Laws of robotics:
1. A robot may not injure a human being or, through inaction, allow a human being to
come to harm.
2. A robot must obey orders given it by human beings except where such orders would
conflict with the First Law.
3. A robot must protect its own existence as long as
such protection does not conflict with the First or
Second Law.
Equations for “simple” car motion:
𝑥̇ = 𝑢𝑠 cos(𝜃)
𝑦̇ = 𝑢𝑠 sin(𝜃)
𝑢𝑠
𝜃̇ = tan (𝑢ϕ )
𝐿
Consider path traced out by pencil attached to center of rear axle
task: minimize length of curve as car travels between any 𝑞𝐼 and 𝑞𝐺
What if 𝑟𝑚𝑖𝑛 = 0?
shortest path straight line in ℝ2
In terms of a cost functional:
𝑡𝐹
ℒ(𝑞̃, 𝑢̃) = ∫ √𝑥̇ (𝑡)2 + 𝑦̇ (𝑡)2
0
𝑡𝐹 is time 𝑞𝐺 reached
configurations are 𝑞 = (𝑥, 𝑦, 𝜃).
If 𝑞𝐺 is not reached, ℒ(𝑞̃, 𝑢̃) = ∞
Since speed is constant, system simplifies:
𝑥̇ = cos(𝜃)
𝑦̇ = sin(𝜃)
𝜃̇ = 𝑢 tan 𝜙𝑚𝑎𝑥 𝑢 chosen from interval 𝑈 = [−1,1]
Page 5 of 6
Assignment
Three motion primitives: S,L,R. How many relevant combinations of these?
{{S, S, S}, {S, S, L}, {S, S, R}, {S, L, S}, {S, L, L}, {S, L, R}, {S,R, S}, {S, R, L}, {S, R, R},
{L, S, S}, {L, S, L}, {L, S, R}, {L, L, S}, {L, L, L}, {L, L, R}, {L, R, S}, {L, R, L}, {L, R, R},
{R,S,S}, {R, S, L}, {R, S, R}, {R, L, S}, {R, L, L}, {R, L, R}, {R, R, S}, {R, R, L}, {R, R, R}}
(never repeated letters next to each other, never start or end with straight, or you could go faster
without)
Label these. Draw the missing combination
Page 6 of 6
