# Node DynamicArmNode

Defined in `mime.nodes.actuation._dynamic.DynamicArmNode`.

Algorithm `MIME-NODE-109`.

A dynamic chain segment: joint torques in, the mass matrix by CRBA and the bias by RNEA with gravity and the loads on its frames, implicit joint damping, rigid joint stops and Coulomb friction as impulses through the step's system by projected Gauss-Seidel, and every frame's world pose by forward kinematics.

## Governing equations

$$
\mathbf{M}_{s} = \mathbf{M}(\theta_{n}; \mathbf{C}_{0}), \quad \mathbf{M}_{e} = \mathbf{M}(\theta_{n}; \mathbf{C}_{1}), \quad \mathbf{G} = \sum_{k} \mathbf{S}_{k}^{\top} [\mathbf{H}_{k}]_{\times} \mathbf{S}_{k}, \quad \dot{\theta}^{\prime} = (\mathbf{M}_{s} - \tfrac{1}{2} \mathrm{d}t \, \mathbf{G})^{-1} (\mathbf{M}_{s} + \tfrac{1}{2} \mathrm{d}t \, \mathbf{G}) \, \dot{\theta}_{n}; \quad \mathbf{A} = \mathbf{M}_{e} + \mathrm{d}t \, \mathbf{D}, \quad \dot{\theta}^{*} = \mathbf{A}^{-1} \left( \mathbf{M}_{s} \dot{\theta}^{\prime} + \mathrm{d}t \, (\mathbf{u} - \mathbf{h}(\theta_{n}, \dot{\theta}^{\prime}; \mathbf{g}, \mathbf{W}, \mathbf{C}_{0})) \right); \quad \dot{\theta}_{n+1} = \arg\min_{v} \tfrac{1}{2} (v - \dot{\theta}^{*})^{\top} \mathbf{A} \, (v - \dot{\theta}^{*}) + \mathrm{d}t \sum_{j} \mu_{j} |v_{j}| \ \text{with} \ l \le \theta_{n} + \mathrm{d}t \, v \le h, \ \text{by projected Gauss-Seidel sweeps}; \quad \theta_{n+1} = \min(\max(\theta_{n} + \mathrm{d}t \, \dot{\theta}_{n+1}, l), h); \quad \mathbf{P}_{k} = \mathbf{O} \circ \mathrm{FK}_{k}(\theta_{n+1}; \mathbf{a}, \mathbf{J}, \mathbf{K}), \quad \boldsymbol{\Omega}_{k} = R(\mathbf{O}) \, \mathrm{W}_{k}(\theta_{n+1}, \dot{\theta}_{n+1})
$$

## Discretisation

Semi-implicit Euler, first order in time, after the gyroscopic torque of the spin momenta on its frames by a Cayley step (Crank-Nicolson on a skew matrix, which does no work): the velocity from the forces at the start of the step at the velocity that step gives, the joint damping taken at its end, the inertia its frames carry at the start and at the end, the stops' and friction's impulses on the step's system; then the position from the new velocity.

## Assumptions

- Rigid joints and links, with no structural mode (MIME-ANO-101)
- Its base is fixed in the world
- A joint limit is a rigid, inelastic stop: a joint driven past it arrives at it at the end of the step, and the stop takes the rest of its velocity on the next
- The stops' and friction's impulses are solved by a fixed count of projected Gauss-Seidel sweeps: exact for one joint and where every joint with friction sticks, converged on coupled joints as SWEEPS states, and never adding kinetic energy

## Limitations

none declared

## Declared regimes (not evidenced here)

none declared

## References

[@Featherstone2008], [@Sciavicco2000]

## Symbols

| Symbol | Kind | Name | Units | Implemented by |
|---|---|---|---|---|
| $\mathbf{u}$ | input | `joint_torques` | N m | `mime.nodes.actuation._dynamic.DynamicArmNode.driving` |
| $\mathbf{W}$ | input | `link_wrench` | N; N m | `mime.nodes.actuation._dynamic.DynamicArmNode.loads` |
| $\mathbf{H}$ | input | `link_momentum` | N m s | `mime.nodes.actuation._dynamic.DynamicArmNode.spinning` |
| $\mathbf{C}$ | input | `link_inertia` | kg m^2 | `mime.nodes.actuation._dynamic.DynamicArmNode.carrying` |
| $\mathbf{g}$ | parameter | `gravity` | m/s^2 | `mime.nodes.actuation._dynamic.DynamicArmNode.weight` |
| $\mathbf{D}$ | parameter | `damping` | N m s | `mime.nodes.actuation._dynamic.DynamicArmNode.advance` |
| $\mu$ | parameter | `friction` | N m | `mime.nodes.actuation._dynamic.DynamicArmNode.constrained` |
| $l$ | parameter | `lower` | 1 | `mime.nodes.actuation._dynamic.DynamicArmNode.constrained` |
| $h$ | parameter | `upper` | 1 | `mime.nodes.actuation._dynamic.DynamicArmNode.constrained` |
| $\theta$ | state | `joint_positions` | 1 | `mime.nodes.actuation._dynamic.DynamicArmNode.advance` |
| $\dot{\theta}$ | state | `joint_velocities` | rad/s | `mime.nodes.actuation._dynamic.DynamicArmNode.advance` |
| $\mathbf{P}$ | state | `frame_poses` | m; 1 | `mime.nodes.actuation._dynamic.DynamicArmNode.advance` |
| $\boldsymbol{\Omega}$ | state | `frame_angular_velocities` | rad/s | `mime.nodes.actuation._dynamic.DynamicArmNode.advance` |
| $\mathbf{O}$ | parameter | `placement` | m; 1 | `mime.nodes.actuation._chain.MountedNode.base` |
| $\mathbf{a}$ | parameter | `axes` | 1 | `mime.nodes.actuation._dynamic.DynamicArmNode.articulation` |
| $\mathbf{J}$ | parameter | `origin_translations` | m | `mime.nodes.actuation._dynamic.DynamicArmNode.articulation` |
| $\mathbf{K}$ | parameter | `frame_translations` | m | `mime.nodes.actuation._dynamic.DynamicArmNode.articulation` |

## Built by

- [`arm`](../entries/dynamics/arm.md) 0.1.0, rung `dynamic`