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