[足式机器人]Part4 南科大高等机器人控制课 Ch09 Dynamics of Open Chains

本文仅供学习使用
本文参考：
B站：CLEAR_LAB
笔者带更新-运动学
课程主讲教师：
Prof. Wei Zhang

---

1. Introduction

1.1 From Single Rigid Body to Open Chains

• Recall Newton-Euler Equation for a single rigid body:
  $$\mathcal{F}=\frac{\mathrm{d}}{\mathrm{d}t}\mathcal{H}=\mathcal{I}\mathcal{A}+{\tilde{\mathcal{V}}}^{\ast }\mathcal{IV}$$
• Open chains consists of multiple rigid links connected through joints
• Dynamics of adjacent links are coupled
• This lecture: model multi-body dynamics subject to joint constrainsts.

1.2 Preview of Open-Chain Dynamics

• Equations of Motion are a set of 2nd-order differential equations:
  $$\tau =M\left(\theta \right)\ddot{\theta }+c\left(\theta ,\dot{\theta }\right)$$
  $\theta \in {\mathbb{R}}^n$ : vector of joint variables ;
  $\tau \in {\mathbb{R}}^n$ : vector of joint forces/torques(与之前的符号不一致);
  $M\left(\theta \right)\in {\mathbb{R}}^{n\times n}$ : mass matrix
  $c\left(\theta ,\dot{\theta }\right)\in {\mathbb{R}}^n$ : forces that lump together centripetal, Coriolis, gravity, friction terms, and torques induced by external forces. These terms depends on $\theta$ and $\dot{\theta }$

• Forward dynamics : Determine accecleration $\ddot{\theta }$ given the state $\left(\theta ,\dot{\theta }\right)$ and the joint forces/torques
  $$\ddot{\theta }\leftarrow FD\left(\tau ,\theta ,\dot{\theta },{\mathcal{F}}_{\mathrm{ext}}\right)$$

• Inverse dynamics : Finding torques/forces given state $\left(\theta ,\dot{\theta }\right)$ and desired acceleration $\ddot{\theta }$ (Given desired motion, find the required torque to generate the desired motion)
  $$\tau \leftarrow ID\left(\theta ,\dot{\theta },\ddot{\theta },{\mathcal{F}}_{\mathrm{ext}}\right)$$

1.3 Lagrangian VS. Newton-Euler Methods

• There are typically two ways to derive the equation of motion for an open-chain robot: Lagrangian method and Newton-Euler method

Lagrangian Formulation : Energy-based method ; Dynamic equations in closed form ; Often used for study of dynamic properties and analysis of control methods

Newton-Euler Formulation : Balance of forces/torques ; Dynamic equations in numeric/recuisive form ; Often used for numerical solution of forward/inverse dynamics

We focus on Newton-Euler Formulation

2. Inverse Dynamics: Recursive Newton-Euler Algorithm(RNEA)

2.1 RNEA: Notations

• Number bodies : 1 to $N$
  Parents : $p\left(i\right):p\left(3\right)=\left\{2\right\},p\left(4\right)=\left\{2\right\}$
  Children : $c\left(i\right):c\left(2\right)=\left\{3,4\right\},c\left(1\right)=\left\{2\right\}$

• Joint $i$ connects $p\left(i\right)$ to $i$

• Frame $\left\{i\right\}$ attached to body $i$ at the joint

• ${\mathcal{S}}_{\mathrm{i}}$ : Spatial velocity (screw axis) of joint $i$

• ${\mathcal{V}}_{\mathrm{i}}$ and ${\mathcal{A}}_{\mathrm{i}}$ : spatial velocity and acceleration of body $i$

• ${\mathcal{F}}_{\mathrm{i}}$ : force(wrench) onto body $i$ from body $p\left(i\right)$

• Note : By default, all vectors $\left({\mathcal{S}}_{\mathrm{i}},{\mathcal{V}}_{\mathrm{i}},{\mathcal{F}}_{\mathrm{i}}\right)$ are expressed in local frame $\left\{i\right\}$

2.1.1 RNEA: Velocity and Accel. Propagation(Forward Pass)

Goal: Given joint velocity $\dot{\theta }$ and acceleration $\ddot{\theta }$ , compute the body spatial velocity ${\mathcal{V}}_{\mathrm{i}}$ and spatial acceleration ${\mathcal{A}}_{\mathrm{i}}$

• Velocity Propagation : ${\mathcal{V}}_{\mathrm{i}}^i=\left[X_{\mathrm{p}\left(i\right)}^i\right]{\mathcal{V}}_{\mathrm{p}\left(i\right)}^{p\left(i\right)}+{\mathcal{S}}_{\mathrm{i}}^i{\dot{\theta }}_{\mathrm{i}}$
• Accel Propagation : ${\mathcal{A}}_{\mathrm{i}}^i=\left[X_{\mathrm{p}\left(i\right)}^i\right]{\mathcal{A}}_{\mathrm{p}\left(i\right)}^{p\left(i\right)}+{\tilde{\mathcal{V}}}_{\mathrm{i}}^i{\mathcal{S}}_{\mathrm{i}}^i{\dot{\theta }}_{\mathrm{i}}+{\mathcal{S}}_{\mathrm{i}}^i{\ddot{\theta }}_{\mathrm{i}}$

从机架侧开始计算

2.1.2 RNEA: Force Propagation(Backward Pass)

Goal : Given body spatial velocity ${\mathcal{V}}_{\mathrm{i}}$ amd spatial acceleration ${\mathcal{A}}_{\mathrm{i}}$, compute the joint wrench ${\mathcal{F}}_{\mathrm{i}}$ and the corresponding torque ${\tau }_{\mathrm{i}}={{\mathcal{S}}_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{F}}_{\mathrm{i}}$

$\{\begin{array}{l}{\mathcal{F}}_{\mathrm{i}}={\mathcal{I}}_{\mathrm{i}}{\mathcal{A}}_{\mathrm{i}}+{{\tilde{\mathcal{V}}}_{\mathrm{i}}}^{\ast }{\mathcal{I}}_{\mathrm{i}}{\mathcal{V}}_{\mathrm{i}}+{\sum }_{j\in c\left(i\right)}^{}{\mathcal{F}}_{\mathrm{j}}\\ {\tau }_{\mathrm{i}}={{\mathcal{S}}_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{F}}_{\mathrm{i}}\end{array}$

从末端执行构件处开始计算：

• Body 4:
  $$\begin{gathered} {\mathcal{F}}_4+{\mathcal{F}}_{\mathrm{G}4}={\mathcal{I}}_4{\mathcal{A}}_4+{{\tilde{\mathcal{V}}}_4}^{\ast }{\mathcal{I}}_4{\mathcal{V}}_4 \\ \Rightarrow {\mathcal{F}}_4={\mathcal{I}}_4{\mathcal{A}}_4+{{\tilde{\mathcal{V}}}_4}^{\ast }{\mathcal{I}}_4{\mathcal{V}}_4-{\mathcal{F}}_{\mathrm{G}4},{\mathcal{F}}_{\mathrm{G}4}={\mathcal{I}}_4{\mathcal{A}}_{\mathrm{G}}^4={\mathcal{I}}_4\left[X_{\mathrm{O}}^4\right]{\mathcal{A}}_{\mathrm{G}}^O \end{gathered}$$
  ${\tau }_4={{\mathcal{S}}_4}^{\mathrm{T}}{\mathcal{F}}_4$
• Body 2:
  $$\begin{gathered} {\mathcal{F}}_2={\mathcal{I}}_2{\mathcal{A}}_2+{{\tilde{\mathcal{V}}}_2}^{\ast }{\mathcal{I}}_2{\mathcal{V}}_2+{\mathcal{F}}_4+{\mathcal{F}}_3-{\mathcal{F}}_{\mathrm{G}2},{\mathcal{F}}_{\mathrm{G}2}={\mathcal{I}}_2\left[X_{\mathrm{O}}^2\right]{\mathcal{A}}_{\mathrm{G}2}^O \\ {\tau }_2={{\mathcal{S}}_2}^{\mathrm{T}}{\mathcal{F}}_2 \end{gathered}$$

2.1.3 Recursive Newton-Euler Algorithm

$$\tau \leftarrow RNEA\left(\theta ,\dot{\theta },\ddot{\theta },{\mathcal{F}}_{\mathrm{ext}};Model\right)$$
Initialize : ${\mathcal{V}}_0=0,{\mathcal{A}}_0=-{\mathcal{A}}_{\mathrm{G}}$ (without gravity “trick” modify ${\mathcal{F}}_{\mathrm{i}}={\mathcal{I}}_{\mathrm{i}}{\mathcal{A}}_{\mathrm{i}}+{{\tilde{\mathcal{V}}}_{\mathrm{i}}}^{\ast }{\mathcal{I}}_{\mathrm{i}}{\mathcal{V}}_{\mathrm{i}}-{\mathcal{I}}_{\mathrm{i}}\left[X_{\mathrm{O}}^i\right]{\mathcal{A}}_{\mathrm{Gi}}^O$)

• Forward pass : For $i=1$ to $N$
  $${\mathcal{V}}_{\mathrm{i}}^i=\left[X_{\mathrm{p}\left(i\right)}^i\right]{\mathcal{V}}_{\mathrm{p}\left(i\right)}^{p\left(i\right)}+{\mathcal{S}}_{\mathrm{i}}^i{\dot{\theta }}_{\mathrm{i}}$$
  $${\mathcal{A}}_{\mathrm{i}}^i=\left[X_{\mathrm{p}\left(i\right)}^i\right]{\mathcal{A}}_{\mathrm{p}\left(i\right)}^{p\left(i\right)}+{\tilde{\mathcal{V}}}_{\mathrm{i}}^i{\mathcal{S}}_{\mathrm{i}}^i{\dot{\theta }}_{\mathrm{i}}+{\mathcal{S}}_{\mathrm{i}}^i{\ddot{\theta }}_{\mathrm{i}}$$
  $${\mathcal{F}}_{\mathrm{i}}={\mathcal{I}}_{\mathrm{i}}{\mathcal{A}}_{\mathrm{i}}+{{\tilde{\mathcal{V}}}_{\mathrm{i}}}^{\ast }{\mathcal{I}}_{\mathrm{i}}{\mathcal{V}}_{\mathrm{i}}$$
• Backward pass : For $i=N$ to 1
  $${\tau }_{\mathrm{i}}={{\mathcal{S}}_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{F}}_{\mathrm{i}}$$
  $${\mathcal{F}}_{\mathrm{p}\left(i\right)}={\mathcal{F}}_{\mathrm{p}\left(i\right)}+\left[X_{\mathrm{i}}^{p\left(i\right)}\right]{\mathcal{F}}_{\mathrm{i}}$$

3. Analytical Form of the Dynamics Model

3.1 Structures in Dynamic Equation

Jacobian of each link(body) : $J_1,\cdots ,J_4$
$J_{\mathrm{i}}$ : denote the Jacobian of body(Link) $i$ , i.e. V i = J i θ ˙ = [ J i 1 J i 2 J i 3 J i 4 ] [ θ ˙ 1 θ ˙ 2 θ ˙ 3 θ ˙ 4 ] \mathcal{V} _{\mathrm{i}}=J_{\mathrm{i}}\dot{\theta}=\left[ \begin{matrix} J_{\mathrm{i}1}& J_{\mathrm{i}2}& J_{\mathrm{i}3}& J_{\mathrm{i}4}\\ \end{matrix} \right] \left[ \begin{array}{c} \dot{\theta}_1\\ \dot{\theta}_2\\ \dot{\theta}_3\\ \dot{\theta}_4\\ \end{array} \right] Vi​=Ji​θ˙=[Ji1​​Ji2​​Ji3​​Ji4​​] ​θ˙1​θ˙2​θ˙3​θ˙4​​ ​
e.g. V 1 = J 1 θ ˙ = [ δ 11 S 1 δ 12 S 2 δ 13 S 3 δ 14 S 4 ] [ θ ˙ 1 θ ˙ 2 θ ˙ 3 θ ˙ 4 ] = [ S 1 0 0 0 ] [ θ ˙ 1 θ ˙ 2 θ ˙ 3 θ ˙ 4 ] δ i j = { 1 , i f    j o i n t    j s u p p o r t    b o d y    i 0 , o t h e r w i s e \mathcal{V} _1=J_1\dot{\theta}=\left[ \begin{matrix} \delta _{11}\mathcal{S} _1& \delta _{12}\mathcal{S} _2& \delta _{13}\mathcal{S} _3& \delta _{14}\mathcal{S} _4\\ \end{matrix} \right] \left[ \begin{array}{c} \dot{\theta}_1\\ \dot{\theta}_2\\ \dot{\theta}_3\\ \dot{\theta}_4\\ \end{array} \right] =\left[ \begin{matrix} \mathcal{S} _1& 0& 0& 0\\ \end{matrix} \right] \left[ \begin{array}{c} \dot{\theta}_1\\ \dot{\theta}_2\\ \dot{\theta}_3\\ \dot{\theta}_4\\ \end{array} \right] \\ \delta _{\mathrm{ij}}=\begin{cases} 1, if\,\,joint\,\,\mathrm{j} support\,\,body\,\,\mathrm{i}\\ 0, otherwise\\ \end{cases} V1​=J1​θ˙=[δ11​S1​​δ12​S2​​δ13​S3​​δ14​S4​​] ​θ˙1​θ˙2​θ˙3​θ˙4​​ ​=[S1​​0​0​0​] ​θ˙1​θ˙2​θ˙3​θ˙4​​ ​δij​={1,ifjointjsupportbodyi0,otherwise​

${\mathcal{V}}_2=J_2\dot{\theta }=\left[\begin{array}{cccc}{\mathcal{S}}_1& {\mathcal{S}}_2& 0& 0\end{array}\right]\dot{\theta }\Rightarrow {\mathcal{V}}_2^2=\left[\left[X_1^2\right]\begin{array}{cccc}{\mathcal{S}}_2^1& {\mathcal{S}}_2^2& 0& 0\end{array}\right]\dot{\theta }$

see the two-body example:

1. Forward pass :
   ${\mathcal{V}}_1={\mathcal{S}}_1{\dot{\theta }}_1$ , ${\mathcal{V}}_2^2=\left[\begin{array}{cc}\left[X_1^2\right]{\mathcal{S}}_2^1& {\mathcal{S}}_2^2\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]$
   ${\mathcal{A}}_1,{\mathcal{A}}_2$
2. Backward pass :
   $$\begin{gathered} {\mathcal{F}}_2={\mathcal{I}}_2{\mathcal{A}}_2+{{\tilde{\mathcal{V}}}_2}^{\ast }{\mathcal{I}}_2{\mathcal{V}}_2-{\mathcal{F}}_{2\mathrm{ext}}-{\mathcal{F}}_{2\mathrm{G}} \\ {\mathcal{F}}_1={\mathcal{I}}_1{\mathcal{A}}_1+{{\tilde{\mathcal{V}}}_1}^{\ast }{\mathcal{I}}_1{\mathcal{V}}_1-{\mathcal{F}}_{1\mathrm{ext}}-{\mathcal{F}}_{1\mathrm{G}}+\left[X_2^1\right]\left({\mathcal{I}}_2{\mathcal{A}}_2+{{\tilde{\mathcal{V}}}_2}^{\ast }{\mathcal{I}}_2{\mathcal{V}}_2-{\mathcal{F}}_{2\mathrm{ext}}-{\mathcal{F}}_{2\mathrm{G}}\right) \\ {\tau }_2={{\mathcal{S}}_2}^{\mathrm{T}}{\mathcal{F}}_2 \\ {\tau }_1={{\mathcal{S}}_1}^{\mathrm{T}}{\mathcal{F}}_1 \end{gathered}$$

Overall torque expression :
$$\begin{gathered} {\tau }_1={{\mathcal{S}}_1}^{\mathrm{T}}{\mathcal{F}}_1={{\mathcal{S}}_1}^{\mathrm{T}}\left({\mathcal{I}}_1{\mathcal{A}}_1+{{\tilde{\mathcal{V}}}_1}^{\ast }{\mathcal{I}}_1{\mathcal{V}}_1-{\mathcal{F}}_{1\mathrm{ext}}-{\mathcal{F}}_{1\mathrm{G}}+\left[X_2^1\right]\left({\mathcal{I}}_2{\mathcal{A}}_2+{{\tilde{\mathcal{V}}}_2}^{\ast }{\mathcal{I}}_2{\mathcal{V}}_2-{\mathcal{F}}_{2\mathrm{ext}}-{\mathcal{F}}_{2\mathrm{G}}\right)\right) \\ ={{\mathcal{S}}_1}^{\mathrm{T}}\left({\mathcal{I}}_1{\mathcal{A}}_1+{{\tilde{\mathcal{V}}}_1}^{\ast }{\mathcal{I}}_1{\mathcal{V}}_1\right)+{\left(\left[X_1^2\right]{\mathcal{S}}_1\right)}^{\mathrm{T}}\left({\mathcal{I}}_2{\mathcal{A}}_2+{{\tilde{\mathcal{V}}}_2}^{\ast }{\mathcal{I}}_2{\mathcal{V}}_2\right)-{{\mathcal{S}}_1}^{\mathrm{T}}\left({\mathcal{F}}_{1\mathrm{ext}}+{\mathcal{F}}_{1\mathrm{G}}+\left[X_2^1\right]\left({\mathcal{F}}_{2\mathrm{ext}}+{\mathcal{F}}_{2\mathrm{G}}\right)\right) \end{gathered}$$

due to motion of body 1 and 2 and external force of body 1 and 2

τ = [ τ 1 τ 2 ] = [ S 1 T ( I 1 A 1 + V ~ 1 ∗ I 1 V 1 ) + ( [ X 1 2 ] S 1 ) T ( I 2 A 2 + V ~ 2 ∗ I 2 V 2 ) − S 1 T ( F 1 e x t + F 1 G + [ X 2 1 ] ( F 2 e x t + F 2 G ) ) S 2 T ( I 2 A 2 + V ~ 2 ∗ I 2 V 2 − F 2 e x t − F 2 G ) ] = [ S 1 T 0 ] ( I 1 A 1 + V ~ 1 ∗ I 1 V 1 − F 1 e x t − F 1 G ) + [ ( [ X 1 2 ] S 1 ) T S 2 T ] ( I 2 A 2 + V ~ 2 ∗ I 2 V 2 − F 2 e x t − F 2 G ) = J 1 T ( I 1 A 1 + V ~ 1 ∗ I 1 V 1 − F 1 e x t − F 1 G ) + J 2 T ( I 2 A 2 + V ~ 2 ∗ I 2 V 2 − F 2 e x t − F 2 G ) \tau =\left[ \begin{array}{c} \tau _1\\ \tau _2\\ \end{array} \right] =\left[ \begin{array}{c} {\mathcal{S} _1}^{\mathrm{T}}\left( \mathcal{I} _1\mathcal{A} _1+{\tilde{\mathcal{V}}_1}^*\mathcal{I} _1\mathcal{V} _1 \right) +\left( \left[ X_{1}^{2} \right] \mathcal{S} _1 \right) ^{\mathrm{T}}\left( \mathcal{I} _2\mathcal{A} _2+{\tilde{\mathcal{V}}_2}^*\mathcal{I} _2\mathcal{V} _2 \right) -{\mathcal{S} _1}^{\mathrm{T}}\left( \mathcal{F} _{1\mathrm{ext}}+\mathcal{F} _{1\mathrm{G}}+\left[ X_{2}^{1} \right] \left( \mathcal{F} _{2\mathrm{ext}}+\mathcal{F} _{2\mathrm{G}} \right) \right)\\ {\mathcal{S} _2}^{\mathrm{T}}\left( \mathcal{I} _2\mathcal{A} _2+{\tilde{\mathcal{V}}_2}^*\mathcal{I} _2\mathcal{V} _2-\mathcal{F} _{2\mathrm{ext}}-\mathcal{F} _{2\mathrm{G}} \right)\\ \end{array} \right] \\ =\left[ \begin{array}{c} {\mathcal{S} _1}^{\mathrm{T}}\\ 0\\ \end{array} \right] \left( \mathcal{I} _1\mathcal{A} _1+{\tilde{\mathcal{V}}_1}^*\mathcal{I} _1\mathcal{V} _1-\mathcal{F} _{1\mathrm{ext}}-\mathcal{F} _{1\mathrm{G}} \right) +\left[ \begin{array}{c} \left( \left[ X_{1}^{2} \right] \mathcal{S} _1 \right) ^{\mathrm{T}}\\ {\mathcal{S} _2}^{\mathrm{T}}\\ \end{array} \right] \left( \mathcal{I} _2\mathcal{A} _2+{\tilde{\mathcal{V}}_2}^*\mathcal{I} _2\mathcal{V} _2-\mathcal{F} _{2\mathrm{ext}}-\mathcal{F} _{2\mathrm{G}} \right) \\ ={J_1}^{\mathrm{T}}\left( \mathcal{I} _1\mathcal{A} _1+{\tilde{\mathcal{V}}_1}^*\mathcal{I} _1\mathcal{V} _1-\mathcal{F} _{1\mathrm{ext}}-\mathcal{F} _{1\mathrm{G}} \right) +{J_2}^{\mathrm{T}}\left( \mathcal{I} _2\mathcal{A} _2+{\tilde{\mathcal{V}}_2}^*\mathcal{I} _2\mathcal{V} _2-\mathcal{F} _{2\mathrm{ext}}-\mathcal{F} _{2\mathrm{G}} \right) τ=[τ1​τ2​​]= ​S1​T(I1​A1​+V~1​∗I1​V1​)+([X12​]S1​)T(I2​A2​+V~2​∗I2​V2​)−S1​T(F1ext​+F1G​+[X21​](F2ext​+F2G​))S2​T(I2​A2​+V~2​∗I2​V2​−F2ext​−F2G​)​ ​=[S1​T0​](I1​A1​+V~1​∗I1​V1​−F1ext​−F1G​)+[([X12​]S1​)TS2​T​](I2​A2​+V~2​∗I2​V2​−F2ext​−F2G​)=J1​T(I1​A1​+V~1​∗I1​V1​−F1ext​−F1G​)+J2​T(I2​A2​+V~2​∗I2​V2​−F2ext​−F2G​)

Overall : in general with N-links / Joints

$$\tau =\sum _{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}\left({\mathcal{I}}_{\mathrm{i}}{\mathcal{A}}_{\mathrm{i}}+{{\tilde{\mathcal{V}}}_{\mathrm{i}}}^{\ast }{\mathcal{I}}_{\mathrm{i}}{\mathcal{V}}_{\mathrm{i}}-{\mathcal{F}}_{\mathrm{iext}}-{\mathcal{F}}_{\mathrm{iG}}\right)$$

$$\begin{gathered} {\mathcal{V}}_{\mathrm{i}}=J_{\mathrm{i}}\dot{\theta } \\ {\mathcal{A}}_{\mathrm{i}}={\dot{\mathcal{V}}}_{\mathrm{i}}=\left({\dot{J}}_{\mathrm{i}}\dot{\theta }+J_{\mathrm{i}}\ddot{\theta }+{\tilde{\mathcal{V}}}_{\mathrm{i}}{J}_{\mathrm{i}}\dot{\theta }\right) \end{gathered}$$

上式看上去难以理解，尤其是加速度旋量，本质上是因为在构件坐标系下的求导，相当于需要对运动基向量求导所产生的加速度

带入可得：
$$\tau =\sum _{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{I}}_{\mathrm{i}}{J}_{\mathrm{i}}\ddot{\theta }+\sum _{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}\left({\mathcal{I}}_{\mathrm{i}}{\dot{J}}_{\mathrm{i}}+{\mathcal{I}}_{\mathrm{i}}{\tilde{\mathcal{V}}}_{\mathrm{i}}{J}_{\mathrm{i}}+{{\tilde{\mathcal{V}}}_{\mathrm{i}}}^{\ast }{\mathcal{I}}_{\mathrm{i}}{J}_{\mathrm{i}}\right)\dot{\theta }$$
${\sum }_{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{I}}_{\mathrm{i}}{J}_{\mathrm{i}}=M\left(\theta \right),{\sum }_{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}\left({\mathcal{I}}_{\mathrm{i}}{\dot{J}}_{\mathrm{i}}+{\mathcal{I}}_{\mathrm{i}}{\tilde{\mathcal{V}}}_{\mathrm{i}}{J}_{\mathrm{i}}+{{\tilde{\mathcal{V}}}_{\mathrm{i}}}^{\ast }{\mathcal{I}}_{\mathrm{i}}{J}_{\mathrm{i}}\right)=c\left(\theta ,\dot{\theta }\right)$ , ${\tau }_G={\sum }_{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{I}}_{\mathrm{i}}\left[X_{\mathrm{O}}^i\right]\left(-{\mathcal{A}}_{\mathrm{iG}}^O\right)$

最终理解：$\tau =M\left(\theta \right)\ddot{\theta }+c\left(\theta ,\dot{\theta }\right)\dot{\theta }+{\tau }_G+J^{\mathrm{T}}\left(\theta \right){\mathcal{F}}_{\mathrm{ext}}$
${\mathcal{F}}_{\mathrm{ext}}$ : applied from the body to environment

回顾：
$J_{\mathrm{i}}$ : body/link i Jacobian , V i ∣ 6 × 1 = J i ∣ 6 × n θ ˙ ∣ n × 1 \left. \mathcal{V} _{\mathrm{i}} \right|_{6\times 1}=\left. J_{\mathrm{i}} \right|_{6\times \mathrm{n}}\left. \dot{\theta} \right|_{\mathrm{n}\times 1} Vi​∣6×1​=Ji​∣6×n​θ˙ ​n×1​
$\tau \in {\mathbb{R}}^n$ , $\tau$ play two major roles :

1. generate motion
2. generate force/torque

3.2 Properties of Dynamics Model of Multi-Body Systems

Only cpnsider body 2’s effect

4. Forward Dynamics Algorithms

4.1 Forward Dynamics Problem

$$\tau =M\left(\theta \right)\ddot{\theta }+c\left(\theta ,\dot{\theta }\right)\dot{\theta }+{\tau }_G+J^{\mathrm{T}}\left(\theta \right){\mathcal{F}}_{\mathrm{ext}}=M\left(\theta \right)\ddot{\theta }+h\left(\theta ,\dot{\theta }\right)$$

• Inverse dynamics : $\tau \leftarrow RNEA\left(\theta ,\dot{\theta },\ddot{\theta },{\mathcal{F}}_{\mathrm{ext}}\right)$ —— $O\left(N\right)$ complexity
  RNEA can work directly with a given URDF-United Robotics Description Format model (kinematic tree + joint model + dynamic parameters). It does not require explicit formula for $M\left(\theta \right),h\left(\theta ,\dot{\theta }\right)$

• Forward dynamics : Given $\left(\theta ,\dot{\theta }\right),\tau ,{\mathcal{F}}_{\mathrm{ext}}$ , find $\ddot{\theta }$
  Calculate $h\left(\theta ,\dot{\theta }\right)=c\left(\theta ,\dot{\theta }\right)\dot{\theta }+{\tau }_{\mathrm{G}}+J^{\mathrm{T}}{\mathcal{F}}_{\mathrm{ext}}$
  Caculate mass matrix $M\left(\theta \right)={\sum }_{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{I}}_{\mathrm{i}}{J}_{\mathrm{i}}$
  Solve $M\ddot{\theta }=\tau -h\Rightarrow \ddot{\theta }=M^{-1}\left(\tau -h\right)$: This is not the most efficient way to find $\ddot{\theta }$

4.2 Caculations of h and M

Denote our inverse dynamics algorithm : $\tau =RNEA\left(\theta ,\dot{\theta },\ddot{\theta },{\mathcal{F}}_{\mathrm{ext}}\right)=M\ddot{\theta }+h$

• Calculation of $h$ : obviously , $\tau =h$ if $\ddot{\theta }=0$. Therefore, $h$ can be computed via : $h\left(\theta ,\dot{\theta }\right)=RNEA\left(\theta ,\dot{\theta },0,{\mathcal{F}}_{\mathrm{ext}}\right)$

• Calculation of $M$ : Note $h\left(\theta ,\dot{\theta }\right)=c\left(\theta ,\dot{\theta }\right)\dot{\theta }+{\tau }_{\mathrm{G}}+J^{\mathrm{T}}{\mathcal{F}}_{\mathrm{ext}}$
  Set $G=0,{\mathcal{F}}_{\mathrm{ext}}=0,\dot{\theta }=0$ , then $h\left(\theta ,\dot{\theta }\right)=0$ , $\Rightarrow \tau =M\left(\theta \right)\ddot{\theta }$
  We can compute the $j$th colum of $M\left(\theta \right)$ by calling the inverse algorithm
  $\tau =M_{:,\mathrm{j}}\left(\theta \right)=RNEA\left(0,0,{\ddot{\theta }}_{0,\mathrm{j}},0\right)$ , ${\ddot{\theta }}_{0,\mathrm{j}}$ is a vector with all zeros except for a 1 at the $j$th entry
  θ ¨ 0 , 1 = [ 1 0 ⋮ 0 ] , τ = [ M 1 ( θ ) ⋯ M n ( θ ) ] , M 1 , j ( θ ) = τ θ ¨ 0 , 1 \ddot{\theta}_{0,1}=\left[ \begin{array}{c} 1\\ 0\\ \vdots\\ 0\\ \end{array} \right] ,\tau =\left[ \begin{matrix} M_1\left( \theta \right)& \cdots& M_{\mathrm{n}}\left( \theta \right)\\ \end{matrix} \right] ,M_{1,\mathrm{j}}\left( \theta \right) =\tau \ddot{\theta}_{0,1} θ¨0,1​= ​10⋮0​ ​,τ=[M1​(θ)​⋯​Mn​(θ)​],M1,j​(θ)=τθ¨0,1​

• A more effiicient algorithm for computing $M$ is the Composite-Rigid-Body Algorithm(CRBA). Details can be found in Featherstone’s Book

4.3 Forward Dynamics Algorithms

Now assume we have $\theta ,\dot{\theta },\tau ,M\left(\theta \right),h$ , then we can immediately compute $\ddot{\theta }$ as $\ddot{\theta }=M^{-1}\left(\tau -h\right)$ —— $\ddot{\theta }=FD\left(\tau ,\theta ,\dot{\theta },{\mathcal{F}}_{\mathrm{ext}}\right)$

两种看似不同的求解思路，之间的关系是等同的

This provides a 2nd-order differential equation in ${\mathbb{R}}^n$ , we can easily simulate the joint trajectory over any time period (under given ICs ${\theta }^0,{\dot{\theta }}^0$)

Computational Complexity:
RNEA : $O\left(N\right)$
$h\left(\theta ,\dot{\theta }\right)=RNEA\left(\theta ,\dot{\theta },0,{\mathcal{F}}_{\mathrm{ext}}\right)$ : $O\left(N\right)$
$M\left(\theta \right)$ : $O\left(N^2\right)$
$M^{-1}\left(\theta \right)$ : $O\left(N^3\right)$

Most efficient forward dynamics algorithm : Articulate-Body Algorithm(ABA) : $O\left(N\right)$

4.4 More Discussions

$M\left(\theta \right)$ : Mass matrix , $M^{\mathrm{T}}\left(\theta \right)=M\left(\theta \right)$, also positive semi-definite.
$M\left(\theta \right)={\sum }_{i=1}^N{J_{\mathrm{i}}}^{\mathrm{T}}{\mathcal{I}}_{\mathrm{i}}{J}_{\mathrm{i}}$——${\mathcal{I}}_{\mathrm{i}}$ Inertia matrix symmetric / postive semi-definite

There are many equivalent ways to define $c\left(\theta ,\dot{\theta }\right)$ , they are all related to the same product $c\left(\theta ,\dot{\theta }\right)\dot{\theta }$ —— common and confusing

Typical expression for $c$ : $c_{\mathrm{ij}}={\sum }_{k=1}^{}\frac{1}{2}\left(\frac{\partial M_{\mathrm{ij}}}{\partial {\theta }_{\mathrm{k}}}+\frac{\partial M_{\mathrm{ik}}}{\partial {\theta }_{\mathrm{j}}}-\frac{\partial M_{\mathrm{jk}}}{\partial {\theta }_{\mathrm{i}}}\right){\dot{\theta }}_{\mathrm{k}},{\Gamma }_{\mathrm{ijk}}=\frac{1}{2}\left(\frac{\partial M_{\mathrm{ij}}}{\partial {\theta }_{\mathrm{k}}}+\frac{\partial M_{\mathrm{ik}}}{\partial {\theta }_{\mathrm{j}}}-\frac{\partial M_{\mathrm{jk}}}{\partial {\theta }_{\mathrm{i}}}\right)$ chrostoffel symbol
$c\left(\theta ,\dot{\theta }\right)$ defind using ${\Gamma }_{\mathrm{ijk}}$ satisies : $\left[\dot{M}-2c\right]$ skew symmetric matrix ($n\times n$)

$M\left(\theta \right),c\left(\theta ,\dot{\theta }\right),{\tau }_G$ all depend on ${\mathcal{I}}_{\mathrm{i}}$ linearly