[足式机器人]Part4 南科大高等机器人控制课 Ch08 Rigid Body Dynamics

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

南科大高等机器人控制课 Ch08 Rigid Body Dynamics

• [1. Spatial Vecocity](#1. Spatial Vecocity)
• • [1.1 Spatial vs. Conventional Accel](#1.1 Spatial vs. Conventional Accel)
  • [1.2 Plueker Coordinate System and Basis Vectors](#1.2 Plueker Coordinate System and Basis Vectors)
  • [1.3 Work with Moving Reference Frame](#1.3 Work with Moving Reference Frame)
  • [1.4 Derivative of Adjoint](#1.4 Derivative of Adjoint)
  • • [1.4.1 Spatial Cross Product](#1.4.1 Spatial Cross Product)
    • [1.4.2 Spatial Acceleration with Moving Reference Frame](#1.4.2 Spatial Acceleration with Moving Reference Frame)
• [2. Spatial Force(Wrench)](#2. Spatial Force(Wrench))
• • [2.1 Spatial Force in Pluecker Coordinate Systems](#2.1 Spatial Force in Pluecker Coordinate Systems)
  • [2.2 Wrench-Twist Pair and Power](#2.2 Wrench-Twist Pair and Power)
  • [2.3 Joint Torque](#2.3 Joint Torque)
• [3. Spatial Momentum](#3. Spatial Momentum)
• • [3.1 Rotational Interial](#3.1 Rotational Interial)
  • [3.2 Change Reference for Momentum](#3.2 Change Reference for Momentum)
  • [3.3 Spatial Inertia](#3.3 Spatial Inertia)
• [4. Newton-Euler Equation using Spatial Vectors](#4. Newton-Euler Equation using Spatial Vectors)
• • [4.1 Cross Product for Spatial Force and Momentum](#4.1 Cross Product for Spatial Force and Momentum)
  • [4.2 Newton-Euler Equation](#4.2 Newton-Euler Equation)
  • [4.3 Derivations of Newton-Euler Equation](#4.3 Derivations of Newton-Euler Equation)

---

1. Spatial Vecocity

Given a rigid body with spatial velocity V = ( ω ⃗ , v ⃗ ) \mathcal{V} =\left( \vec{\omega},\vec{v} \right) V=(ω ,v ) , its spatial acceleration (coordinate-free)
A = V ˙ = [ ω ⃗ ˙ v ⃗ ˙ O ] , A = lim ⁡ δ → 0 V ( t + δ ) − V ( t ) δ \mathcal{A} =\dot{\mathcal{V}}=\left[ \begin{array}{c} \dot{\vec{\omega}}\\ \dot{\vec{v}}_{\mathrm{O}}\\ \end{array} \right] ,\mathcal{A} =\underset{\delta \rightarrow 0}{\lim}\frac{\mathcal{V} \left( t+\delta \right) -\mathcal{V} \left( t \right)}{\delta} A=V˙=[ω ˙v ˙O],A=δ→0limδV(t+δ)−V(t)

Recall that: v ⃗ O \vec{v}_{\mathrm{O}} v O i sthe velocity of the body-fixed particle coincident with frame origin o o o at the current time t t t

Note : ω ⃗ ˙ \dot{\vec{\omega}} ω ˙ is the angular acceleration of the body

v ⃗ ˙ O \dot{\vec{v}}_{\mathrm{O}} v ˙O is not the acceleration of any body-fixed point ! v ⃗ O = R ⃗ ˙ q ( t ) , v ⃗ ˙ O ≠ R ⃗ ¨ q ( t ) \vec{v}_{\mathrm{O}}=\dot{\vec{R}}q\left( t \right) ,\dot{\vec{v}}{\mathrm{O}}\ne \ddot{\vec{R}}_q\left( t \right) v O=R ˙q(t),v ˙O=R ¨q(t)

In face, v ⃗ ˙ O \dot{\vec{v}}_{\mathrm{O}} v ˙O gives the rate of change in stream velocity of body-fixed particles passing through o o o

1.1 Spatial vs. Conventional Accel

Suppose R ⃗ q ( t ) \vec{R}_q\left( t \right) R q(t) is the body fixed particle coincides with o o o at time t t t

So by definition , we have v ⃗ O ( t ) = R ⃗ ˙ q ( t ) \vec{v}_{\mathrm{O}}\left( t \right) =\dot{\vec{R}}q\left( t \right) v O(t)=R ˙q(t) , however v ⃗ ˙ O ≠ R ⃗ ¨ q ( t ) \dot{\vec{v}}{\mathrm{O}}\ne \ddot{\vec{R}}_q\left( t \right) v ˙O=R ¨q(t) , where R ⃗ ¨ q ( t ) \ddot{\vec{R}}_q\left( t \right) R ¨q(t) is the conventional acceleration of the body-fixed point q q q

At time t t t : R ⃗ q ( t ) = 0 \vec{R}q\left( t \right) =0 R q(t)=0 , v ⃗ O ( t ) = R ⃗ ˙ q ( t ) \vec{v}{\mathrm{O}}\left( t \right) =\dot{\vec{R}}_q\left( t \right) v O(t)=R ˙q(t)

At time t + δ t+\delta t+δ : R ⃗ q ′ ( t ) = 0 \vec{R}{q^{\prime}}\left( t \right) =0 R q′(t)=0 , v ⃗ O ( t + δ ) =    R ⃗ ˙ q ′ ( t + δ ) ≠ R ⃗ ˙ q ( t + δ ) \vec{v}{\mathrm{O}}\left( t+\delta \right) =\,\,\dot{\vec{R}}_{q^{\prime}}\left( t+\delta \right) \ne \dot{\vec{R}}_q\left( t+\delta \right) v O(t+δ)=R ˙q′(t+δ)=R ˙q(t+δ) ------ q ′ q^{\prime} q′ another body-fixed particle

• Note : q q q and q ′ q^{\prime} q′ are different points, lim ⁡ δ → 0 v ⃗ O ( t ) = v ⃗ O ( t + δ ) − v ⃗ O ( t ) δ = R ⃗ ˙ q ′ ( t + δ ) − R ⃗ q ( t ) δ \underset{\delta \rightarrow 0}{\lim}\vec{v}{\mathrm{O}}\left( t \right) =\frac{\vec{v}{\mathrm{O}}\left( t+\delta \right) -\vec{v}{\mathrm{O}}\left( t \right)}{\delta}=\frac{\dot{\vec{R}}{q^{\prime}}\left( t+\delta \right) -\vec{R}_q\left( t \right)}{\delta} δ→0limv O(t)=δv O(t+δ)−v O(t)=δR ˙q′(t+δ)−R q(t)

实际上只需考虑Twist最开始的定义，即速度 v ⃗ O \vec{v}_{\mathrm{O}} v O 并不是某一点的速度，而是考虑相对坐标系原点而言的虚拟点在该角速度下的瞬时速度（ R ⃗ ˙ q ( t ) = v ⃗ O ( t ) + ω ⃗ ( t ) × R ⃗ q ( t ) \dot{\vec{R}}q\left( t \right) =\vec{v}{\mathrm{O}}\left( t \right) +\vec{\omega}\left( t \right) \times \vec{R}_q\left( t \right) R ˙q(t)=v O(t)+ω (t)×R q(t)），而与该坐标系所代表的真实点的运动无关( R ⃗ q ( t ) \vec{R}_q\left( t \right) R q(t) is the body fixed particle coincides with o o o at time t t t)，即为：
R ⃗ ¨ q ( t ) = v ⃗ ˙ O ( t ) + ω ⃗ ˙ ( t ) × R ⃗ q ( t ) ↗ 0 + ω ⃗ ( t ) × R ⃗ ˙ q ( t ) = v ⃗ ˙ O ( t ) + ω ⃗ ( t ) × R ⃗ ˙ q ( t ) \ddot{\vec{R}}q\left( t \right) =\dot{\vec{v}}{\mathrm{O}}\left( t \right) +\dot{\vec{\omega}}\left( t \right) \times \vec{R}_q\left( t \right) _{\nearrow 0}+\vec{\omega}\left( t \right) \times \dot{\vec{R}}q\left( t \right) =\dot{\vec{v}}{\mathrm{O}}\left( t \right) +\vec{\omega}\left( t \right) \times \dot{\vec{R}}_q\left( t \right) R ¨q(t)=v ˙O(t)+ω ˙(t)×R q(t)↗0+ω (t)×R ˙q(t)=v ˙O(t)+ω (t)×R ˙q(t)

1.2 Plueker Coordinate System and Basis Vectors

按照向量的本质理解即可，这也是笔者为啥不是很喜欢旋量的原因。

Recall coordinate-free concept: let R ⃗ ∈ R 3 \vec{R}\in \mathbb{R} ^3 R ∈R3 be a free vector with { O } \left\{ O \right\} {O} and { B } \left\{ B \right\} {B} frame coordinate R ⃗ O \vec{R}^O R O and R ⃗ B \vec{R}^B R B

矢量的变换：

旋量的变换：

e B 1 O e B 2 O e B 3 O e B 4 O e B 4 O e B 5 O \] 6 × 6 = \[ X B O \] = \[ A d \[ T B O \] \] \\left\[ \\begin{array}{l} e_{\\mathrm{B}1}\^{O}\& e_{\\mathrm{B}2}\^{O}\& e_{\\mathrm{B}3}\^{O}\& e_{\\mathrm{B}4}\^{O}\& e_{\\mathrm{B}4}\^{O}\& e_{\\mathrm{B}5}\^{O}\\\\ \\end{array} \\right\] _{6\\times 6}=\\left\[ X_{\\mathrm{B}}\^{O} \\right\] =\\left\[ Ad_{\\left\[ T_{\\mathrm{B}}\^{O} \\right\]} \\right\] \[eB1OeB2OeB3OeB4OeB4OeB5O\]6×6=\[XBO\]=\[Ad\[TBO\]

1.3 Work with Moving Reference Frame

Now let's work with { O } \left\{ O \right\} {O} frame to find the derivative ------ we need to compute : [ e ˙ B 1 O e ˙ B 2 O e ˙ B 3 O e ˙ B 4 O e ˙ B 4 O e ˙ B 5 O ] 6 × 6 = [ X ˙ B O ] = d d t [ A d [ T B O ] ] \left[ \begin{array}{l} \dot{e}{\mathrm{B}1}^{O}& \dot{e}{\mathrm{B}2}^{O}& \dot{e}{\mathrm{B}3}^{O}& \dot{e}{\mathrm{B}4}^{O}& \dot{e}{\mathrm{B}4}^{O}& \dot{e}{\mathrm{B}5}^{O}\\ \end{array} \right] {6\times 6}=\left[ \dot{X}{\mathrm{B}}^{O} \right] =\frac{\mathrm{d}}{\mathrm{d}t}\left[ Ad_{\left[ T_{\mathrm{B}}^{O} \right]} \right] [e˙B1Oe˙B2Oe˙B3Oe˙B4Oe˙B4Oe˙B5O]6×6=[X˙BO]=dtd[Ad[TBO]]

Let's denote : [ T B O ] = ( [ Q ] , R ⃗ ) ⇒ d d t ( [ [ Q ] 0 R ⃗ ~ [ Q ] [ Q ] ] ) = [ [ Q ˙ ] 0 ( R ⃗ ~ [ Q ] ) ′ [ Q ˙ ] ] \left[ T_{\mathrm{B}}^{O} \right] =\left( \left[ Q \right] ,\vec{R} \right) \Rightarrow \frac{\mathrm{d}}{\mathrm{d}t}\left( \left[ \begin{matrix} \left[ Q \right]& 0\\ \tilde{\vec{R}}\left[ Q \right]& \left[ Q \right]\\ \end{matrix} \right] \right) =\left[ \begin{matrix} \left[ \dot{Q} \right]& 0\\ \left( \tilde{\vec{R}}\left[ Q \right] \right) ^{\prime}& \left[ \dot{Q} \right]\\ \end{matrix} \right] [TBO]=([Q],R )⇒dtd([[Q]R ~[Q]0[Q]])= [Q˙](R ~[Q])′0[Q˙]

{ B } \left\{ B \right\} {B} frame has instantaneous velocity V B = [ ω ⃗ v ⃗ O ] \mathcal{V} B=\left[ \begin{array}{c} \vec{\omega}\\ \vec{v}{\mathrm{O}}\\ \end{array} \right] VB=[ω v O]

1.4 Derivative of Adjoint

Note : [ Q ˙ ] = ω ⃗ × [ Q ] , R ⃗ ˙ = v ⃗ O + ω ⃗ × R ⃗ , [ Q ] ω ⃗ ~ = [ Q ] ω ⃗ ~ [ Q ] T , ω ⃗ 1 × ω ⃗ 2 ~ = ω ⃗ ~ 1 ω ⃗ ~ 2 − ω ⃗ ~ 2 ω ⃗ ~ 1 \left[ \dot{Q} \right] =\vec{\omega}\times \left[ Q \right] ,\dot{\vec{R}}=\vec{v}_{\mathrm{O}}+\vec{\omega}\times \vec{R},\widetilde{\left[ Q \right] \vec{\omega}}=\left[ Q \right] \tilde{\vec{\omega}}\left[ Q \right] ^{\mathrm{T}},\widetilde{\vec{\omega}_1\times \vec{\omega}_2}=\tilde{\vec{\omega}}_1\tilde{\vec{\omega}}_2-\tilde{\vec{\omega}}_2\tilde{\vec{\omega}}_1 [Q˙]=ω ×[Q],R ˙=v O+ω ×R ,[Q]ω =[Q]ω ~[Q]T,ω 1×ω 2 =ω ~1ω ~2−ω ~2ω ~1(Jacobi's Identity)

After some computation :
d d t [ A d [ T B O ] ] = [ ω ⃗ ~ 0 v ⃗ ~ O ω ⃗ ~ ] [ A d [ T B O ] ] = [ X ˙ B O ] \frac{\mathrm{d}}{\mathrm{d}t}\left[ Ad_{\left[ T_{\mathrm{B}}^{O} \right]} \right] =\left[ \begin{matrix} \tilde{\vec{\omega}}& 0\\ \tilde{\vec{v}}{\mathrm{O}}& \tilde{\vec{\omega}}\\ \end{matrix} \right] \left[ Ad{\left[ T_{\mathrm{B}}^{O} \right]} \right] =\left[ \dot{X}_{\mathrm{B}}^{O} \right] dtd[Ad[TBO]]=[ω ~v ~O0ω ~][Ad[TBO]]=[X˙BO]

Define : [ ω ⃗ ~ 0 v ⃗ ~ O ω ⃗ ~ ] = V ~ B \left[ \begin{matrix} \tilde{\vec{\omega}}& 0\\ \tilde{\vec{v}}{\mathrm{O}}& \tilde{\vec{\omega}}\\ \end{matrix} \right] =\tilde{\mathcal{V}}B [ω ~v ~O0ω ~]=V~B
{ [ Q ˙ B O ] = ω ⃗ ~ B [ Q B O ] [ X ˙ B O ] = V ~ B [ X ˙ B O ] \begin{cases} \left[ \dot{Q}{\mathrm{B}}^{O} \right] =\tilde{\vec{\omega}}B\left[ Q{\mathrm{B}}^{O} \right]\\ \left[ \dot{X}{\mathrm{B}}^{O} \right] =\tilde{\mathcal{V}}B\left[ \dot{X}{\mathrm{B}}^{O} \right]\\ \end{cases} ⎩ ⎨ ⎧[Q˙BO]=ω ~B[QBO][X˙BO]=V~B[X˙BO]

In coordinate free: e ˙ B 1 O = V ~ B e B 1 O \dot{e}_{\mathrm{B}1}^{O}=\tilde{\mathcal{V}}Be{\mathrm{B}1}^{O} e˙B1O=V~BeB1O

1.4.1 Spatial Cross Product

Given two spatial velocities(twists) V 1 \mathcal{V} _1 V1 and V 2 \mathcal{V} _2 V2 , their spatial product is
V 1 × V 2 = [ ω ⃗ 1 v ⃗ 1 O ] × [ ω ⃗ 2 v ⃗ 2 O ] = [ ω ⃗ 1 × ω ⃗ 2 ω ⃗ 1 × v ⃗ 2 O + v ⃗ 1 O × ω ⃗ 2 ] \mathcal{V} _1\times \mathcal{V} _2=\left[ \begin{array}{c} \vec{\omega}_1\\ {\vec{v}1}{\mathrm{O}}\\ \end{array} \right] \times \left[ \begin{array}{c} \vec{\omega}_2\\ {\vec{v}2}{\mathrm{O}}\\ \end{array} \right] =\left[ \begin{array}{c} \vec{\omega}_1\times \vec{\omega}_2\\ \vec{\omega}_1\times {\vec{v}2}{\mathrm{O}}+{\vec{v}1}{\mathrm{O}}\times \vec{\omega}_2\\ \end{array} \right] V1×V2=[ω 1v 1O]×[ω 2v 2O]=[ω 1×ω 2ω 1×v 2O+v 1O×ω 2]

Matrix representation : V 1 × V 2 = V ~ 1 V 2 , V ~ 1 = [ ω ⃗ ~ 1 0 v ⃗ ~ 1 O ω ⃗ ~ 1 ] \mathcal{V} _1\times \mathcal{V} _2=\tilde{\mathcal{V}}_1\mathcal{V} _2,\tilde{\mathcal{V}}_1=\left[ \begin{matrix} \tilde{\vec{\omega}}_1& 0\\ {\tilde{\vec{v}}1}{\mathrm{O}}& \tilde{\vec{\omega}}_1\\ \end{matrix} \right] V1×V2=V~1V2,V~1=[ω ~1v ~1O0ω ~1]

Roughly speaking, when a motion V \mathcal{V} V is moving with a spatial velocity Z \mathcal{Z} Z (e.g. it is attached to a moving frame) but is otherwise not changing , then
V ˙ = Z × V \dot{\mathcal{V}}=\mathcal{Z} \times \mathcal{V} V˙=Z×V

• Propertries

Assume A is moving wrt O O O with velocity V A \mathcal{V} {\mathrm{A}} VA : [ X ˙ A O ] = V ~ A O [ X A O ] \left[ \dot{X}{\mathrm{A}}^{O} \right] =\tilde{\mathcal{V}}{\mathrm{A}}^{O}\left[ X{\mathrm{A}}^{O} \right] [X˙AO]=V~AO[XAO]

X \] V \~ = \[ X \] V \~ \[ X \] T \\widetilde{\\left\[ X \\right\] \\mathcal{V} }=\\left\[ X \\right\] \\tilde{\\mathcal{V}}\\left\[ X \\right\] \^{\\mathrm{T}} \[X\]V =\[X\]V\~\[X\]T for any transformation \[ X \] \\left\[ X \\right\] \[X\] and twist V \\mathcal{V} V #### 1.4.2 Spatial Acceleration with Moving Reference Frame Consider a body with velocity V B o d y \\mathcal{V} _{\\mathrm{Body}} VBody (wrt inertia frame), and V B o d y O \\mathcal{V} _{\\mathrm{Body}}\^{O} VBodyO and V B o d y B \\mathcal{V} _{\\mathrm{Body}}\^{B} VBodyB be its Plueker coordinates wrt { O } \\left\\{ O \\right\\} {O} and { B } \\left\\{ B \\right\\} {B} : A B o d y B = d d t ( V B o d y B ) + V \~ B O B V B o d y B \\mathcal{A} _{\\mathrm{Body}}\^{B}=\\frac{\\mathrm{d}}{\\mathrm{d}t}\\left( \\mathcal{V} _{\\mathrm{Body}}\^{B} \\right) +\\tilde{\\mathcal{V}}_{\\mathrm{BO}}\^{B}\\mathcal{V} _{\\mathrm{Body}}\^{B} ABodyB=dtd(VBodyB)+V\~BOBVBodyB A B o d y O = \[ X B O \] A B o d y B \\mathcal{A} _{\\mathrm{Body}}\^{O}=\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\mathcal{A} _{\\mathrm{Body}}\^{B} ABodyO=\[XBO\]ABodyB > A B o d y O = d d t ( V B o d y O ) = d d t ( \[ X B O \] V B o d y B ) = \[ X ˙ B O \] V B o d y B + \[ X B O \] V ˙ B o d y B = V \~ B O \[ X B O \] V B o d y B + \[ X B O \] V ˙ B o d y B = \[ X B O \] ( \[ X O B \] V \~ B O \[ X B O \] V B o d y B + V ˙ B o d y B ) = \[ X B O \] ( \[ X O B \] V B O \~ V B o d y B + V ˙ B o d y B ) = \[ X B O \] ( V \~ B O B V B o d y B + V ˙ B o d y B ) = \[ X B O \] A B o d y B \\mathcal{A} _{\\mathrm{Body}}\^{O}=\\frac{\\mathrm{d}}{\\mathrm{d}t}\\left( \\mathcal{V} _{\\mathrm{Body}}\^{O} \\right) =\\frac{\\mathrm{d}}{\\mathrm{d}t}\\left( \\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\mathcal{V} _{\\mathrm{Body}}\^{B} \\right) =\\left\[ \\dot{X}_{\\mathrm{B}}\^{O} \\right\] \\mathcal{V} _{\\mathrm{Body}}\^{B}+\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\dot{\\mathcal{V}}_{\\mathrm{Body}}\^{B}=\\tilde{\\mathcal{V}}_{\\mathrm{B}}\^{O}\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\mathcal{V} _{\\mathrm{Body}}\^{B}+\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\dot{\\mathcal{V}}_{\\mathrm{Body}}\^{B}=\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\left( \\left\[ X_{\\mathrm{O}}\^{B} \\right\] \\tilde{\\mathcal{V}}_{\\mathrm{B}}\^{O}\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\mathcal{V} _{\\mathrm{Body}}\^{B}+\\dot{\\mathcal{V}}_{\\mathrm{Body}}\^{B} \\right) =\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\left( \\widetilde{\\left\[ X_{\\mathrm{O}}\^{B} \\right\] \\mathcal{V} _{\\mathrm{B}}\^{O}}\\mathcal{V} _{\\mathrm{Body}}\^{B}+\\dot{\\mathcal{V}}_{\\mathrm{Body}}\^{B} \\right) =\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\left( \\tilde{\\mathcal{V}}_{\\mathrm{BO}}\^{B}\\mathcal{V} _{\\mathrm{Body}}\^{B}+\\dot{\\mathcal{V}}_{\\mathrm{Body}}\^{B} \\right) =\\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\mathcal{A} _{\\mathrm{Body}}\^{B} ABodyO=dtd(VBodyO)=dtd(\[XBO\]VBodyB)=\[X˙BO\]VBodyB+\[XBO\]V˙BodyB=V\~BO\[XBO\]VBodyB+\[XBO\]V˙BodyB=\[XBO\](\[XOB\]V\~BO\[XBO\]VBodyB+V˙BodyB)=\[XBO\](\[XOB\]VBO VBodyB+V˙BodyB)=\[XBO\](V\~BOBVBodyB+V˙BodyB)=\[XBO\]ABodyB **EXAMPLE:**  ## 2. Spatial Force(Wrench) Consider a rigid body with many forces on it and fix an arbitrary point O O O in space  The net effect of these forces can be expressed as: * A force f f f , acting along a line passing through O O O ------ f ⃗ = ∑ f ⃗ i i \\vec{f}=\\sum{\\vec{f}_{\\mathrm{i}}}_{\\mathrm{i}} f =∑f ii * A moment m ⃗ O \\vec{m}_{\\mathrm{O}} m O about point O O O ------ m ⃗ O = ∑ R ⃗ P i O × f ⃗ i \\vec{m}_{\\mathrm{O}}=\\sum{\\vec{R}_{\\mathrm{Pi}}\^{O}\\times \\vec{f}_{\\mathrm{i}}} m O=∑R PiO×f i > **Spatial Force(Wrench)** : is given by the 6D vector > F = \[ m ⃗ O f ⃗ \] \\mathcal{F} =\\left\[ \\begin{array}{c} \\vec{m}_{\\mathrm{O}}\\\\ \\vec{f}\\\\ \\end{array} \\right\] F=\[m Of

What is we choose reference point to Q Q Q?
m ⃗ Q = ∑ R ⃗ P i Q × f ⃗ i = ∑ ( R ⃗ O Q + R ⃗ P i O ) × f ⃗ i = m ⃗ O + ∑ R ⃗ O Q × f ⃗ i \vec{m}{\mathrm{Q}}=\sum{\vec{R}{\mathrm{Pi}}^{Q}\times \vec{f}{\mathrm{i}}}=\sum{\left( \vec{R}{\mathrm{O}}^{Q}+\vec{R}{\mathrm{Pi}}^{O} \right) \times \vec{f}{\mathrm{i}}}=\vec{m}{\mathrm{O}}+\sum{\vec{R}{\mathrm{O}}^{Q}\times \vec{f}_{\mathrm{i}}} m Q=∑R PiQ×f i=∑(R OQ+R PiO)×f i=m O+∑R OQ×f i

2.1 Spatial Force in Pluecker Coordinate Systems

Given a frame { A } \left\{ A \right\} {A}, the Plueker coordinate of a spatial force F \mathcal{F} F is given by F A = [ m ⃗ O A f ⃗ A ] \mathcal{F} ^A=\left[ \begin{array}{c} \vec{m}_{\mathrm{O}}^{A}\\ \vec{f}^A\\ \end{array} \right] FA=[m OAf A]

Coordinate transform :
{ f ⃗ A = [ Q B A ] f ⃗ B m ⃗ O A = [ Q B A ] m ⃗ O B + R ⃗ B A × [ Q B A ] f ⃗ B ⇒ F A = [ X B A ] T F B = [ X B A ] ∗ F B \begin{cases} \vec{f}^A=\left[ Q_{\mathrm{B}}^{A} \right] \vec{f}^B\\ \vec{m}{\mathrm{O}}^{A}=\left[ Q{\mathrm{B}}^{A} \right] \vec{m}{\mathrm{O}}^{B}+\vec{R}{\mathrm{B}}^{A}\times \left[ Q_{\mathrm{B}}^{A} \right] \vec{f}^B\\ \end{cases}\Rightarrow \mathcal{F} ^A=\left[ X_{\mathrm{B}}^{A} \right] ^{\mathrm{T}}\mathcal{F} ^B=\left[ X_{\mathrm{B}}^{A} \right] ^*\mathcal{F} ^B {f A=[QBA]f Bm OA=[QBA]m OB+R BA×[QBA]f B⇒FA=[XBA]TFB=[XBA]∗FB

2.2 Wrench-Twist Pair and Power

Recall that for a point mass with linear velocity v ⃗ \vec{v} v and a linear force f ⃗ \vec{f} f . Then we know that the power (instantaneous work done by f ⃗ \vec{f} f ) is given by : f ⃗ ⋅ v ⃗ = f ⃗ T v ⃗ \vec{f}\cdot \vec{v}=\vec{f}^{\mathrm{T}}\vec{v} f ⋅v =f Tv

This relation can be generalized to spatial force (i.e. wrench) and spatial velocity (i.e. twist)

Suppose a rigid body has a twist V A = ( ω ⃗ A , v ⃗ O A ) \mathcal{V} ^A=\left( \vec{\omega}^A,\vec{v}{\mathrm{O}}^{A} \right) VA=(ω A,v OA) and a wrench F A = ( m ⃗ O A , f ⃗ A ) \mathcal{F} ^A=\left( \vec{m}{\mathrm{O}}^{A},\vec{f}^A \right) FA=(m OA,f A) acts on the body. Then the power is simply
P = ( V A ) T F A = ( F A ) T V A = ( ω ⃗ A ) T m ⃗ O A + ( v ⃗ O A ) T f ⃗ A P=\left( \mathcal{V} ^A \right) ^{\mathrm{T}}\mathcal{F} ^A=\left( \mathcal{F} ^A \right) ^{\mathrm{T}}\mathcal{V} ^A=\left( \vec{\omega}^A \right) ^{\mathrm{T}}\vec{m}{\mathrm{O}}^{A}+\left( \vec{v}{\mathrm{O}}^{A} \right) ^{\mathrm{T}}\vec{f}^A P=(VA)TFA=(FA)TVA=(ω A)Tm OA+(v OA)Tf A

2.3 Joint Torque

Consider a link attached to a 1-dof joint(r.g. revolute or prismatic). be the screw axis of the joint. Then the power produced by the joint is V = S ^ θ ˙ \mathcal{V} =\hat{\mathcal{S}}\dot{\theta} V=S^θ˙

F \mathcal{F} F be the wrench provided by the joint. Then the power produced by the joint is P = ( V ) T F = ( S ^ θ ˙ ) T F = ( S ^ T F ) θ ˙ = τ θ ˙ P=\left( \mathcal{V} \right) ^{\mathrm{T}}\mathcal{F} =\left( \hat{\mathcal{S}}\dot{\theta} \right) ^{\mathrm{T}}\mathcal{F} =\left( \hat{\mathcal{S}}^{\mathrm{T}}\mathcal{F} \right) \dot{\theta}=\tau \dot{\theta} P=(V)TF=(S^θ˙)TF=(S^TF)θ˙=τθ˙

τ = S ^ T F = F T S ^ \tau =\hat{\mathcal{S}}^{\mathrm{T}}\mathcal{F} =\mathcal{F} ^{\mathrm{T}}\hat{\mathcal{S}} τ=S^TF=FTS^ is the projection of the wrench onto the screw axis, i.e. the effective part of the wrench

Often times, τ \tau τ is referred to as joint "torque" or generalized force

3. Spatial Momentum

笔者待整理： 链接

3.1 Rotational Interial

• Recall momentum for point mass:

笔者待整理： 链接

H = [ h ⃗ p ⃗ ] ∈ R 6 \mathcal{H} =\left[ \begin{array}{c} \vec{h}\\ \vec{p}\\ \end{array} \right] \in \mathbb{R} ^6 H=[h p ]∈R6

3.2 Change Reference for Momentum

• Spatial momentum transforms in the same way as spatial forces:
  H A = [ X C A ] ∗ H C \mathcal{H} ^A=\left[ X_{\mathrm{C}}^{A} \right] ^*\mathcal{H} ^C HA=[XCA]∗HC
  H C = [ h ⃗ B o d y / C C p ⃗ C ] , H A = [ h ⃗ A A p ⃗ A ] = [ [ Q C A ] h ⃗ B o d y / C C − R ⃗ ~ C A [ Q C A ] p ⃗ C [ Q C A ] p ⃗ C ] = [ [ Q C A ] − R ⃗ ~ C A [ Q C A ] 0 [ Q C A ] ] [ h ⃗ B o d y / C C p ⃗ C ] = [ X C A ] ∗ [ h ⃗ B o d y / C C p ⃗ C ] \mathcal{H} ^C=\left[ \begin{array}{c} \vec{h}{\mathrm{Body}/\mathrm{C}}^{C}\\ \vec{p}^C\\ \end{array} \right] ,\mathcal{H} ^A=\left[ \begin{array}{c} \vec{h}{\mathrm{A}}^{A}\\ \vec{p}^A\\ \end{array} \right] =\left[ \begin{array}{c} \left[ Q_{\mathrm{C}}^{A} \right] \vec{h}{\mathrm{Body}/\mathrm{C}}^{C}-\tilde{\vec{R}}{\mathrm{C}}^{A}\left[ Q_{\mathrm{C}}^{A} \right] \vec{p}^C\\ \left[ Q_{\mathrm{C}}^{A} \right] \vec{p}^C\\ \end{array} \right] =\left[ \begin{matrix} \left[ Q_{\mathrm{C}}^{A} \right]& -\tilde{\vec{R}}{\mathrm{C}}^{A}\left[ Q{\mathrm{C}}^{A} \right]\\ 0& \left[ Q_{\mathrm{C}}^{A} \right]\\ \end{matrix} \right] \left[ \begin{array}{c} \vec{h}{\mathrm{Body}/\mathrm{C}}^{C}\\ \vec{p}^C\\ \end{array} \right] =\left[ X{\mathrm{C}}^{A} \right] ^*\left[ \begin{array}{c} \vec{h}_{\mathrm{Body}/\mathrm{C}}^{C}\\ \vec{p}^C\\ \end{array} \right] HC=[h Body/CCp C],HA=[h AAp A]=[[QCA]h Body/CC−R ~CA[QCA]p C[QCA]p C]=[[QCA]0−R ~CA[QCA][QCA]][h Body/CCp C]=[XCA]∗[h Body/CCp C]

3.3 Spatial Inertia

Inertia of a rigid body defines linear relationship between velocity and momentum

Spacial inertia I \mathcal{I} I is the one such that
H = I V \mathcal{H} =\mathcal{I} \mathcal{V} H=IV

Let { M } \left\{ M \right\} {M} be a frame whose origin coincide with CoM. Then
I B o d y / C o M M = [ I B o d y / C o M M 0 0 m t o t a l E 3 × 3 ] G \mathcal{I} {\mathrm{Body}/\mathrm{CoM}}^{M}=\left[ \begin{matrix} I{\mathrm{Body}/\mathrm{CoM}}^{M}& 0\\ 0& m_{\mathrm{total}}E_{3\times 3}\\ \end{matrix} \right] G IBody/CoMM=[IBody/CoMM00mtotalE3×3]G

• Spatial inertia wrt another frame { F } \left\{ F \right\} {F}:
  I F = [ X M F ] ∗ I M [ X F M ] \mathcal{I} ^F=\left[ X_{\mathrm{M}}^{F} \right] ^*\mathcal{I} ^M\left[ X_{\mathrm{F}}^{M} \right] IF=[XMF]∗IM[XFM]

Special case : [ Q F M ] = E 3 × 3 \left[ Q_{\mathrm{F}}^{M} \right] =E_{3\times 3} [QFM]=E3×3

X M F \] = \[ E 3 × 3 0 R ⃗ \~ M F E 3 × 3 \] ⇒ I F = \[ I M + m t o t a l R ⃗ \~ M F T R ⃗ \~ M F m t o t a l R ⃗ \~ M F m t o t a l R ⃗ \~ M F m t o t a l E 3 × 3 \] \\left\[ X_{\\mathrm{M}}\^{F} \\right\] =\\left\[ \\begin{matrix} E_{3\\times 3}\& 0\\\\ \\tilde{\\vec{R}}_{\\mathrm{M}}\^{F}\& E_{3\\times 3}\\\\ \\end{matrix} \\right\] \\Rightarrow \\mathcal{I} \^F=\\left\[ \\begin{matrix} \\mathcal{I} \^M+m_{\\mathrm{total}}{\\tilde{\\vec{R}}_{\\mathrm{M}}\^{F}}\^{\\mathrm{T}}\\tilde{\\vec{R}}_{\\mathrm{M}}\^{F}\& m_{\\mathrm{total}}\\tilde{\\vec{R}}_{\\mathrm{M}}\^{F}\\\\ m_{\\mathrm{total}}\\tilde{\\vec{R}}_{\\mathrm{M}}\^{F}\& m_{\\mathrm{total}}E_{3\\times 3}\\\\ \\end{matrix} \\right\] \[XMF\]=\[E3×3R \~MF0E3×3\]⇒IF= IM+mtotalR \~MFTR \~MFmtotalR \~MFmtotalR \~MFmtotalE3×3

4. Newton-Euler Equation using Spatial Vectors

4.1 Cross Product for Spatial Force and Momentum

Assume frame A A A is moving with velocity V A A \mathcal{V} _{\mathrm{A}}^{A} VAA
( d d t F ) A = d d t F A + V A × ∗ F A \left( \frac{\mathrm{d}}{\mathrm{d}t}\mathcal{F} \right) ^A=\frac{\mathrm{d}}{\mathrm{d}t}\mathcal{F} ^A+\mathcal{V} ^A\times ^*\mathcal{F} ^A (dtdF)A=dtdFA+VA×∗FA
( d d t H ) A = d d t H A + V A × ∗ H A \left( \frac{\mathrm{d}}{\mathrm{d}t}\mathcal{H} \right) ^A=\frac{\mathrm{d}}{\mathrm{d}t}\mathcal{H} ^A+\mathcal{V} ^A\times ^*\mathcal{H} ^A (dtdH)A=dtdHA+VA×∗HA

where × ∗ \times ^* ×∗ defined as V = [ ω ⃗ v ⃗ ] , F = [ m ⃗ f ⃗ ] , V × ∗ F = [ ω ⃗ ~ m ⃗ + v ⃗ ~ f ⃗ ω ⃗ ~ f ⃗ ] \mathcal{V} =\left[ \begin{array}{c} \vec{\omega}\\ \vec{v}\\ \end{array} \right] ,\mathcal{F} =\left[ \begin{array}{c} \vec{m}\\ \vec{f}\\ \end{array} \right] ,\mathcal{V} \times ^*\mathcal{F} =\left[ \begin{array}{c} \tilde{\vec{\omega}}\vec{m}+\tilde{\vec{v}}\vec{f}\\ \tilde{\vec{\omega}}\vec{f}\\ \end{array} \right] V=[ω v ],F=[m f ],V×∗F=[ω ~m +v ~f ω ~f ], or equivately V × ∗ ~ = [ ω ⃗ ~ v ⃗ ~ 0 ω ⃗ ~ ] \widetilde{\mathcal{V} \times ^*}=\left[ \begin{matrix} \tilde{\vec{\omega}}& \tilde{\vec{v}}\\ 0& \tilde{\vec{\omega}}\\ \end{matrix} \right] V×∗ =[ω ~0v ~ω ~]

Fact : V × ∗ ~ = V ~ T \widetilde{\mathcal{V} \times ^*}=\tilde{\mathcal{V}}^{\mathrm{T}} V×∗ =V~T

4.2 Newton-Euler Equation

• Newton-Euler equation :
  F = d d t H = I A + V ~ T I V \mathcal{F} =\frac{\mathrm{d}}{\mathrm{d}t}\mathcal{H} =\mathcal{I} \mathcal{A} +\tilde{\mathcal{V}}^{\mathrm{T}}\mathcal{I} \mathcal{V} F=dtdH=IA+V~TIV
  (due to velocity is changing and account for the face that inertia is moving)

Adopting spatial vectors, the Newton-Euler equation has the same form in any frame

4.3 Derivations of Newton-Euler Equation

d d t H O = d d t ( I O V O ) = I ˙ O V O + I O A O = d d t ( [ X B O ] ∗ I B [ X O B ] ) V O + I O A O = [ X ˙ B O ] ∗ I B [ X O B ] V O + [ X B O ] ∗ I B [ X ˙ O B ] V O + I O A O = V ~ B O T [ X B O ] ∗ I B [ X O B ] V O − [ X B O ] ∗ I B [ X O B ] V ~ B O T V O ↗ 0 + I O A O = V ~ B O T I O V O + I O A O \frac{\mathrm{d}}{\mathrm{d}t}\mathcal{H} ^O=\frac{\mathrm{d}}{\mathrm{d}t}\left( \mathcal{I} ^O\mathcal{V} ^O \right) =\dot{\mathcal{I}}^O\mathcal{V} ^O+\mathcal{I} ^O\mathcal{A} ^O=\frac{\mathrm{d}}{\mathrm{d}t}\left( \left[ X_{\mathrm{B}}^{O} \right] ^*\mathcal{I} ^B\left[ X_{\mathrm{O}}^{B} \right] \right) \mathcal{V} ^O+\mathcal{I} ^O\mathcal{A} ^O \\ =\left[ \dot{X}{\mathrm{B}}^{O} \right] ^*\mathcal{I} ^B\left[ X{\mathrm{O}}^{B} \right] \mathcal{V} ^O+\left[ X_{\mathrm{B}}^{O} \right] ^*\mathcal{I} ^B\left[ \dot{X}{\mathrm{O}}^{B} \right] \mathcal{V} ^O+\mathcal{I} ^O\mathcal{A} ^O \\ ={\tilde{\mathcal{V}}{\mathrm{B}}^{O}}^{\mathrm{T}}\left[ X_{\mathrm{B}}^{O} \right] ^*\mathcal{I} ^B\left[ X_{\mathrm{O}}^{B} \right] \mathcal{V} ^O-\left[ X_{\mathrm{B}}^{O} \right] ^*\mathcal{I} ^B\left[ X_{\mathrm{O}}^{B} \right] {\tilde{\mathcal{V}}{\mathrm{B}}^{O}}^{\mathrm{T}}{\mathcal{V} ^O}{\nearrow 0}+\mathcal{I} ^O\mathcal{A} ^O \\ ={\tilde{\mathcal{V}}_{\mathrm{B}}^{O}}^{\mathrm{T}}\mathcal{I} ^O\mathcal{V} ^O+\mathcal{I} ^O\mathcal{A} ^O dtdHO=dtd(IOVO)=I˙OVO+IOAO=dtd([XBO]∗IB[XOB])VO+IOAO=[X˙BO]∗IB[XOB]VO+[XBO]∗IB[X˙OB]VO+IOAO=V~BOT[XBO]∗IB[XOB]VO−[XBO]∗IB[XOB]V~BOTVO↗0+IOAO=V~BOTIOVO+IOAO

Note :
{ [ X ˙ B O ] = V ~ B O [ X B O ] [ X B O ] [ X O B ] = E ⇒ [ X ˙ B O ] [ X O B ] + [ X B O ] [ X ˙ O B ] = 0 ⇒ [ X ˙ O B ] = − [ X O B ] [ X ˙ B O ] [ X O B ] = − [ X O B ] V ~ B O \begin{cases} \left[ \dot{X}{\mathrm{B}}^{O} \right] =\tilde{\mathcal{V}}{\mathrm{B}}^{O}\left[ X_{\mathrm{B}}^{O} \right]\\ \left[ X_{\mathrm{B}}^{O} \right] \left[ X_{\mathrm{O}}^{B} \right] =E\\ \end{cases}\Rightarrow \left[ \dot{X}{\mathrm{B}}^{O} \right] \left[ X{\mathrm{O}}^{B} \right] +\left[ X_{\mathrm{B}}^{O} \right] \left[ \dot{X}{\mathrm{O}}^{B} \right] =0\Rightarrow \left[ \dot{X}{\mathrm{O}}^{B} \right] =-\left[ X_{\mathrm{O}}^{B} \right] \left[ \dot{X}{\mathrm{B}}^{O} \right] \left[ X{\mathrm{O}}^{B} \right] =-\left[ X_{\mathrm{O}}^{B} \right] \tilde{\mathcal{V}}_{\mathrm{B}}^{O} {[X˙BO]=V~BO[XBO][XBO][XOB]=E⇒[X˙BO][XOB]+[XBO][X˙OB]=0⇒[X˙OB]=−[XOB][X˙BO][XOB]=−[XOB]V~BO

Frame B is attached to the body , V B = V B o d y , I B \mathcal{V} _B=\mathcal{V} _{Body},\mathcal{I} ^B VB=VBody,IB is constant