本文仅供学习使用
本文参考：
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~BOFrame 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