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 0.1.0, rung dynamic