TUM Robotics Cheat Sheet Winter 2012/13

1 D–H / Kinematics
2 3 4
R1 R2 R3  
cos θi − sin θi 0 ai−1
i−1
sin θ cos α
i i−1 cos θi cos αi−1 − sin αi−1 − sin αi−1 di 
Ti = 
 
1 2 3 4 
 sin θi sin αi−1 cos θi sin αi−1 cos αi−1 cos αi−1 di 
1 2 3
0 0 0 1
R2 R3 R4

h i
B B B B
D–H: RA = X̂ A Ŷ A Ẑ A
ai−1 : Distance between zi−1 and zi along xi−1
αi−1 : Angle between zi−1 and zi about xi−1  
A
di : Distance between xi−1 and xi along zi v
  = A J · θ̇
θi : Angle between xi−1 and xi about zi A
ω
ṡi = ci θ̇i
The rotational part of the Jacobi matrix:
ċi = −si θ̇i
sk±l = sk cl ± ck sl 
.. .. ..

ck±l = ck cl ∓ sk sl A . . .
J = .
.. A R · j Ẑ ...
j j

2 Newton–Euler

Outward iteration
i+1
ω i+1 = i+1 Ri · i ω i + θ̇i+1 i+1 Ẑ i+1

i+1

 
ω̇ i+1 = i+1 Ri · i ω̇ i + i+1
Ri · i ω i × θ̇i+1 i+1 Ẑ i+1 + θ̈i+1 i+1 Ẑ i+1

i+1
v i+1 = i+1 Ri · i
v i + i ω i × i P i+1 + d˙i+1 i+1 Ẑ i+1

i+1
v̇ i+1 = i+1 Ri · i
v̇ i + i ω̇ i × i P i+1 + i ω i × i
+ 2i+1 ω i+1 × d˙i+1 i+1 Ẑ i+1 + d¨i+1 i+1 Ẑ i+1
ω i × i P i+1



 
i+1
v̇ Ci+1 = i+1 ω̇ i+1 × i+1 P Ci+1 + i+1 ω i+1 × i+1 ω i+1 × i+1 P Ci+1 + i+1 v̇ i+1

i+1
F i+1 = mi+1 i+1 v̇ Ci+1
C
C 
i+1 i+1
N i+1 = J i+1 · i+1 ω̇ i+1 + i+1 ω i+1 × i+1
J i+1 · i+1 ω i+1

Inward iteration
i
f i = i Ri+1 · i+1 f i+1 + i F i
 
i
ni = i Ri+1 · i+1 ni+1 + i P i+1 × i
Ri+1 · i+1 f i+1 + i N i + i P Ci × i F i

T T
τi = i ni · i Ẑ i | τi = i f i · i Ẑ i

1
TUM Robotics Cheat Sheet Winter 2012/13

3 Dynamics

Lagrangian dynamics
Miscellaneous
 T  T  T
  d ∂k ∂k ∂u
A
f − + =τ
τ̇ = A J T ·   dt ∂ q̇ ∂q ∂q
A
n X1 X1
T T C
k= mi v Ci · i v Ci + j
ωj · j
J j · j ωj
State space equation: i
2 j
2
X T
τ = M (q) · q̈ + V (q, q̇) · q̇ + G(q) v=− mi g C · i P Ci
i
i

n(n−1)
Configuration space equation (the compound velocities vector has a dimension of 2 × 1):
   
q̇1 q̇2 q̇12
 .  .
 ..  + C (q) ·  ..  + G(q)
τ = M (q) · q̈ + B(q) ·    
q̇n−1 q̇n q̇n2

4 Control
p √
ODE mẍ + bẋ + kx = 0: ωn = k/m; ζ = √b ; therefore is critically damped for b = 2 mk (ζ = 1).
2 mk

Simple control law: RHS f = −kp x − kv ẋ ⇒ closed-loop stiffness k 0 = k + kp and damping b0 = b + kv .

Control law partitioning: RHS f = αf 0 + β (model-based portion) with α = m, β = bẋ + kx ⇒ new ODE ẍ = f 0 .
Servo portion: f 0 = −kv ẋ − kp x, choose kv = 2 kp for critical damping.
p

Trajectory following control: Partitioning + servo control law f 0 = ẍd + kv (ẋd − ẋ) + kp (xd − x). Error (e = xd − x)
p
equation: ë + kv ė + kp e = 0, critically damped for kv = 2 kp .
R
Disturbance rejection: Add an integral term ki e dt to the control law.
Force control of a mass–spring: ODE mẍ + fe = −fdist + f . fdist is the (unknown) disturbance, f is the control
force, we want to control fe = ke x (spring force). Transform the ODE to the terms of fe : mke −1 f¨e + fe + fdist = f .
−1 0 ¨
Now apply the control law partitioning with
 α = mke , β = fe + fdist and the servo part f = fd + kvf ėf + kpf ef . This
−1 ¨
results in overall control law f = mke fd + kvf ėf + kpf ef + fe + fdist and error ODE ëf + kvf ėf + kpf ef = 0.
 
Problem: The disturbance fdist is unknown. Solution: Modify the control law to be f = mke −1 f¨d + kvf ėf + kpf ef +
fdist fdist
fd . Now the steady state error is ef = 1+α instead of ef = α .

Active stiffness control: According to Salisbury with servo error E = Θd − Θ

 
τ = J T · K px · J · E + K v · Ė

5 Tables

α 0◦ 30◦ 45◦ 60◦ 90◦ 180◦

√ √
1 2 3
sin α 0 1 0
√2 √2 2
3 2 1
cos α 1 0 −1
√2 2 √2
3
tan α 0 3 1 3 ∞ 0
sin(90◦ + α) = cos α
cos(90◦ + α) = − sin α