example.md

Acceleration

The linear acceleration of frame $A$ with respect to a universal frame is given as

$$\dot{v}{a}={}^{U}\dot{V}{AORG}$$

and the angular acceleration is

$$\dot{\omega}{a}=\ ^{U}\dot{\Omega}{AORG}$$

Deriving this equation is hell, so I'll just state the solution. Refer to page.166 of Craig's Intro to Robotics for derivation:

$${}^{A}\dot{V}{Q}={}^{A}\dot{V}{BORG}+{}^{A}\Omega_{B}\times({}^{A}V_{B}\times{}^{A}_{B}R{}^{B}Q)+{}^{A}\dot{\Omega}B\times{}^{A}{B}R{}^{B}Q$$

The "similar" equation for angular velocity is:

$${}^{A}\dot{\Omega}{C}={}^{A}\dot{\Omega}{B}+{}^{A}{B}R\ {}^{B}\dot{\Omega}{C} + {}^{A}\Omega_{B}\times{}^{A}{B}R\ {}^{B}\Omega{C}$$

These can be used along with the equations for velocity to generate iterative equations for linear and rotational acceleration:

$$^{i+1}\dot{\omega}{i+1}={}^{i+1}{i}{R}{}^{i}\dot{\omega}{i}+{}^{i+1}{i}{R}{}^{i}{\omega}{i}\times \dot{\theta}{i+1}{}^{i+1}\hat{Z}{i+1}+\ddot{\theta}{i+1}{}^{i+1}\hat{Z}{i+1}$$ $$\text{(Rot:) }{}^{i+1}\dot{v}{i+1}={}^{i+1}{i}{R}({}^{i}\dot{\omega}{i}\times {}^{i}{P}{i+1}+{}^{i}{\omega}{i}\times({}^{i}{\omega}{i}\times {}^{i}{P}{i+1})+{}^{i}\dot{v}{i})$$ $$\begin{align*} ^{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({}^{i+1}{\omega}{i+1}\times {}^{i+1}{P}{C{i+1}})+{}^{i+1}\dot{v}_{i+1} \end{align*} $$

Forces and mass

Along with acceleration, the Newtonian model for dynamics also involves masses and forces, culminating in the iconic Newton's second law, suited better to our needs:

$$F_{i}=ma=m\dot{v}{C{i}}$$

When dealing with rigid bodies, we usually consider their inertia's to take into account the shape of their bodies when using them in equations. The mass distribution of a rigid body can be given by an inertia tensor:

$${}^{A}I=\begin{matrix} I_{xx} & -I_{xy} & -I_{xz} \ -I_{yx} & I_{yy} & -I_{yz} \ -I_{zx} & -I_{zy} & I_{zz}\end{matrix}$$ $$I_{xx}=\iiint_{V}{(y^{2}+z^{2})}\rho\ dv$$ $$I_{yy}=\iiint_{V}{(x^{2}+z^{2})}\rho\ dv$$ $$I_{zz}=\iiint_{V}{(x^{2}+y^{2})}\rho\ dv$$ $$I_{xy}=\iiint_{V}{xy}\rho\ dv$$ $$I_{xz}=\iiint_{V}{xz}\rho\ dv$$ $$I_{yz}=\iiint_{V}{yz}\rho\ dv$$

We can use this tensor in Euler's rotation equation, where $N$ is the applied torque to the rigid body:

$$N={}^{A}I\dot{\omega}+\omega\times{}^{A}I\omega$$

This can give us the recursive equations for force and torque applied to the frames {$i$} and {$i+1$}:

$${}^{i+1}{F}{i+1}={m}{i+1}{}^{i+1}\dot{v}{C{i+1}}$$ $${}^{i+1}{N}{i+1}={}^{i+1}{I}{i+1}{}^{i+1}\dot{\omega}{i+1}+{}^{i+1}{\omega}{i+1}\times {}^{i+1}{I}{i+1}{}^{i+1}{\omega}{i+1}$$ $${}^{i}{f}{i}={}^{i}{i+1}{R}{}^{i+1}{f}{i+1}+{}^{i}{F}{i}$$ $${}^{i}{n}{i}={}^{i}{N}{i}+{}^{i}{i+1}{R}{}^{i+1}{n}{i+1}+{}^{i}{P}{C{i}}\times {}^{i}{F}{i}+{}^{i}{P}{i+1}\times {}^{i}{i+1}{R}{}^{i+1}{f}{i+1}$$

To account for gravity, we just assume the entire base frame accelerates up at the same magnitude, which adds the effect with no additional computation cost: $^{0}\dot{v}_{0}=g$

Finally, we use these force and moment equations to find a system for applied torque ${}^{i}{\uptau}_{i}$. For revolute joint {$i$}:

$$\uptau_{i}={}^{i}{n}{i}^{T}{}^{i}\hat{Z}{i}$$

And for prismatic joint {$i$}:

$$\uptau_i={}^{i}{f}{i}^{T}{}^{i}\hat{Z}{i}$$

State-space representation

After deriving the dynamic equations for a manipulator's torques, we can simplify these into matrices giving us the state-space representation:

$$\uptau=M(\Theta)\ddot{\Theta}+V(\Theta,\dot{\Theta})+G(\Theta)$$

Where the mass matrix contains $n$ columns equal to the number of joint variables, and $n$ rows equal to the number of torque equations. The entries of the matrix are the coefficients of the double-differential of the joint variables, in a $n\times{n}$ matrix.

The velocity matrix is an $n\times{1}$ vector where each row contains the terms involving the velocities of any joint variables. The gravity matrix is similarly an $n\times{1}$ vector of all terms involving gravity. In the velocity matrix, the terms that are dependant on the square of any joint velocity are known as centrifugal forces, whereas those dependant on the product of two joint velocities are known as Coriolis forces.

We can then rewrite this form in a configuration-space equation, which has terms dependant only on the joint positions, making it less computationally expensive:

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

where $B(\Theta)$ contains the Coriolis forces and $C(\Theta)$ the centrifugal forces.