currently known. In order to perform gradient-based optimization, such as with ..... [2] Gill, P. E., Murray, W., and Wright, M. H. Practical Optimization. Academic ...
Derivations of the Balafoutis & Patel Recursive Dynamics Formulation Brian Guenter Charles F. Rose, III July 2000 Technical Report MSR-TR-2000-80
Microsoft Research Microsoft Corporation One Microsoft Way Redmond, WA 98052 http://www.research.microsoft.com
In the Siggraph 1996 paper, Efficient Generation of Motion Transitions Using Spacetime Constraints [3], we used the Balafoutis and Patel dynamics formulation [1] together with the spacetime optimization animation technique [4] to create high-quality transitions for a full-body human-figure model. Balafoutis and Patel created the most efficient linear-recursive inverse dynamics method currently known. In order to perform gradient-based optimization, such as with BFGS [2], we derived the partials of their equations with respect to joint angle position, velocity, and acceleration. This technical report details the complete dynamics formulation used in [3], including all the partial equations which could not appear in Siggraph due to space limitations. This tech-report is intended as a companion to that paper.
1
Equations of dynamics & their derivatives
Constants, symbols, and notation: oi ci ωii zii
= = = =
sii,j rii,j Ai
= = =
Ic ki
=
Jc ki
=
Ωii
=
Fc ii Mc ii fii ηii τi g mi
= = = = = = =
origin of the i-th link coordinate frame. center of mass of the i-th link. angular velocity of the i-th link . joint axis of the i-thlink expressed in the i-th coordinate frame. vector from oi to oj expressed in the i-th coordinate frame. vector from oi to cj expressed in the i-th coordinate frame. 3x3 coordinate (or 4x4 homogeneous) transformation relating the i-th coordinate frame to the (i − 1)-th frame. inertia tensor of the i-th link about ci expressed in the k-th coordinate frame. Euler’s inertia tensor of the i-th frame about ci expressed in the k-th coordinate frame. angular acceleration tensor of the i-th link expressed in the i-th coordinate frame. force vector acting on ci expressed in the i-th coordinate frame. moment vector about ci expressed in the i-th coordinate frame. force vector exerted on link i by link (i − 1). moment vector exerted on link i by link (i − 1). torque at joint i. gravity. mass of the i-th link.
In the above, the subscript indicates the coordinate frame being represented and superscript the coordinate frame in which it is represented.
1
We use + and - on index variables to denote relative placement in the joint hierarchy. Thus, i1 is the predecessor of i which is the predecessor of i+. For i+ i example, in the equation ωi+ = ATi+ ωii + zi+ i+ q˙i+ , the variable ωi is the angular i+ . In velocity in the coordinate frame which precedes the coordinate frame of ωi+ other words, coordinate frame i is closer to the root coordinate frame than is frame i+. Note that there is no guarantee of a uniquely defined successor.
g Jc ii
T
=
[0.0, −9.80655, 0.0] 1 trace Ic ii 1 − Ic ii = 2
0 ˜ = v3 dual(v) = v −v2
−v3 0 v1
v2 −v1 0
dual(˜ v) = v
Inverse-dynamics equations: Base conditions at the root of the creature:
ω00
ω˙ 00 ¨s00,0
= z00 q˙0 = z00 q¨0
= AT0 g
Recursive dynamics equations:
i+ ωi+
= ATi+ ωii + zi+ i+ q˙i+
i+ ω˙ i+
i+ ¨i+ ˜ii+ zi+ = ATi+ ω˙ ii + ω i+ q˙i+ + zi+ q
Ωi+ i+ ¨si+ 0,i+
i+ i+ i+ ω ˜i+ ˜i+ = ω ˜˙ i+ + ω i T = Ai+ ¨s0,i + Ωii sii,i+
¨ri+ 0,i+
i+ si+ = Ωi+ 0,i+ i+ ri+,i+ + ¨
Fc i+ i+
= mi+ ¨ri+ 0,i+ i+ i+ i+ i+ T = Ωi+ Jc i+ − Ωi+ Jc i+
˜ c i+ M i+
2
Backward recursive equations (torque equations): At a joint controlling an end-effector:
fii ηIi
= Fc ii = ˜rii,i Fc ii + Mc ii
τi
= ηii · zii
At an internal joint: i+ Ai+ fi+
fii
= Fc ii +
ηii
=
˜rii,i Fc ii
=
ηii
i+
+ Mc ii +
i+ i + ˜sii,i+ fi+ Ai+ ηi+ i+
τi
·
zii
The energy function & the partials:
P δP δθj
= =
t
τi2 dt
i
δτi δτi δτi + + 2 δ q¨j δq δ q ˙ j j t i
The recursive partials & their initial conditions:
i+ δωi+ δqj
i+>j
i+ δωi+ δqj
i+=j
δωii δqj
=
ATi+
=
δATj j− ω δqj j−
3
i+ δ ω˙ i+ δqj
i+>j
i+ δ ω˙ i+ δqj
i+=j
δΩi+ i+ δqj
= =
=
i+ δωi+ δ q˙j
i+>j
i+ δωi+ δ q˙j
i+=j
i+ δ ω˙ ii δω + i zi+ q˙i+ δqj i+ δqj δATj j− j δATj j− z q˙j ω˙ j− + ω˙ δqj δqj j− j
ATi+
T i+ i+ i+ δ ω˙ i+ δω δω ˜ i+ + i+ ω ˜ i+ + i+ ω δqj i+ δqj i+ δqj
δωii δ q˙j
=
ATi+
=
zjj
=
δ ω˙ i ATi+ i δ q˙j
=
j− j zj ATj ωj−
i+ δ ω˙ i+ δ q˙j
i+>j
i+ δ ω˙ i+ δ q˙j
i+=j
+
δω i ATi+ i δ q˙j
zi+ i+ q˙i+
T i+ i+ i+ δω δω δ ω˙ i+ ˜ i+ ˜ i+ + i+ ω + i+ ω δ q˙j i+ δ q˙j i+ δ q˙j
δΩi+ i+ δ q˙j
=
i+ δωi+ δ q¨j
=
0
i+ δ ω˙ i+ δ q¨j
i+>j
=
ATi+
i+ δ ω˙ i+ δ q¨j
i+=j
=
zjj
δΩi+ i+ δ q¨j
=
δ ω˙ ii δ q¨j
i+ δ ω˙ i+ δ q¨j
4
δ¨si+ 0,i+ δqj δ¨si+ 0,i+ δqj δ¨si+ 0,i+ δ q˙j δ¨si+ 0,i+ δ q˙j δ¨si+ 0,i+ δ q¨j δ¨si+ 0,i+ δ q¨j
=
i+>j
=
i+=j
=
i+>j
=
i+=j
=
i+>j
=
i+=j
ATi+
δ¨si0,i δΩii i + s δqj δqj i,i+
δATj j− j− ¨s0,j− + Ωj− j− sj−,j δqj ATi+
δ¨si0,i δΩii i + s δ q˙j δ q˙j i,i+
0 ATi+
δ¨si0,i δΩii i + s δ q¨j δ q¨j i,i+
0
δ¨ri+ 0,i+ δqj
=
δ¨si+ δΩi+ 0,i+ i+ i+ ri,i+ + δqj δqj
δ¨ri+ 0,i+ δ q˙j
=
δ¨si+ δΩi+ 0,i+ i+ i+ ri,i+ + δ q˙j δ q˙j
δ¨ri+ 0,i+ δ q¨j
=
δ¨si+ δΩi+ 0,i+ i+ i+ ri,i+ + δ q¨j δ q¨j
=
δΩi+ i+ Jc i+ i+ − δqj
δMc i+ i+ δqj
5
δΩi+ i+ Jc i+ i+ δqj
T
δMc i+ i+ δ q˙j δMc i+ i+ δ q¨j
=
δΩi+ i+ Jc i+ i+ − δ q˙j
=
δΩi+ i+ Jc i+ i+ − δ q¨j
δFc i+ i+ δqj
= mi+
δ¨ri+ 0,i+ δqj
δFc i+ i+ δ q˙j
= mi+
δ¨ri+ 0,i+ δ q˙j
δFc i+ i+ δ q¨j
= mi+
δ¨ri+ 0,i+ δ q¨j
δΩi+ i+ Jc i+ i+ δ q˙j δΩi+ i+ Jc i+ i+ δ q¨j
T
T
The backward-recursive partials:
=
δf i δFc ii δAi+ i+ fi+ + Ai+ i + δqj δqj δqj i+
=
δFc ii δqj
=
δf i δFc ii Ai+ i + δ q˙j δ q˙j i+
=
δFc ii δ q˙j
=
δfii δFc ii Ai+ + δ q¨j δ q¨j i+
=
δFc ii δ q¨j
δfii δqj
∃i+
δfii δqj
∃i+
δfii δ q˙j
∃i+
δfii δ q˙j
∃i+
δfii δ q¨j
∃i+
δfii δ q¨j
∃i+
6
δηii δqj
∃i+
δηii δqj
∃i+
=
i δFc ii δM ci i + + r i,i+ δqj δqj i+ i+ δAi+ δηi+ δf δA i+ i+ i+ i+ η + Ai+ + sii,i+ f + sii,i+ Ai+ δqj δqj i+ δqj δqj i+ i+
=
i δFc ii δM ci i + r i,i+ δqj δqj
=
i+ i+ i i δf δη δF δM c c i+ i i i i Ai+ i+ + s + + r i,i+ Ai+ i,i+ δ q ˙ δ q ˙j δ q˙j δ q˙j j i+
=
i δFc ii δM ci i + r i,i+ δ q˙j δ q˙j
=
i+ i+ i δfi+ δηi+ δFc ii δM ci i i Ai+ + si,i+ Ai+ + + ri,i+ δ q¨j δ q¨j δ q¨j δ q¨j i+
δηii δ q˙j
∃i+
δηii δ q˙j
∃i+
δηii δ q¨j
∃i+
δηii δ q¨j
∃i+
=
i δFc ii δM ci i + r i,i+ δ q¨j δ q¨j
δτi δqj
=
δηii i ·z δqj i
δτi δ q˙j
=
δηii i ·z δ q˙j i
δτi δ q¨j
=
δηii i ·z δ q¨j i
7
References [1] Balafoutis, C. A., and Patel, R. V. Dynamic Analysis of Robot Manipulators: A Cartesian Tensor Approach. Kluwer Academic Publishers, 1991. [2] Gill, P. E., Murray, W., and Wright, M. H. Practical Optimization. Academic Press, 1981. [3] Rose, C. F., Guenter, B., Bodenheimer, B., and Cohen, M. Efficient generation of motion transitions using spacetime constraints. In Computer Graphics (Aug. 1996), pp. 147–154. Proceedings of SIGGRAPH 96. [4] Witkin, A., and Kass, M. Spacetime constraints. In Computer Graphics (Aug. 1988), pp. 159–168. Proceedings of SIGGRAPH 88.
8
