lesson4_dynamics.md

##acceleration

线加速度

动点在相对坐标系B中的运动，变换到坐标系A中的表达式：

$$^AV_Q=^AR_B{}^BV_Q+{}^AV_{BORG}+{}^A\Omega B\times {}^AR_B{}^BQ$$ $$\begin{align}\frac{d({}^AV{BORG})}{dt}&={}^A\dot{V}_{BORG} \ \frac{d({}^AR_B{}^BV_Q)}{dt}&=^AR_B{}^B\dot{V}_Q+{}^A\Omega _B\times ^AR_B{}^BV_Q \ \frac{d({}^A\Omega _B\times {}^AR_B{}^BQ)}{dt}&= {}^A\dot{\Omega} _B\times {}^AR_B{}^BQ+{}^A\Omega _B\times ({}^A\Omega B\times {}^AR_B{}^BQ+{}^AR_B{}^BV_Q+{}^AV{BORG}) \end{align}$$

其中叉积的性质：

$$\frac{d(a\times b)}{dt}=\frac{da}{dt}\times b+a\times \frac{db}{dt}$$

in revolute joint:

$$^A\dot{V}_Q={}^A\dot{\Omega} _B\times {}^AR_B{}^BQ+{}^A\Omega _B\times {}^A\Omega _B\times {}^AR_B{}^BQ$$

in prismatic joint:

$$^A\dot{V}Q={}^A\dot{V}{BORG}+{}^A\dot{\Omega} B\times {}^AV{BORG}$$

角加速度

$${}^A\Omega_C={}^A\Omega_B+{}^A_BR{}^B\Omega_C \ {}^A\dot{\Omega}_C={}^A\dot{\Omega}_B+{}^A_BR{}^B\dot{\Omega}_C+ {}^A\Omega_B\times {}^A_BR{}^B\Omega_C$$

rigid body dynamics

linear momentum

$$\psi=mv \ \frac{d\psi}{dt}=F$$

angular momentum(角动量/动量矩)

$$p\times m\dot{v}=p\times F=N$$(moments, 力矩)

$$\phi=p\times mv$$

刚体定轴转动：

$$\phi=\int_Vp\times(\omega\times p)\rho d\nu$$

$$p\times(\omega\times p)=p\times(-p)\times \omega=\hat{p}(-\hat{p})\omega$$

so:

$$\phi=\int\hat{p}(-\hat{p})\rho d\nu\cdot\omega=I\omega$$ ($I$, inertia tensor)

Newton equation and Euler equation

$$\begin{align}\dot{\psi}&=F \ F&=ma \ \dot{\phi}&=N \ N&=I\dot{\omega}+\omega\times I\omega \end{align}$$（此处请注意对角动量的求导，对惯性张量I的求导）

inertia tensor

$$\begin{align}I&=\int_V\hat{p}(-\hat{p})\rho d\nu \&=\int_V((p^Tp)E_3-pp^T)\rho d\nu \end{align}$$

parallel axis theorem

$p_C$为坐标系C原点在坐标系A中的坐标。

$$I_A=I_C+M[(p_C^Tp_C)E_3-p_Cp_C^T]$$

##Newton-Euler algorithm

$$F_i=m\dot{v}_{c_1} \ N_i={}^{C_i}I\dot{\omega}_i+\omega_i\times {}^{C_i}I\omega_i $$

where the origin of frame{$C_i$} is at the center of the body, with the same orientation as frame{i}.

由关节位置，速度和加速度计算所需的关节力矩。

forward equation: vel, acceleration

$$\begin{aligned} &\boldsymbol{\omega}{i}=\boldsymbol{\omega}{i-1}+\mathbf{z}{i} \dot{\theta}{i} \ &\dot{\boldsymbol{\omega}}{i}=\dot{\boldsymbol{\omega}}{i-1}+\mathbf{z}{i} \ddot{\theta}{i}+\boldsymbol{\omega}{i-1} \times \mathbf{Z}{i} \dot{\theta}{i} \ &\mathbf{a}{i}=\mathbf{a}{i-1}+\dot{\boldsymbol{\omega}}{i-1} \times \mathbf{s}{i-1}+\boldsymbol{\omega}{i-1} \times\left(\boldsymbol{\omega}{i-1} \times \mathbf{s}{i-1}\right) \ &\mathbf{a}{C{i}}=\mathbf{a}{i}+\dot{\boldsymbol{\omega}}{i} \times \mathbf{r}{i-1}+\boldsymbol{\omega}{i} \times\left(\boldsymbol{\omega}{i} \times \mathbf{r}{i-1}\right) \end{aligned}$$

forward:

$$\begin{aligned} &{ }^{i+1} \omega_{i+1}={ }{i}^{i+1} R^{i} \omega{i}+\dot{\theta}{i+1}{ }^{i+1} Z{i+1}\ &{ }^{i+1} \dot{\omega}{i+1}={ }{i}^{i+1} R^{i} \dot{\omega}{i}+{i}^{i+1} R^{i} \omega_{i} \times (\dot{\boldsymbol{\theta}}{i+1}{ }^{i+1} Z{i+1})+\ddot{\theta}{i+1}{ }^{i+1} Z{i+1}\ &{ }^{i+1} \dot{v}{i+1}={i}^{i+1} R\left(^{i} \dot{\omega}{i} \times^{i} P{i+1}+^{i} \omega_{i} \times\left({ }^{i} \omega_{i} \times{ }^{i} P_{i+1}\right)+{ }^{i} \dot{v}{i}\right)\ &{ }^{i+1} \dot{v}{c_{i+1}}={ }^{i+1} \dot{\omega}{i+1} \times{ }^{i+1} P{C_{i+1}}\ &+{ }^{i+1} \omega_{i+1} \times\left({ }^{i+1} \omega_{i+1} \times ^{i+1} P_{c_{i+1}}\right)+{ }^{i+1} \dot{v}{i+1}\ &{ }^{i+1} F{i+1}=m_{i+1}{ }^{i+1} \dot{v}{C{i+1}}\ &{ }^{i+1} N_{i+1}={ }^{c_{i+1}} I_{i+1}{ }^{i+1} \dot{\omega}{i+1}+{ }^{i+1} \omega{i+1} \times{ }^{c_{i+1}} I_{i+1}{ }^{i+1} \omega_{i+1}\end{aligned}$$

backward

$$\begin{aligned}&{ }^{i} f_{i}={ }{i+1}^{i} R^{i+1} f{i+1}+{ }^{i} F_{i}\ &n_{i}=^{i} N_{i}+{ }{i+1}^{i} R^{i+1} n{i+1}+{ }^{i} P_{C_{i}} \times{ }^{i} F_{i}\ &+{ }^{i} P_{i+1} \times_{i+1}^{i} R^{i+1} f_{i+1}\ &\tau_{i}={ }^{i} n_{i}^{T} \cdot {}^iZ_{i} \end{aligned}$$

惯性张量：

$$\mathbf{I}=\left[\begin{array}{lll} I_{x x} & I_{x y} & I_{x z} \\ I_{y x} & I_{y y} & I_{y z} \\ I_{z x} & I_{z y} & I_{z z} \end{array}\right]$$

设 $w=[w_x, w_y, w_z]^T$,角动量：$L=Iw$

two link arm ref

• inverse dynamics: q to torque
• forward dynamics: torque to q

$$\ddot{q} = M^{-1}(q)(\tau-C(q, \dot{q})\dot{q} -G(q)) \\ \dot{q} = \int \ddot{q}dt \\ q = \int \dot{q}dt$$

动力学方程的结构

state space equation

$$\tau=M(q)\ddot{q}+V(q, \dot{q})+G(q)$$

$$\tau=M(q)\ddot{q}+V(q, \dot{q})+G(q) + J^T(\theta) F_{tip}$$

configuration space equation

$$\tau=M(q)\ddot{q}+B(q)[\dot{q}\dot{q}]+C(q)[\dot{q}^2]+G(q)$$

Lagrange equation

$$L=K-U$$(kinetic energy-potential energy)

$$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}}-\frac{\partial L}{\partial q}=\tau$$

$$\frac{d}{dt}\frac{\partial K}{\partial \dot{q}}-\frac{\partial K}{\partial q}+\frac{\partial U}{\partial q}=\tau$$

kinetic energy: $K=\sum_iK_i=\sum_i(\frac{1}{2}m_i\upsilon^T_{C_i}\upsilon_{C_i}+\frac{1}{2}{}^i\omega^T_i {}^{C_i}I_i\omega_i )=\frac{1}{2}\dot{q}^TM(q)\dot{q}$

$$\frac{\partial K}{\partial \dot{q}}=M(q)\dot{q} \ \frac{d}{dt}\frac{\partial K}{\partial \dot{q}}=\frac{d}{dt}(M\dot{q})=M\ddot{q}+\dot{M}\dot{q}$$

$$\begin{align}\frac{d}{dt}\frac{\partial K}{\partial \dot{q}}-\frac{\partial K}{\partial q}&=M\ddot{q}+\dot{M}\dot{q}-\frac{1}{2}\begin{bmatrix}\dot{q}^T\frac{\partial M}{\partial q_1}\dot{q}\ \vdots \ \dot{q}^T\frac{\partial M}{\partial q_n}\dot{q}\end{bmatrix}\ &=M\ddot{q}+V(q, \dot{q}) \end{align}$$

kinetic energy: work done by external forces to bring the system from rest to its current state.

two link arm

$$\begin{aligned} &\boldsymbol{\tau}=\boldsymbol{M}(\boldsymbol{\theta}) \ddot{\boldsymbol{\theta}}+\boldsymbol{h}(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}})+\boldsymbol{g}(\boldsymbol{\theta}) \ &\boldsymbol{M}(\boldsymbol{\theta})=\left[\begin{array}{ll} M_{11} & M_{12} \ M_{21} & M_{22} \end{array}\right] \quad \boldsymbol{h}(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}})=\left[\begin{array}{c} -m_{2} l_{1} l_{12} \dot{\theta}{2}\left(2 \dot{\theta}{1}+\dot{\theta}{2}\right) \sin \theta{2} \ m_{2} l_{1} l_{g 2} \dot{\theta}{1}^{2} \sin \theta{2} \end{array}\right] \ &\boldsymbol{g}(\boldsymbol{\theta})=\left[\begin{array}{c} m_{1} g l_{g 1} \cos \theta_{1}+m_{2} g\left(l_{1} \cos \theta_{1}+l_{g 2} \cos \left(\theta_{1}+\theta_{2}\right)\right) \ m_{2} g l_{92} \cos \left(\theta_{1}+\theta_{2}\right) \end{array}\right. \ &M_{11}=I_{1}+I_{2}+m_{1} l_{g 1}^{2}+m_{2}\left(l_{1}^{2}+l_{g 2}^{2}+2 l_{1} l_{g 2}+2 l_{1} l_{g 2} \cos \theta_{2}\right) \ &M_{12}=M_{21}=I_{2}+m_{2}\left(l_{g 2}^{2}+l_{1} l_{g 2} \cos \theta_{2}\right) \ &M_{22}=I_{2}+m_{2} l_{g 2}^{2} \end{aligned}$$

another version:

$$ \begin{aligned} &\tau_{1}=H_{11} \ddot{\theta}{1}+H{12} \ddot{\theta}{2}-h \dot{\theta}{2}^{2}-2 h \dot{\theta}{1} \dot{\theta}{2}+G_{1} \ &\tau_{2}=H_{22} \ddot{\theta}{2}+H{21} \ddot{\theta}{1}+h \dot{\theta}{1}^{2}+G_{2} \end{aligned} $$ where $$ \begin{aligned} &H_{11}=m_{1} \ell_{c 1}^{2}+I_{1}+m_{2}\left(\ell_{1}^{2}+\ell_{c 2}^{2}+2 \ell_{1} \ell_{c 2} \cos \theta_{2}\right)+I_{2} \ &H_{22}=m_{2} \ell_{c 2}^{2}+I_{2} \ &H_{12}=m_{2}\left(\ell_{c 2}^{2}+\ell_{1} \ell_{c 2} \cos \theta_{2}\right)+I_{2} \ &h=m_{2} \ell_{1} \ell_{c 2} \sin \theta_{2} \ &G_{1}=m_{1} \ell_{c 1} g \cos \theta_{1}+m_{2} g\left{\ell_{c 2} \cos \left(\theta_{1}+\theta_{2}\right)+\ell_{1} \cos \theta_{1}\right} \ &G_{2}=m_{2} g \ell_{c 2} \cos \left(\theta_{1}+\theta_{2}\right) \end{aligned} $$

ref

• from urdf to kinematic and dynamic