[足式机器人]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}}}^{?}\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 ( i ) : p ( 3 ) = { 2 } , p ( 4 ) = { 2 } p\left( i \right) :p\left( 3 \right) =\left\{ 2 \right\} ,p\left( 4 \right) =\left\{ 2 \right\} p(i):p(3)={2},p(4)={2}
  Children : c ( i ) : c ( 2 ) = { 3 , 4 } , c ( 1 ) = { 2 } c\left( i \right) :c\left( 2 \right) =\left\{ 3,4 \right\} ,c\left( 1 \right) =\left\{ 2 \right\} c(i):c(2)={3,4},c(1)={2}

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

• Frame { i } \left\{ i \right\} {i} 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 { i } \left\{ i \right\} {i}

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}}}^{?}{\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}^{?}{\mathcal{I}}_4{\mathcal{V}}_4 \\ ?{\mathcal{F}}_4={\mathcal{I}}_4{\mathcal{A}}_4+{{\tilde{\mathcal{V}}}_4}^{?}{\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}^{?}{\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}}}^{?}{\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}}}^{?}{\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,?,J_4$
$J_{\mathrm{i}}$ : denote the Jacobian of body(Link) $i$ , i.e. ${\mathcal{V}}_{\mathrm{i}}=J_{\mathrm{i}}\dot{\theta }=\left[\begin{array}{cccc}{J}_{\mathrm{i}1}& J_{\mathrm{i}2}& J_{\mathrm{i}3}& J_{\mathrm{i}4}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\\ {\dot{\theta }}_3\\ {\dot{\theta }}_4\end{array}\right]$
e.g. $$\begin{gathered} {\mathcal{V}}_1=J_1\dot{\theta }=\left[\begin{array}{cccc}{\delta }_{11}{\mathcal{S}}_1& {\delta }_{12}{\mathcal{S}}_2& {\delta }_{13}{\mathcal{S}}_3& {\delta }_{14}{\mathcal{S}}_4\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\\ {\dot{\theta }}_3\\ {\dot{\theta }}_4\end{array}\right]=\left[\begin{array}{cccc}{\mathcal{S}}_1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\\ {\dot{\theta }}_3\\ {\dot{\theta }}_4\end{array}\right] \\ {\delta }_{\mathrm{ij}}=\{\begin{array}{l}1,ifjoint\mathrm{j}supportbody\mathrm{i}\\ 0,otherwise\end{array} \end{gathered}$$

${\mathcal{V}}_2=J_2\dot{\theta }=\left[\begin{array}{cccc}{\mathcal{S}}_1& {\mathcal{S}}_2& 0& 0\end{array}\right]\dot{\theta }?{\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}^{?}{\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}^{?}{\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}^{?}{\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}^{?}{\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}^{?}{\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}^{?}{\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) \end{gathered}$$

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

$$\begin{gathered} \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) \end{gathered}$$

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}}}^{?}{\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}}}^{?}{\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}}}^{?}{\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 , ${{\mathcal{V}}_{\mathrm{i}}|}_{6\times 1}={J_{\mathrm{i}}|}_{6\times \mathrm{n}}{\dot{\theta }|}_{\mathrm{n}\times 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?\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$ , $?\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
  ${\ddot{\theta }}_{0,1}=\left[\begin{array}{c}1\\ 0\\ ?\\ 0\end{array}\right],\tau =\left[\begin{array}{ccc}{M}_1\left(\theta \right)& ?& M_{\mathrm{n}}\left(\theta \right)\end{array}\right],M_{1,\mathrm{j}}\left(\theta \right)=\tau {\ddot{\theta }}_{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{?M_{\mathrm{ij}}}{?{\theta }_{\mathrm{k}}}+\frac{?M_{\mathrm{ik}}}{?{\theta }_{\mathrm{j}}}?\frac{?M_{\mathrm{jk}}}{?{\theta }_{\mathrm{i}}}\right){\dot{\theta }}_{\mathrm{k}},{\Gamma }_{\mathrm{ijk}}=\frac{1}{2}\left(\frac{?M_{\mathrm{ij}}}{?{\theta }_{\mathrm{k}}}+\frac{?M_{\mathrm{ik}}}{?{\theta }_{\mathrm{j}}}?\frac{?M_{\mathrm{jk}}}{?{\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