SE(3) operations
N. Mansard
1
Rigid transformation
m : p ∈ E(3) → m(p) ∈ E(3)
Transformation from B to A:
A
mB :
B
p ∈ R3 ∼
= E(3) →
A
A
A
p=
mB (
B
A
p) =
MB
A
RB B p + A AB
A
RB A AB
A
MB =
0
1
p=
Transformation from A to B:
B
p=
A
T
RB
A
B
2
A
MA =
RA =
B
MA =
A
BA,
B
For Featherstone, E =
B
B
p+
A
B
MB =
T
BA = − A RB
T
T
RB
− A RB
0
1
A
T and r =
RB
A
RA −
0
B
with
T
RA
0
B
RA
1
A
A
AB
AB
A
AB
AB. Then:
E −Er
=
0
1
T
E
AB
=
0
1
r
1
Composition
A
A
MB
B
A
MC =
MB−1 A MC
=
A
RB B RC
0
A
T AR
RB
C
0
A
1
AB +
A
RB
B
BC
1
T ( A AC −
RB
1
A
AB)
B
p
3
Motion Application
A
B
A
A
B
4
B
RA =
B
RA −
0
B
XB =
A
RB
0
A
XA
A
A
ν
AB × A RB
A
RB
T
T A AB
RB
− A RB
×
A T
0
RB
T and r =
RB
B
A
T
RA
0
Aω
B
XA =
B
XA =
A
A
XB =
−1
XB
=
For Featherstone, E =
ν=
ν=
Av
A
AB. Then:
E −Er×
RA A AB ×
=
B
0
E
RA
T
T
E
AB × B RA
=
B T
0
RA
r× E T
ET
Force Application
A
B
A T A
φ
ν
For any φ, ν, φν̇ =
A
(because
A
∗
XB
A
=
=
A
∗
XA
A
φ=
φ=
B
B T B
φ
ν
=
A
φ
and then:
A
−T
XB
f
Aτ
RB
AB ×
A
RB
0
A
RB
T
AB × = − A AB × ).
−∗
XB
=
For Featherstone, E =
B
A
B
∗
XA
A
∗
XB
B
∗
XA
RA =
B
B
A
T
RB
=
A T A
− RB AB ×
A
T and r =
RB
RA
=
− B RA A AB ×
=
A
T
RA
AB ×
B
A
0
A T
RB
AB. Then:
0
E
0
=
B
−Er× E
RA
T
0
E
B T =
r× E T
RA
T
RA
2
0
ET
5
Inertia
5.1
Inertia application
A
Y :
A
A
ν→
A
φ=
Y
A
ν
Coordinate transform:
∗
XA
B
Y =
B
B
∗
XA
φ=
A
B
Y
−1
XA
since:
B
φ=
B
B
∗
XA
A
I
A
XB
B
ν
Cannonical form. The inertia about the center of mass c is:
m 0
c
Y =
0 CI
Expressed in any non-centered coordinate system A:
T
m
m A AC×
A
A ∗ C A −1
Y = XC I XC =
m A AC× A I + m A AC× A AC×T
Changing the coordinates system from B to A:
A
=
Y =
A
∗
XB
B
XC∗
m
m[ A AB +
A
RB
B
BC]×
A
RB
C
I
B
XC−1
A
−1
XB
m[ A AB + A RB B BC]T×
B A T
I RB − m[ A AB + A RB
Representing the spatial inertia in B by the triplet (m,
expression in A is:
A
mB :
B
Y = (m,
B
BC,
B
I) →
A
Y = (m,
A
B
BC,
AB+ A RB B BC,
A
B
BC]2×
B
I), the
T
RB B I A RB
)
Similarly, the inverse action is:
A
m−1
B :
A
Y →
B
Y = (m,
A
T A
RB
( AC −
A
AB),
A
T
RB
A
I
A
RB )
Motion-to-force map:
m
mcT×
v
mv − mc × ω
Yν =
=
mc× I + mc× cT× ω
mc × v + Iω − mc × (c × ω)
Nota: the square
 2

x
0
y  =  z
z ×
−y
of the cross product is:
2  2

−z y
−y − z 2
xy
xz

0 −x =  xy
−x2 − z 2
yz
2
2
x
0
xz
yz
−x − y
There is no computational interest in using it.
3
5.2
Inertia addition
mp
mp pT×
Yp =
mp p× Ip + mp p× pT×
T
mq
mq q×
Yq =
T
mq q× Iq + mq q× q×
6
Cross products
Motion-motion product:
v1
v2
v1 × ω2 + ω1 × v2
ν1 × ν2 =
×
=
ω1
ω2
ω1 × ω2
Motion-force product:
v
f
ω×f
ν×φ=
×
=
ω
τ
ω×τ +v×f
A special form of the motion-force product is often used:
mv − mc × ω
ν × (Y ν) = ν ×
mc × v + Iω − mc × (c × ω)
mω × v − ω × (mc × ω)
=
ω × (mc × v) + ω × (Iω) − ω × (c × (mc × ω)) − v × (mc × ω)
Setting β = mc × ω, this product can be written:
ω × (mv − β)
ν × (Y ν) =
ω × (c × (mv − β) + Iω) − v × β
This last form cost five ×, four + and one 3×3 matrix-vector multiplication.
7
Joint
We denote by 1 the coordinate system attached to the parent (predecessor)
body at the joint input, ad by 2 the coordinate system attached to the (child)
successor body at the joint output. We neglect the possible time variation
of the joint model (ie the bias velocity σ = ν(q, 0) is null).
The joint geometry is expressed by the rigid transformation from the
input to the ouput, parametrized by the joint coordinate system q ∈ Q:
2
m1 ∼
=
2
M1 (q)
The joint velocity (i.e. the velocity of the child wrt. the parent in the
child coordinate system) is:
2
ν12 = νJ (q, vq ) =
4
2
S(q)vq
where 2 S is the joint Jacobian (or constraint matrix) that define the motion
subspace allowed by the joint, and vq is the joint coordinate velocity (i.e. an
element of the Lie algebra associated with the joint coordinate manifold),
which would be vq = q̇ when q̇ exists.
The joint acceleration is:
2
α12 = S v̇q + cJ + 2 ν1 × 2 ν12
Pnq ∂S
2
where cJ = i=1
∂qi q̇i (null in the usual cases) and ν1 is the velocity of the
parent body with respect to an absolute (Galilean) coordinate system1 .
The joint calculations take as input the joint position q and velocity vq
and should output 2 M1 , 2 ν12 and 2 c (this last vector being often a trivial 06
vector). In addition, the joint model should store the position of the joint
input in the central coordinate system of the previous joint 0 m1 which is a
constant value.
The joint integrator computes the exponential map associated with the
joint manifold. The function inputs are the initial position q0 , the velocity
vq and the length of the integration interval t. It computes qt as:
Z
qt = q0 +
t
vq dt
0
For the simple vectorial case where vq = q̇, we have qt = q0 + tvq . Written
in the more general case of a Lie groupe, we have qt = q0 exp(tvq ) where exp
denotes the exponential map (i.e. integration of a constant vector field from
the Lie algebra into the Lie group). This integration only consider first order
explicit Euler. More general integrators (e.g. Runge-Kutta in Lie groupes)
remains to be written. Adequate references are welcome.
8
RNEA
8.1
Initialization
0
ν0 = 0; 0 α0 = −g
In the following, the coordinate system i is attached to the output of the
joint (child body), while lambda(i) is the central coordinate system attached
to the parent joint. The coordinated system associated with the joint input
is denoted by i0 . The constant rigid transformation from λ(i) to the joint
input is then λ(i) Mi0 .
1
The abosulte velocity ν1 is also the relative velocity wrt. the Galilean coordinate
system Ω. The exhaustive notation should be νΩ1 but ν1 is prefered for simplicity.
5
8.2
Forward loop
For each joint i, update the joint calculation ji .calc(q, vq ). This compute
Pnq ∂ i S
j.M = λ(i) Mi0 (q), j.ν = i νλ(i)i (q, vq ), j.S = i S(q) and j.c = k=1
∂qk q̇k .
Attached to the joint is also its placement in body λ(i) denoted by j.M0 =
λ(i) M . Then:
i0
λ(i)
0
i
i
αi =
i
8.3
λ(i)
Mi =
λ(i)
νi =
Xi−1
Mi = j.M0 j.M
λ(i)
0
Mλ(i)
Xi−1
λ(i)
λ(i)
Mi
νλ(i) + j.ν
αλ(i) + j.S v̇q + j.c + i νi × j.ν
φi = i Yi i αi + i νi × i Yi i νi − 0 Xi−∗ 0 φext
i
Backward loop
For each joint i from leaf to root, do:
τi = j.S T i φi
λ(i)
8.4
φλ(i) +=
λ(i)
Xi∗ i φi
Nota
It is more efficient to apply X −1 than X. Similarly, it is more efficient to
apply X −∗ than X ∗ . Therefore, it is better to store the transformations
λ(i) m and 0 m than i m
i
i
i
λ(i) and m0 .
6