0 −ξ1 −ξ2 −ξ3

"ξ # "ξ #
0
ξ1 ξ0 −ξ3 ξ2 ξ1 2.6 Multi-task IDK
ξAB ⊗ ξBC = ξ2 ξ3 ξ0 −ξ1 ξ2
  ξ3 −ξ2
 ξ1  ξ0 AB ξ3 BC Given nt tasks {Ji , w∗i }, we have:
0 0 −1
Robot Dynamics HS18 Ar
= ξAB ⊗ Br
⊗ ξAB  J +  w∗ 
1 1

q̇ =  ..   .. 
 
. .∗
Sean Bone 1.3 Angular Velocity Jnt wn
t
http://weblog.zumguy.com/  0 −ωz ωy

In case the row-rank of the stacked Jacobian is
[ A ωAB ]× = ωz 0 −ωx = ĊAB CT
AB
−ωy ωx 0 greater that the column-rank, we are only mini-
January 30, 2019 mizing ||w̄ − J¯q̇||2 . We can weigh the tasks with
A ω AB = ER (χR ) χ̇R
¯ −1 JW
J¯+W = (J¯T W J) ¯
1.4 Transformations
h
CAB A r AB
i where W = diag(w1 , ..., wm ) and we minimize
A r AP B r BP



1 = 01×3 1 1 ||W 1/2 (w̄ − J¯q̇)||2 .
0
(sin(x)) = − cos(x) | {z }
(cos(x))0 = sin(x) TAB
 r BA
 Task Prioritization
z B }|
T−1
{
= CT −CT
AB A r AB

1 Parametrizations AB AB q̇ = J1+ w∗1 + N1 q̇0
01×3 1
w2 = J2 q̇ = J2 (J1+ w∗1 + N1 q̇0 )
1.1 Position and velocity
2 Kinematics ⇒ q˙0 = (J2 N1 )+ (w∗2 − J2 J1+ w∗1 )
For every position parametrization, there is a
linear mapping between linear velocities ṙ and ⇒ q̇ = J1+ w∗1 + N1 (J2 N1 )+ (w∗2 − J2 J1+ w∗1 )
2.1 Velocity in rigid bodies
derivatives of the representation χ̇.
In general:
ṙ = EP (χP ) χ̇P , χ̇P = EP (χP )−1 ṙ • v P : abs. velocity of P
• aP : abs. acceleration of P nt
X
Cartesian Coordinates: EPc = I • ΩB = I ω B : angular vel. of frame B q̇ = N̄i q̇i
χPc = [x y z]T , A r = [x y z]T • ΨB = Ω̇B : angular accel. of frame B i=1
Cylindrical coordinates: i−1
!
X
χPz = [ρ θ z]T , q̇i = (Ji N̄i ) +
w∗i − Ji N̄k q̇k
T A v AP = A (ṙAP ) = A v AB + A ω AB × A r BP
A r = [ρcos θ ρ sin θ z]
k=1
cos θ −ρ sin θ 0 In general, unless C is an inertial frame:
EPz = sin θ −ρ cos θ 0 d 2.7 Inverse Kinematics
 0 0 1  C v AP = C (ṙAP ) 6= dt (C r AP )
−1 cos θ sin θ 0 In r igid body formulation: 1. q ← q0
EP = − sin θ/ρ cos θ/ρ 0
z
0 0 1 v P = v B + Ω × r BP 2. While ||χ∗e χe (q)|| > tol do
Spherical coordinates: aP = aB + Ψ × r BP + Ω × (Ω × r BP ) 3. JA ← JA (q) = ∂χ∂q
e
(q)
χPs = [r θ φ]T , 4. +
JA ← (JA )+
Ar = [r cos θ sin φ r sin θ cos φ z]T In a kinematic chain: 5. ∆χe ← χ∗e χe (q)
cos θ sin φ −r sin φ sin θ r cos φ cos θ
 
I v IE = I ω I1 × I r 12 + ... + I ω In × I r nE
+
sin φ sin θ r cos θ sin φ r cos φ sin θ 6. q ← q + JA ∆χe
−r sin φ
 cos φ
cos θ sin φ
0
sin φ sin θ cos φ
 I ω IE = I ω I1 + I ω 12 + ... + I ω nE
One issue is that for very large errors ∆χe , we
− sin θ/(r sin φ) cos θ/(r sin φ) 0 get too imprecise. We can avoid this by scal-
(cos φ cos θ)/r (cos φ sin θ)/r − sin φ/r
2.2 Forward kinematics ing the update with a factor 0 < k < 1: q ←
+
nj ! q + kJA ∆χe . But we still have issues inverting
1.2 Rotation Y JA in singular configurations. An alternative is
TIE (q) = TI0 Tk−1,k (qk ) Tnj E T ∆χ , which converges for small α.
y
atan2(y, x) := atan( x ), checking for correct quad- q ← q + αJA e
k=1
rant. We must also appropriately compute the differ-
ence χ∗e χe (q) depending on the parametriza-
A u = CAC · C u = CAB CBC · C u 2.3 Analytical Jacobian tion. For cartiesian coordinates, this is regular
CBA = C−1 AB = CAB
T
" ∂χ
pos
# vector subtraction. Also note that with cartesian
Elementary rotations: ∂χ

1 0 0
 χ̇(q) = q̇ = JA (q) · q̇ = ∂q q̇ coordinates J0,P = JA,P . For rotational differ-
∂χrot
Cx = 0 cos ϕ − sin ϕ ∂q ∂q ence we can extract the rotation vector ∆ϕ from
0 sin ϕ cos ϕ
  the ”rotation difference”, and use that for the up-
cos ϕ 0 sin ϕ
Cy = 0 1 0 date:
2.4 Geometric Jacobian
 − sin ϕ 0 cos ϕ  CGS (∆ϕ) = CGI (ϕ∗ )CSI (ϕt )
cos ϕ − sin ϕ 0  
Cz = sin ϕ cos ϕ 0 vE
wE = = J0 (q)q̇ +
0 0 1 ωE q ← q + kpR J0,R ∆ϕ
Euler ZYZ (proper) angles:
atan2(c23 ,c13 )
!    
J n1 × r1,E ... nn × rn,E 2.8 Trajectory control
J0 (q) = 0,P =
q
χR,ZY Z = atan2( c2 2
13 +c23 ,c33 ) J0,R n1 ... nn
atan2(c32 ,−c31 ) Position: with ∆rte = r∗e (t) − re (qt )
Euler ZXZ (proper) angles:
atan2(c13 ,−c23 )
q
! 2.5 Inverse differential kinematics q̇∗ = Je0P
+
(qt )(ṙ∗e (t) + kpP ∆rte )
χR,ZXZ = atan2( c2 2
13 +c23 ,c33 )
atan2(c31 ,c32 )
wE = J q̇ ⇒ q̇ = J + wE Orientation: with ∆ϕ as above,
Euler ZYX (Tait-Bryan) angles: where J+
= J T (JJ T )−1
(Moore-Penrose). How- q̇∗ = Je0R
+
(qt )(ωe∗ (t) + kpR ∆ϕ)
atan2(c21 ,c11 )
!
q ever we risk encountering singular configurations
χR,ZY X = atan2(−c31 , c2 2
32 +c33 )
qs where rank(J(qs )) < m0 , m0 being the num- 3 Dynamics
atan2(c32 ,c33 )
Euler XYZ (Cardan) angles: ber of operational-space coordinates. Here J is
atan2(−c23 ,c33 )
! badly conditioned. We can mitigate this by using
q
a redundant robot to carefully avoid singularities, M(q)q̈ + b(q, q̇) + g(q) = τ + Jc (q)T Fc
χR,XY Z = atan2(c13 , c2 2
11 +c12 )
and/or by damping the pseudo-inverse:
atan2(c12 ,−c11 ) • M(q) ∈ Rnq ×nq Mass matrix (⊥).
Angle-axis:
  T
q̇ = J (JJ T
+ λ I)2 −1
wE • q, q̇, q̈ ∈ Rnq Gen. pos., vel., accel.
1 c32 −c23
b(q, q̇) ∈ Rnq Coriolis and centrifugal terms
 
χR,AA = n θ
, n= · c31 −c13 , •
2 sin(θ) c21 −c12
Now the pseudo-inverse minimizes ||w∗E − J q̇||2 + • g(q) ∈ Rnq Gravity terms
c11 + c22 + c33 − 1 λ2 ||q̇||2 instead of just ||w∗E − J q̇||2 , so conver- • τ ∈ Rnq External generalized forces
θ = acos( ), ϕ = θ · n • Fc ∈ R3×nc External cartesian forces
2 gence is slower but more stable for larger λ.
• Jc (q) ∈ Rnc ×nq Geometric Jacobian of loca-
In a redundant configuration q ∗ where
Unit Quaternions: tion where external forces apply
ξ  ξ  rank(J(q∗ )) < n, the pseudoinverse minimizes
χR,quat = ξ = ξ̌ , ξ−1 = − ξ̌ ||q̇||2 while satisfying w∗E = J q̇ by using    
vs J
ξ0 = cos(θ/2), ξ̌ =√n · sin(θ/2) = P q̇
Ω JR
J(J + w∗E + N q̇0 ) = w∗E ∀q̇0
 
c11 +c22 +c33 +1
√
1  sgn(c32 −c23 )√c11 −c22 −c33 +1  J˙
       
χR,quat = as v̇s JP
2 sgn(c13 −c31 ) c22 −c11 −c33 +1
√ where N = I − J + J. = = q̈ + ˙P q̇
sgn(c21 −c12 ) c33 −c11 −c22 +1 Ψ Ω̇ JR JR
Newton-Euler method • Fext ∈ R3×nc External cartesian forces acting Inverse dynamics control
on robot
• m body mass Compensate for system dynamics:
• Jext (q) ∈ Rnc ×nu Geometric Jacobian of loca-
• ΘS inertia matrix around CoG
tion where external forces apply τ = M̂(q)q̈∗ + b̂(q, q̇) + ĝ(q)
• pS = mvS linear momentum
• NS = ΘS · Ω angular momentum around CoG Position and velocity of a point Q on the robot: If the model is exact, we have Iq̈ = q̈∗ , mean-
• ṗ = maS change in linear momentum ing we can perfectly control system dynamics. We
• ṄS = ΘS · Ψ + Ω × ΘS · Ω change in angular I r IQ (q) = I r IB (q) + CIB (q) · B r BQ (q) could apply a PD-control law, making each joint
momentum behave like a mass-spring-damper with unitary
 
I3×3 −CIB ·[B r BQ ]× CIB ·B J Pq (qj )·u
I vQ = j
| {z } mass:
=I J Q (q)
Cut each link free as a single rigid body, and in- q̈∗ = kp (q∗ − q) + kd (q̇∗ − q̇)
troduce constraint forces Fi acting on the body
at the joint. Then apply conservation of linear Contact kinematics p
ω = kp , D = p
kd
and angular momentum in all DoFs subject to all The point of contant C is not allowed to move: 2 kp
external forces (including contraints Fi ): rC = const. and ṙC = r̈C = 0. Written in gener-
alized coordinates these are: Operational space control
ṗS = Fext,S
˙ Generalized framework to control motion and
ṄS = Text I J Ci u = 0, I J Ci u̇ + I J Ci u = 0
force. End-effector dynamics:
We can therefore stack the constraint Jacobians:
For calculations all quantities must be in the Λẇe + µ + p = Fe
same coordinate system. For the inertia matrix I J C1
 
we have B Θ = CBA · A Θ · CT  .  Λ = (Je M−1 JT
e)
−1
BA . Jc =  ..  ∈ R3nc ×(nb +nj )
µ = ΛJe M−1 b − ΛJ̇e q̇
Lagrange method I J Cnc
p = ΛJe M−1 g
Define the Lagrangian function: By using the nullspace projection Nc of Jc we
can still move the system: τ = JT
e Fe

L := T − U ⇒ τ ∗ = ĴT (Λ̂ẇe∗ + µ̂ + p̂)

0 = ṙ = Jc q̇ ⇒ q̇ = J+
c 0 + Nc q̇0
Where T is the kinetic energy and U the potential Hence we can steer the robot along any trajec-
0 = r̈ = Jc q̈ + J̇c q̇ ⇒ q̈ = J+
c (−J̇c q̇) + Nc q̈0
energy. Then the Euler-Lagrange equation of the tory by determining the desired task-space end-
second kind holds for the total external general- The contact Jacobian tells us how the system can effector acceleration:
ized forces τ : move. If we partition it into the part relating to ẇe∗ = kp E(χ∗e χe )+kd (we∗ −we ) +ẇ∗ (t)
    the base and the part relating to the joints: | {ze }
d ∂L ∂L trajectory control
− =τ • Jc = [Jc,b Jc,j ]
dt ∂ q̇ ∂q In order to also control the contact force, we
• rank(Jc,b ) is the number of constraints on the
use selection matrices:
The kinetic energy for a system of nb bodies is base → the number of controllable base DoFs.
defined as: • rank(Jc ) − rank(Jc,b ) is the number of con- Fc + Λẇe + µ + p = Fe
traints on the actuators.
Xnb ∗
τ = Ĵ (Λ̂SM ẇe∗ + SF Fc + µ̂ + p̂)
T
T T
1m 1


T := 2 i A ṙ Si A ṙ Si + 2 B Ω̇Si ·B ΘSi ·B ΩSi A typical quadruped has 18 DoF (6 for base, 12
Let C represent the rotation from the inertial
i=1 actuators). Each foot in contact with the ground
frame to the contact force frame. The selection
nb ! adds 3 total constraints. One foot on the ground
1 T X matrices can be calculated as:
= q̇ (JSTi mJSi + JR
T
i
ΘSi JRi ) q̇ allows us to control 3 base DoFs, two feet 5, and
2 i=1 three or more allow us to control all base DoFs.
σ
px 0 0
 σ
rx 0 0

| {z } Σp = 0 σpy 0 , Σr = 0 σry 0
M(q) 0 0 σpz 0 0 σrz
Support-consistent dynamics
 
1 CT Σp C 0
= q̇T M(q)q̇ SM =
0 CT Σr C
2 If we use soft contacts to model the contact, we  
The potential energy is typically in the form simply introduce an external force acting on the T
C (I−Σp )C 0
SF =
robot: 0 CT (I−Σr )C
of gravitational and elastic terms:
Fc = kp (rc − rc0 ) + kd ṙc
nb nE
X X 1 However such problems are hard to accurately Floating-base inverse dynamics
U =− rT
Si (mi g · eg ) + kj (d(q) − d0,j )2
2 solve numerically (slow system dynamics, fast
i=1 j=1 From the support-consistent dynamics:
| {z } contact dynamics).
τ ∗ = (NT ∗
| {z } T + T T T
c S ) Nc (Mu̇ + b + g) +N (Nc S )τ0
gravitational elastic Instead it works better to use hard contacts.
We impose the kinematic constraint I J Ci u̇ +
| {z }
Here we have nE elastic components with coeffi- multiple solutions
˙
cients kj and rest configuration d0,j . I J Ci u = 0 from above and calculate the resulting
force and null-space matrix: OSC with multiple objectives
Projected Newton-Euler Method Fc = (Jc M−1 JT
c )
−1
(Jc M−1 (ST τ − b − g) + J̇c u) Example: quadruped with three stationary legs
Nc = I − M−1 JT (Jc M−1 JT )−1 Jc and one in swing.
c c
nb
X • Leg swing: r̈OF = JF q̈F + J̇F q̇F =
M= (A J T
Si m A J Si + T
B J Ri B ΘSi B J Ri ) ⇒ NT
c (Mu̇ + b + g) = NT T
c S τ, Jc Nc = 0 r̈OF,des (t) = kp (qr∗ − r) + kd (ṙ∗ − ṙ) + r̈∗
i=1
• Body movement (translation and orientation):
nb By defining the end-effector inertia Λc =
X ẇB = JBq̈B + J̇B q̇B = ẇOB,des (t) =
b= (A J T ˙ T ˙
Si m A J Si q̇ + B J Ri (B ΘSi B J Ri q̇ (Jc M−1 T −1 we can write the kinetic energy
c Jc ) r∗ − r
i=1 loss on impact: kp + kd (w∗ − w) + ẇ∗
ϕ∗ ϕ
+ B ΩSi × B ΘSi B ΩSi )) u+ = Nc u− • Enforce contact constraints: r̈c = Jc q̈c +
nb J̇c q̇c = 0
X 1 1
g= (− A J T
Si A F g,i ) Eloss = ∆Ekin = − ∆uT M∆u = − ṙ−T Mṙ− Solve for generalized acceleration and torque giv-
i=1
2 2
ing each task equal priority:

4 Floating-base dynamics 5 Dynamic control   + 

JF r̈OF,des (t)
   
J̇F
∗
q̈ = JB
    ẇB,des (t)  − J̇B  q̇
Generalized coordinates are now q = [qT T T Joint impedance control Jc 0 J̇c
b qj ] ,
where qb are the generalized coordinates of the Solve with prioritization:
M(q)q̈ + b(q, q̇) + g(q) = τ
base (position and orientation). The generalized
nt
velocities are therefore no longer q̇, but are de- Torque as a function of position and velocity X
noted u = [I v T T T T q̈∗ = Ni q̈i ,
B B ω IB q̇j ] . error:
i=1
τ ∗ = kp (q∗ − q) + kd (q̇∗ − q̇) i−1
!
X
M(q)u̇ + b(q, u) + g(q) = S τ + T
JT q̈i :=(Jj Ni )+ wi∗ − J̇i q̇ − J Nk q̇k
ext Fext Compensate for gravity by adding an esti- k=1
mated gravity term:
• u, u̇ ∈ Rnu Gen. vel., accel. Where Ni is the nullspace projection of Ji :=
• S selection matrix of actuated joints τ ∗ = kp (q∗ − q) + kd (q̇∗ − q̇) + ĝ(q) [JT T T
1 ... Ji ] .
Quadratic minimization The quadrotor automatically has full control
over all rotational speeds, independently of the
Least squares problems can be expressed in form
current position state. On the other hand, it can
of quadratic minimization problems. We can also
only directly control vertical cartesian velocity -
perform multiple tasks with or without prioritiza-
attitude control must be used for full position con-
tion:
trol.
→ Ax − b = 0 ⇒ x = A+ b
⇔ min ||Ax − b||2 ; min ||x||2 Propeller aerodynamics 7 Fixed wing aerodynamics
x x
 
x1  + Propeller in hover:
→ A1 x2 − b = A2 x2 ⇒ = A1 A2 b For aircraft rotation, use Tait-Bryan angles (like
x2
  • Thrust
ρ
force T normal to prop. plane, |T | = for copters).
AP CT (ωp Rp )2
 
  x1 x1 2
⇔ min A1 A2 − b ; min
x1 ,x2 x2 2
x1 ,x2 x2 2 • Drag torque Q, around rotor plane |Q| =
ρ
    A C (ωp Rp )2 Rp
2 P Q
A 1 x = b1 A1 b1 • CT and CQ depend on blade pitch angle (prop
→ Equal priority ⇒ x =
A 2 x = b2 A2 b2 geometry), Reynolds number (prop speed, ve-
   
A1 b1 locity, rotational speed).
⇔ min x− ; min ||x||2
x A2 b2 2 x
 Propeller in forward flight: additional forces
A1 x = b1
→ Hierarchy ⇒ (nullspace projections)due to force unbalance between forward- and
A2 x = b2 backward-moving props.

minx kA2 x − b2 k2 • Hub force H (orthogonal to T , opposite
⇔ min kA1 x − b1 k2 ;
x s.t. kA1 x − b1 k2 = c1 to horizontal flight direction VH ), |H| =
ρ
A C (ωp Rp )2 Rp
2 P H
OSC as quadratic program • Rolling torque R around flight direction |R| =
ρ
A C (ωp Rp )2 Rp
2 P R
Rewrite the equations of motion and subsequent
• CR and CH depend on advance ratio µ =
tasks as a prioritized sequence of quadratic mini- V
mization problems: ωP RP

Ideal power consumption at hover: P =

 
u̇ 3/2
min ||Ai x − bi ||2 x = Fc  FT hrust (mg)3/2
x √ = √ . The prop efficiency is mea-
τ 2ρA R 2ρAR
sured with the Figure of Merit FM:

6 Rotorcraft FM =
Ideal power to hover
<1
Actual power to hover
Propeller thrust and drag proportional to squared
rotational speed (b: thrust constant; d: drag con- Blade Elemental and Momentum Theory
stant): (BEMT): blade shape determines drag and lift co-
2 2
Ti = bωp,i , Qi = dωp,i efficients cD , cL .

Representation of rotation
Use Tait-Bryan angles, consisting of yaw ψ (Z-
axis), pitch θ (Y-axis) and roll φ (X-axis).
CEB = CE1 (z, ψ) · C12 (y, θ) · C2B (x, φ)
Angular velocity:

Bω = B ω roll + B ω pitch + B ω yaw

B ω roll = (ψ̇, 0, 0)T

B ω pitch = CT
2B (0, θ̇, 0)
T

B ω yaw = [C12 · C2E ]T (0, 0, φ̇)T

 
φ̇
B ω = Jr χ̇r = Jr
 θ̇ 
ψ̇
 
1 0 − sin θ
Jr =  0 cos φ sin φ cos θ
0 − sin φ sin φ cos θ
NB: singularity for θ = ±90◦ (gimbal lock).

Body Dynamics
Change of momentum and spin in the body frame
(M = total moment/torque):
      
mE 0 B v̇ ω × m Bv F
+ B = B
0 I B ω̇ Bω × I Bω BM

Forces and moments come from gravity and

aerodynamics:

BF = B F G + B F Aero
BM = B M Aero
 
0
BF G = CTEB
 0 
mg
 
X4 0
B F Aero =  0 
2
i=1 −Ti = −bωp,i
   
l(T4 − T2 ) 0
B M Aero = B M T + B Q = l(T1 − T3 ) + P 0 
4 (i−1)
0 i=1 Qi (−1)