Dynamics and Control

Dynamics

All force and torque acting on the system can be represented as a single vector in the generalized velocity space. This representation is called generalized force $\mathit{\tau }$. Just like in a Cartesian coordinate (i.e., x, y, z axes), the power exerted by an articulated system is computed as a dot product of generalized force and generalized velocity (i.e., $\mathit{u}\cdot \mathit{\tau }$).

We can also combine the mass and inertia of the whole articulated system and represent them in a single matrix. This matrix is called mass matrix or inertia matrix and denoted by $\mathit{M}$. A mass matrix represents how much the articulated system resists change in generalized velocities. Naively speaking, a large mass matrix means that the articulated system experiences a low velocity change for a given generalized force.

The total kinetic energy of the system is computed as $\frac{1}{2}{\mathit{u}}^T\mathit{M}\mathit{u}$. This quantity can be obtained by getKineticEnergy().

The total potential energy due to the gravity is a sum of $mgz$ (mass, gravitational acceleration and height) for all bodies. This quantity can be obtained by getPotentialEnergy(gravity). The gravity vector must be passed because only the world stores it (e.g., world.getGravity()).

The equation of motion of an articulated system is shown below:

$$\mathit{\tau }=\mathit{M}(\mathit{q})\dot{\mathit{u}}+\mathit{h}(\mathit{q},\mathit{u}).$$

Here $\mathit{h}$ is called a non-linear term. There are three sources of force that contribute to the non-linear term: gravity, Coriolis, and centrifugal force. It is rarely useful to compute the gravity contribution to the nonlinear term alone. If it is needed, evaluate the nonlinear term at zero generalized velocity, e.g., on a copy of the robot in another world: then the Coriolis and centrifugal contributions are zero.

The following methods are used to obtain dynamic quantities

• getMassMatrix() (it includes the rotor inertia on its diagonal)
• getNonlinearities(gravity)
• getInverseMassMatrix() (call getMassMatrix() first)

Inverse Dynamics

RaiSim can compute inverse dynamics using the recursive Newton-Euler algorithm. It is the only option for computing the force and torque acting at joints. Joint force/torque are the sum of the constraint joint force/torque and actuation force/torque. For example, a revolute joint constrains motions in 5 degrees of freedom, which means that there are 5-dimensional constraint forces/torque and 1-dimensional joint actuation torque acting at a revolute joint.

In minimal coordinate simulation (such as RaiSim), these constraint forces/torques are not computed in the simulation loop. These forces/torques can be computed after a simulation loop using the inverse dynamics pipeline.

To enable inverse dynamics, call raisim::ArticulatedSystem::setComputeInverseDynamics(true). This flag is set automatically if the robot has an IMU sensor. Note that the inverse dynamics pipeline will slow down the simulation by about 10%.

After a simulation loop, you can call raisim::ArticulatedSystem::getForceAtJointInWorldFrame() and raisim::ArticulatedSystem::getTorqueAtJointInWorldFrame() to get forces and torques acting at the specified joint.

Assuming that there are no joint position/velocity limit forces acting at the joint, you can compute the joint actuation as a dot product of the joint axis and the joint torque. The inverse_dynamics example (Server Example: Inverse Dynamics) does this for ANYmal and compares the result with the applied generalized force.

PD Controller

When naively implemented, a PD controller can often make a robot unstable. However, this is often less problematic for robotics since this instability is also present in real systems (discrete-time control systems).

For other applications like animation and graphics, it is often desirable to have a PD controller that stays stable with a large time step. Therefore, the built-in PD controller is integrated implicitly and stays stable with much larger time steps and gains than a naive implementation.

The built-in PD torque and the feedforward generalized force are clamped together to the joint actuation limits (URDF effort or setActuationLimits()). Joint damping and actuator torques are added after this clamp and are not clipped by it. Because PD is integrated implicitly, the clamp bounds only its explicit part, and getGeneralizedForce() reports the applied PD torque only approximately (see Joint torque of a simulation step). The built-in PD torque is not an actuator torque, so the motor operating regions of actuators do not clip it (see Actuators below).

To use this PD controller, set the desired control gains first:

Eigen::VectorXd pGain(robot->getDOF()), dGain(robot->getDOF());
pGain<< ...; // set your proportional gain values here
dGain<< ...; // set your differential gain values here
robot->setPdGains(pGain, dGain);

Note that the dimension of the pGain vector is the same as that of the generalized velocity NOT that of the coordinate. For a floating base, the first six gains are forced to zero. setPdGains() also switches the control mode to ControlMode::PD_PLUS_FEEDFORWARD_TORQUE.

Finally, the target position and the velocity can be set as follows:

Eigen::VectorXd pTarget(robot->getGeneralizedCoordinateDim()), vTarget(robot->getDOF());
pTarget<< ...; // set your position target
vTarget<< ...; // set your velocity target
robot->setPdTarget(pTarget, vTarget);

Here, the dimension of the pTarget vector is the same as that of the generalized coordinate NOT that of the velocity. This can be confusing and may seem inconsistent. However, this is a valid convention. The only reason the two dimensions differ is the quaternion representation. The quaternion target is represented by a quaternion whereas the virtual spring stiffness between the two orientations can be represented by a 3D vector, which is composed of motions in each angular velocity components.

A feedforward force term can be added by setGeneralizedForce() if desired. This term is set to zero by default. Note that this value is stored in the class instance and does not change unless the user specifies it so. If this feedforward force should be applied for a single time step, set it to zero in the subsequent control loop (after the integrate() call of the world).

The theory of the implemented PD controller can be found in chapter 1.2 of this article. This document is intended for advanced users and is not required to use RaiSim.

Actuators

The URDF effort limit is a constant bound per joint on the feedforward generalized force plus the built-in PD torque; joint damping is passive and is not clipped. Actuator torques set with setActuatorTorque() or setActuatorTorques() are clipped separately, to the motor operating regions (MOR) of their motors, mapped to the joints (with their couplings), and added after the effort clamp. Effort limits do not clip actuator torques, and MOR does not clip setGeneralizedForce() or the built-in PD controller. The combined generalized force is not clamped again. To keep a PD controller inside the motor operating regions, compute its torques explicitly and send them with setActuatorTorques(). See Actuators.

Apply External Forces/Torques

The following two methods are used to apply external force and torque respectively

• setExternalForce
• setExternalTorque

You will find the above methods in the ArticulatedSystem API.

Joint torque of a simulation step

This section gives exactly what a simulation step of length $\mathrm{\Delta }t$ computes for an articulated system. Bold symbols are vectors with one entry per generalized velocity (one per revolute or prismatic joint, three per spherical joint and six for a floating base), and a product of two such vectors is taken entry by entry. ${\mathit{q}}_t$ and ${\mathit{u}}_t$ are the generalized coordinate and velocity at the beginning of the step, and ${\mathit{u}}_{t+1}$ is the velocity at its end. For a spherical joint, ${\mathit{q}}_{ref}-{\mathit{q}}_t$ stands for the rotation vector of ${\mathit{q}}_t^{-1}{\mathit{q}}_{ref}$, the shorter rotation (an angle of at most $\pi$): a quaternion and its negative are the same orientation, as the joint state or as the target.

1. The command, clamped to the actuation limits.

$$\begin{array}{rl}{\mathit{\tau }}_{PD}& ={\mathit{k}}_p({\mathit{q}}_{ref}-{\mathit{q}}_t)+{\mathit{k}}_d({\mathit{u}}_{ref}-{\mathit{u}}_t)\\ {\mathit{\tau }}_{cmd}& =\mathrm{clamp}({\mathit{\tau }}_{ff}+{\mathit{\tau }}_{PD},{\mathit{\tau }}_{lower},{\mathit{\tau }}_{upper})\end{array}$$

${\mathit{\tau }}_{ff}$ is the force set with setGeneralizedForce(), ${\mathit{k}}_p$ and ${\mathit{k}}_d$ are the gains of setPdGains(), and ${\mathit{q}}_{ref}$ and ${\mathit{u}}_{ref}$ are the targets of setPdTarget(). In the FORCE_AND_TORQUE control mode, ${\mathit{k}}_p={\mathit{k}}_d=0$ here and in step 4. The bounds are $\mp$ the URDF effort or those of setActuationLimits(); a joint without them is not clamped.

2. The actuator torques. Take an actuator with the gear ratio $G$, the driven joint $j$, the couplings $(c,r_c)$ and the torque ${\tau }_a$ set with setActuatorTorques() (see Actuators). Its motor has the stall torque ${\tau }_{stall}=K_t{V}_{bus}/R$, the back-EMF damping $b=K_t/(R{K}_v)$ and the peak torque ${\tau }_{peak}$. With $u_{t,j}$ the entry of ${\mathit{u}}_t$ for joint $j$:

$$\begin{array}{rl}{\omega }_m& =G{u}_{t,j}+\sum _c{r}_c{u}_{t,c}\\ {\tau }_{m,min}& =\mathrm{clamp}(-{\tau }_{stall}-b{\omega }_m,-{\tau }_{peak},{\tau }_{peak})\\ {\tau }_{m,max}& =\mathrm{clamp}(\phantom{-}{\tau }_{stall}-b{\omega }_m,-{\tau }_{peak},{\tau }_{peak})\\ {\tau }_m& =\mathrm{clamp}({\tau }_a/G,{\tau }_{m,min},{\tau }_{m,max})\end{array}$$

The actuator adds $G{\tau }_m$ to joint $j$ and $r_c{\tau }_m$ to each coupled joint $c$, and ${\mathit{\tau }}_{act}$ is the sum over all actuators. With setMotorOperatingRegionEnforced(false), ${\tau }_m={\tau }_a/G$. The actuation limits do not clip actuator torques.

3. The explicit torque. Everything evaluated at the beginning of the step is summed, and the sum is not clamped:

$${\mathit{\tau }}_{exp}={\mathit{\tau }}_{cmd}-({\mathit{b}}_{joint}+{\mathit{b}}_{out}){\mathit{u}}_t+{\mathit{\tau }}_{act}+{\mathit{k}}_s({\mathit{q}}_s-{\mathit{q}}_t)+{\mathit{J}}^T{\mathit{F}}_{ext}$$

${\mathit{b}}_{joint}$ is the joint damping (URDF damping or setJointDamping(), see Joint Damping and Friction) and ${\mathit{b}}_{out}$ the output damping of the joint's actuator. ${\mathit{k}}_s$ and ${\mathit{q}}_s$ are the stiffness and the rest position of the joint springs (URDF <dynamics stiffness spring_mount> or addSpring(), see getSprings()). ${\mathit{J}}^T{\mathit{F}}_{ext}$ are the forces and torques of setExternalForce(), setExternalTorque() and setConstraintForce(), mapped through their Jacobians.

4. The velocity. The PD gains, the damping and the springs are integrated implicitly: the diagonal $\mathit{w}$ is added to the mass matrix $\mathit{M}$ (getMassMatrix(), which includes the rotor inertia). With the nonlinear term $\mathit{h}$ (gravity, Coriolis and centrifugal forces, getNonlinearities()), the articulated-body algorithm computes the velocity without contacts, ${\mathit{u}}^{\ast }$, and the contact solver adds the impulses ${\mathit{\lambda }}_c$ of the contacts, the joint limits and the joint friction through their Jacobian ${\mathit{J}}_c$ and the same matrix:

$$\begin{array}{rl}\mathit{w}& =\frac{\mathrm{\Delta }t}{2}({\mathit{b}}_{joint}+{\mathit{b}}_{out}+{\mathit{k}}_d)+\frac{\mathrm{\Delta }{t}^2}{4}({\mathit{k}}_p+{\mathit{k}}_s)\\ \tilde{\mathit{M}}& =\mathit{M}+\mathrm{diag}(\mathit{w})\\ {\mathit{u}}^{\ast }& ={\mathit{u}}_t+\mathrm{\Delta }t{\tilde{\mathit{M}}}^{-1}({\mathit{\tau }}_{exp}-\mathit{h})\\ {\mathit{u}}_{t+1}& ={\mathit{u}}^{\ast }+{\tilde{\mathit{M}}}^{-1}{\mathit{J}}_c^T{\mathit{\lambda }}_c\end{array}$$

A position-limit impulse acts on a joint outside its range. A velocity-limit impulse acts on a joint whose entry of ${\mathit{u}}^{\ast }$ exceeds its velocity limit, unless a position-limit impulse acts on that joint. A joint-friction impulse acts on every revolute or prismatic joint with friction (see Joint Damping and Friction).

5. The applied joint torque. With $\mathrm{\Delta }\mathit{u}={\mathit{u}}_{t+1}-{\mathit{u}}_t$, moving the implicit part to the right-hand side gives the equation of motion of the step:

$$\mathit{M}\frac{\mathrm{\Delta }\mathit{u}}{\mathrm{\Delta }t}+\mathit{h}={\mathit{\tau }}_{exp}-\mathit{w}\frac{\mathrm{\Delta }\mathit{u}}{\mathrm{\Delta }t}+\frac{{\mathit{J}}_c^T{\mathit{\lambda }}_c}{\mathrm{\Delta }t}$$

The torque on the joints over the step is therefore, term by term:

Source	Torque over the step
feedforward and PD	${\mathit{\tau }}_{cmd}-(\frac{1}{2}{\mathit{k}}_d+\frac{\mathrm{\Delta }t}{4}{\mathit{k}}_p)\mathrm{\Delta }\mathit{u}$
damping	$-\frac{1}{2}({\mathit{b}}_{joint}+{\mathit{b}}_{out})({\mathit{u}}_t+{\mathit{u}}_{t+1})$ (trapezoidal rule)
actuators	${\mathit{\tau }}_{act}$, constant over the step
springs	${\mathit{k}}_s({\mathit{q}}_s-{\mathit{q}}_t)-\frac{\mathrm{\Delta }t}{4}{\mathit{k}}_s\mathrm{\Delta }\mathit{u}$
external forces	${\mathit{J}}^T{\mathit{F}}_{ext}$
joint friction	${\mathit{\lambda }}_f/\mathrm{\Delta }t$, with $|{\mathit{\lambda }}_f|\le ({\mathit{\tau }}_{c,joint}+{\mathit{\tau }}_{c,out})\mathrm{\Delta }t$
contacts and joint limits	the rest of ${\mathit{J}}_c^T{\mathit{\lambda }}_c/\mathrm{\Delta }t$

The friction impulse ${\mathit{\lambda }}_f$, the joints' share of ${\mathit{J}}_c^T{\mathit{\lambda }}_c$, stops a joint if the bound allows it, and otherwise opposes the motion with the bound. ${\mathit{\tau }}_{c,joint}$ is the joint friction (see Joint Damping and Friction) and ${\mathit{\tau }}_{c,out}$ the output friction of the joint's actuator.

Only the explicit part of the feedforward and PD torque is clamped. The implicit part is not, so the applied PD torque can exceed the actuation limits by $(\frac{1}{2}{\mathit{k}}_d+\frac{\mathrm{\Delta }t}{4}{\mathit{k}}_p)|\mathrm{\Delta }\mathit{u}|$, e.g., when an impact changes the velocity within one step. Without the clamp, the feedforward and PD torque is

$${\mathit{\tau }}_{ff}+{\mathit{k}}_p({\mathit{q}}_{ref}-{\mathit{q}}_t-\frac{\mathrm{\Delta }t}{4}\mathrm{\Delta }\mathit{u})+{\mathit{k}}_d({\mathit{u}}_{ref}-\frac{1}{2}({\mathit{u}}_t+{\mathit{u}}_{t+1}))$$

6. The position. The position is integrated with the average velocity $\overline{\mathit{u}}$ of the scheme set with setIntegrationScheme() (a quaternion with the same angular velocity):

$${\mathit{q}}_{t+1}={\mathit{q}}_t+\mathrm{\Delta }t\overline{\mathit{u}}$$

Integration scheme	$\overline{\mathit{u}}$
SEMI_IMPLICIT (default)	${\mathit{u}}_{t+1}$
TRAPEZOID	$\frac{1}{2}({\mathit{u}}_t+{\mathit{u}}_{t+1})$
EULER	${\mathit{u}}_t$

The torque is the same for the three. SEMI_IMPLICIT moves the robot with the velocity the contact solver leaves, so a foot that an impact stops stays at the surface. TRAPEZOID follows a body under a constant force exactly (a thrown ball), but under forces that change with the state (gravity on a joint, springs, the PD torque, the gyroscopic force) it gains energy: an undamped pendulum's swing grows by about 9% in 20 s at $\mathrm{\Delta }t$ = 1 ms, where SEMI_IMPLICIT keeps it within 0.1%. On an impact, TRAPEZOID also goes into the ground by up to half the step's travel. RUNGE_KUTTA_4 applies the velocity change of the contact solver at the beginning of the step and then evaluates steps 1-4 without contacts at each of its four stages, at the state of the stage; the actuator bounds use the motor speeds of the stage, and getActuatorStates() describes the last stage. In the PD control mode, SEMI_IMPLICIT replaces EULER and RUNGE_KUTTA_4 once the robot has been in contact (from the step after its first contact), until its state is set again with setState(), setGeneralizedCoordinate() or an applied solveIK() solution. SEMI_IMPLICIT also replaces RUNGE_KUTTA_4 while the robot has loop or mimic constraints, which are eliminated from the velocity of one step (see Closed-Loop Systems).

getGeneralizedForce() returns ${\mathit{\tau }}_{cmd}+{\mathit{\tau }}_{act}$ at the current state: the PD term with the position error of the last step and the current velocity, and the actuator bounds at the current motor speeds. It does not include the damping, the springs, the external forces, the implicit terms or the solver impulses.

Kinematic loops

An articulated system is simulated in the generalized coordinates of a kinematic tree. A loop constraint closes a loop of the tree again: it ties an anchor point on body1 to the coincident point on body2. A pin holds the two points together in all three directions. An equality constraint holds them together only along one or two axes fixed in body1; along the remaining directions the points slide freely. A mimic constraint couples two joint coordinates linearly. The three are handled in the same way. This section derives the dynamics with the equality constraint as the main case; Closed-Loop Systems describes how to model loops and how they behave, and Mimic Joints the mimic constraints. The symbols are those of Joint torque of a simulation step above.

The constraints are not iterated by the contact solver. In every step, each articulated system eliminates them exactly, before the contacts are solved:

1. it takes the velocity ${\mathit{u}}^{\ast }$ that the system would reach without them (step 4 above),
2. applies the one impulse that makes that velocity satisfy them, and
3. replaces the response of everything else that acts on the system (contacts, joint limits, joint friction, tendons) by its response on the constrained mechanism.

The contact solver then works on a smaller problem whose every solution already satisfies the constraints.

Notation

Symbol	Meaning
$\mathit{q},\mathit{u}$	generalized coordinate and velocity of the tree ($|\mathit{u}|$ entries; see State and Kinematics)
$\mathit{M},\tilde{\mathit{M}}$	the mass matrix, and the mass matrix with the implicit diagonal of step 4
${\mathit{p}}_1,{\mathit{p}}_2$	world positions of the anchor on body1 and on body2
${\mathit{a}}_i$	constrained unit axes of an equality constraint in the world frame. They are fixed in body1's frame $B_1$, where they are ${}^{B_1}{\mathit{a}}_i$.
${\mathit{v}}_b(\mathit{x}),{\mathit{\omega }}_b$	velocity of the material point of body $b$ at the world point $\mathit{x}$, and the angular velocity of body $b$
${\mathit{J}}_b(\mathit{x})$	Jacobian of that point, ${\mathit{J}}_b(\mathit{x})\mathit{u}={\mathit{v}}_b(\mathit{x})$ (see State and Kinematics)
$\mathit{J},\mathit{\lambda }$	all constraint rows stacked ($|\mathit{\lambda }|\times |\mathit{u}|$), and their impulses
${\mathit{J}}_c,{\mathit{\lambda }}_c$	the rows and impulses of the contacts, the joint limits and the joint friction (step 4)
$\mathit{e}$	the position errors of the rows
$\mathit{D}=\mathit{J}{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T$	the coupling of the rows (their inverse effective mass, the Delassus matrix of the constraints), $|\mathit{\lambda }|\times |\mathit{\lambda }|$
$\mathrm{\Delta }t,\beta$	the time step and the error-correction rate, $\beta =\mathrm{clamp}(0.2\mathrm{erp},0,0.3)/\mathrm{\Delta }t$ (see Closed-Loop Systems)

The constraint and its rate

For each of its axes, the equality constraint requires

$$e_i(\mathit{q})={\mathit{a}}_i\cdot ({\mathit{p}}_1-{\mathit{p}}_2)=0.$$

A pin uses three fixed world directions as its axes ${\mathit{a}}_i$. A mimic constraint uses $e=q_{\mathrm{follower}}-m{q}_{\mathrm{leader}}-c$ for the multiplier $m$ and the offset $c$ (see Mimic Joints).

The exact rate of an equality row. The step is solved for velocities, so each row needs the time derivative of its error, linear in the generalized velocity $\mathit{u}$. Take one axis $\mathit{a}$, with $e=\mathit{a}\cdot ({\mathit{p}}_1-{\mathit{p}}_2)$. Three things move:

• the axis: it is fixed in body1, so it turns with body1's angular velocity, $\dot{\mathit{a}}={\mathit{\omega }}_1\times \mathit{a}$;
• body1's anchor: ${\mathit{p}}_1$ is a material point of body1, so ${\dot{\mathit{p}}}_1={\mathit{v}}_1({\mathit{p}}_1)$;
• body2's anchor: ${\mathit{p}}_2$ is a material point of body2, so ${\dot{\mathit{p}}}_2={\mathit{v}}_2({\mathit{p}}_2)$.

One more fact is needed, the velocity field of a rigid body. The material point of body1 that currently sits at a world point $\mathit{x}$ moves with ${\mathit{v}}_1(\mathit{x})={\mathit{v}}_1(\mathit{y})+{\mathit{\omega }}_1\times (\mathit{x}-\mathit{y})$ for any other point $\mathit{y}$: two points of the same body differ in velocity only by the rotation.

1. The product rule gives $\dot{e}=\dot{\mathit{a}}\cdot ({\mathit{p}}_1-{\mathit{p}}_2)+\mathit{a}\cdot ({\dot{\mathit{p}}}_1-{\dot{\mathit{p}}}_2)$.

2. Inserting the three rates, the first term is the axis turning and the second the anchors moving:

   $$\dot{e}=\underset{\text{A: the axis turns}}{\underbrace{({\mathit{\omega }}_1\times \mathit{a})\cdot ({\mathit{p}}_1-{\mathit{p}}_2)}}+\underset{\text{B: the anchors move}}{\underbrace{\mathit{a}\cdot ({\mathit{v}}_1({\mathit{p}}_1)-{\mathit{v}}_2({\mathit{p}}_2))}}.$$

3. Move body1's velocity from ${\mathit{p}}_1$ to ${\mathit{p}}_2$. By the velocity field, ${\mathit{v}}_1({\mathit{p}}_1)={\mathit{v}}_1({\mathit{p}}_2)+{\mathit{\omega }}_1\times ({\mathit{p}}_1-{\mathit{p}}_2)$, so term B splits into

   $$\text{B}=\mathit{a}\cdot ({\mathit{v}}_1({\mathit{p}}_2)-{\mathit{v}}_2({\mathit{p}}_2))+\mathit{a}\cdot ({\mathit{\omega }}_1\times ({\mathit{p}}_1-{\mathit{p}}_2)).$$

4. The scalar triple product is invariant under a cyclic permutation and changes sign when two factors are swapped, so the last term is $-({\mathit{\omega }}_1\times \mathit{a})\cdot ({\mathit{p}}_1-{\mathit{p}}_2)$: term A with the opposite sign.

5. Term A and the rest of term B cancel:

   $$\dot{e}=\mathit{a}\cdot ({\mathit{v}}_1({\mathit{p}}_2)-{\mathit{v}}_2({\mathit{p}}_2))=\underset{\text{a row of }\mathit{J}}{\underbrace{{\mathit{a}}^T({\mathit{J}}_1({\mathit{p}}_2)-{\mathit{J}}_2({\mathit{p}}_2))}}\mathit{u}.$$

Nothing was approximated: the rate holds at every instant, however far the anchors have slid apart. An equality constraint with two axes has one such row per axis.

The same result, seen from body1. In body1's frame, ${}^{B_1}\mathit{a}$ and ${}^{B_1}{\mathit{p}}_1$ are constant, so $e$ changes only through the motion of body2's anchor relative to body1. That relative velocity is body2's velocity at the anchor minus the velocity of the point of body1 that coincides with it, ${\mathit{v}}_2({\mathit{p}}_2)-{\mathit{v}}_1({\mathit{p}}_2)$, which gives $\dot{e}=\mathit{a}\cdot ({\mathit{v}}_1({\mathit{p}}_2)-{\mathit{v}}_2({\mathit{p}}_2))$ again. An equality constraint says that body2's anchor stays on a plane (one axis) or a line (two axes) fixed in body1; what matters is how that anchor moves relative to body1, measured where the anchor is.

Body1's own anchor would give a wrong row. Evaluating body1 at its own anchor keeps only term B, ${\mathit{a}}^T({\mathit{J}}_1({\mathit{p}}_1)-{\mathit{J}}_2({\mathit{p}}_2))\mathit{u}=\dot{e}-({\mathit{\omega }}_1\times \mathit{a})\cdot ({\mathit{p}}_1-{\mathit{p}}_2)$. That row misses the turning of the axis, an error that vanishes only when the anchors coincide or when ${\mathit{\omega }}_1\times \mathit{a}$ happens to be perpendicular to ${\mathit{p}}_1-{\mathit{p}}_2$. An equality constraint lets the anchors slide apart along its free directions by design, so the error would grow with the slide and with body1's rotation: a 5 cm slide at 2 rad/s gives 0.1 m/s along the constrained axis. RaiSim uses the exact row above.

A pin. Its axes are fixed in the world, so $\dot{\mathit{a}}=0$ and there is no term A, and it holds the anchors together in every direction, so ${\mathit{p}}_1-{\mathit{p}}_2$ stays at zero. Its rows ${\mathit{a}}_i^T({\mathit{J}}_1({\mathit{p}}_1)-{\mathit{J}}_2({\mathit{p}}_2))$ are exact as they are.

Where the force acts. By virtual work, a row ${\mathit{a}}^T({\mathit{J}}_1({\mathit{p}}_2)-{\mathit{J}}_2({\mathit{p}}_2))$ with the impulse $\lambda$ applies the generalized impulse

$$({\mathit{J}}_1({\mathit{p}}_2)-{\mathit{J}}_2({\mathit{p}}_2){)}^T\mathit{a}\lambda ={\mathit{J}}_1({\mathit{p}}_2{)}^T(\lambda \mathit{a})-{\mathit{J}}_2({\mathit{p}}_2{)}^T(\lambda \mathit{a}):$$

the point impulse $\lambda \mathit{a}$ on body1 at ${\mathit{p}}_2$, and $-\lambda \mathit{a}$ on body2 at the same point. The two are equal, opposite and collinear, so they apply no net force or torque to the system. They have no component along the free directions, and their power $\lambda \dot{e}$ is zero while the constraint holds: the constraint does no work while the points slide.

Equations of motion of a step

With the constraint impulses $\mathit{\lambda }$ added to step 4, one step of the velocity-level integration reads

$$\tilde{\mathit{M}}({\mathit{u}}_{t+1}-{\mathit{u}}_t)=\mathrm{\Delta }t({\mathit{\tau }}_{exp}-\mathit{h})+{\mathit{J}}_c^T{\mathit{\lambda }}_c+{\mathit{J}}^T\mathit{\lambda }.$$

The constraints are imposed on the velocity at the end of the step, with the position error fed back:

$$\begin{array}{}\text{(1)}& \mathit{J}{\mathit{u}}_{t+1}=-\beta \mathit{e}.\end{array}$$

As one linear system in ${\mathit{u}}_{t+1}$ and $\mathit{\lambda }$ (for given ${\mathit{\lambda }}_c$), this is the saddle-point (KKT) problem

$$\left[\begin{array}{cc}\tilde{\mathit{M}}& -{\mathit{J}}^T\\ \mathit{J}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}{\mathit{u}}_{t+1}\\ \mathit{\lambda }\end{array}\right]=\left[\begin{array}{c}\tilde{\mathit{M}}{\mathit{u}}_t+\mathrm{\Delta }t({\mathit{\tau }}_{exp}-\mathit{h})+{\mathit{J}}_c^T{\mathit{\lambda }}_c\\ -\beta \mathit{e}\end{array}\right].$$

The impulses ${\mathit{\lambda }}_c$ are unknown as well, and subject to the Coulomb friction cones and to one-sided limits; that is what the contact solver iterates on. The elimination below removes $\mathit{\lambda }$ from this problem exactly, so the solver iterates only on ${\mathit{\lambda }}_c$.

Eliminating the constraints

1. The free velocity. Without any impulse, the velocity at the end of the step is ${\mathit{u}}^{\ast }={\mathit{u}}_t+\mathrm{\Delta }t{\tilde{\mathit{M}}}^{-1}({\mathit{\tau }}_{exp}-\mathit{h})$ (step 4).

2. Solve the first block row for ${\mathit{u}}_{t+1}$:

$${\mathit{u}}_{t+1}={\mathit{u}}^{\ast }+{\tilde{\mathit{M}}}^{-1}{\mathit{J}}_c^T{\mathit{\lambda }}_c+{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T\mathit{\lambda }.$$

3. Substitute it into the constraints (1). This leaves a system for $\mathit{\lambda }$ alone, whose matrix $\mathit{D}$ is the Schur complement of $\tilde{\mathit{M}}$ in the KKT matrix:

$$\mathit{D}\mathit{\lambda }=-\beta \mathit{e}-\mathit{J}{\mathit{u}}^{\ast }-\mathit{J}{\tilde{\mathit{M}}}^{-1}{\mathit{J}}_c^T{\mathit{\lambda }}_c.$$

Its solution splits into a part that depends only on the free motion and a part induced by the other impulses:

$$\begin{array}{}\text{(2)}& \mathit{\lambda }=\underset{{\mathit{\lambda }}_0}{\underbrace{{\mathit{D}}^{-1}(-\beta \mathit{e}-\mathit{J}{\mathit{u}}^{\ast })}}-{\mathit{D}}^{-1}\mathit{J}{\tilde{\mathit{M}}}^{-1}{\mathit{J}}_c^T{\mathit{\lambda }}_c.\end{array}$$

4. Substitute $\mathit{\lambda }$ back:

$${\mathit{u}}_{t+1}=\underset{{\mathit{u}}_0}{\underbrace{{\mathit{u}}^{\ast }+{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T{\mathit{\lambda }}_0}}+\underset{{\mathit{M}}_c^{-1}}{\underbrace{({\tilde{\mathit{M}}}^{-1}-{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T{\mathit{D}}^{-1}\mathit{J}{\tilde{\mathit{M}}}^{-1})}}{\mathit{J}}_c^T{\mathit{\lambda }}_c.$$

These are the reduced dynamics, in two parts:

• ${\mathit{u}}_0$ is the free velocity moved onto the constraints; by construction, $\mathit{J}{\mathit{u}}_0=-\beta \mathit{e}$. Each step applies the impulse ${\mathit{J}}^T{\mathit{\lambda }}_0$ before the contacts are solved.

• Every other impulse acts through the inverse mass of the constrained mechanism,

  $${\mathit{M}}_c^{-1}={\tilde{\mathit{M}}}^{-1}-{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T{\mathit{D}}^{-1}\mathit{J}{\tilde{\mathit{M}}}^{-1},$$

  whose response to an impulse is the unconstrained response minus its constrained part: ${\mathit{M}}_c^{-1}{\mathit{J}}_c^T={\tilde{\mathit{M}}}^{-1}{\mathit{J}}_c^T-{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T{\mathit{D}}^{-1}(\mathit{J}{\tilde{\mathit{M}}}^{-1}{\mathit{J}}_c^T)$.

Three properties make this exact:

• No impulse can open a constraint. $\mathit{J}{\mathit{M}}_c^{-1}=\mathit{J}{\tilde{\mathit{M}}}^{-1}-\mathit{D}{\mathit{D}}^{-1}\mathit{J}{\tilde{\mathit{M}}}^{-1}=\mathbf{0}$, so $\mathit{J}{\mathit{u}}_{t+1}=\mathit{J}{\mathit{u}}_0=-\beta \mathit{e}$ for any ${\mathit{\lambda }}_c$. The constraints hold whatever the contact solver does and however many iterations it runs.
• The contact problem keeps its form. ${\mathit{M}}_c^{-1}$ is symmetric and positive semidefinite. The contacts' apparent inverse inertia becomes ${\mathit{J}}_c{\mathit{M}}_c^{-1}{\mathit{J}}_c^T$: the same kind of quantity as without constraints, only smaller, because the closed mechanism is stiffer.
• The constraint impulse is still known. Equation (2) gives $\mathit{\lambda }$ once the solver has found ${\mathit{\lambda }}_c$; this is the impulse reported for each constraint (see Reading constraint forces).

In short, RaiSim solves the KKT system by block elimination: it eliminates $\mathit{\lambda }$ through $\mathit{D}$, solves the contacts on the constrained mechanism with the inverse mass ${\mathit{M}}_c^{-1}$, and recovers $\mathit{\lambda }$ from the solved ${\mathit{\lambda }}_c$.

If the rows of $\mathit{J}$ are dependent (redundant constraints, or a loop at a singular pose), $\mathit{D}$ is singular. RaiSim then keeps an independent subset of the rows; the motion is the same, and the load goes to the rows that are kept (see Redundant constraints and singular poses).

Why these are the reduced dynamics

Constrained dynamics can equivalently be written in coordinates of the allowed motion. Let the columns of $\mathit{N}$ span the null space of $\mathit{J}$, the velocities that keep the constraints. Restricting the equations of motion to that space (d'Alembert's principle: the constraint forces do no work on the allowed motions) gives the reduced mass ${\mathit{N}}^T\tilde{\mathit{M}}\mathit{N}$, and the constrained mechanism responds to a generalized impulse through $\mathit{N}({\mathit{N}}^T\tilde{\mathit{M}}\mathit{N}{)}^{-1}{\mathit{N}}^T$. For $\mathit{J}$ of full row rank, this is the same matrix:

$$\mathit{N}({\mathit{N}}^T\tilde{\mathit{M}}\mathit{N}{)}^{-1}{\mathit{N}}^T={\tilde{\mathit{M}}}^{-1}-{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T(\mathit{J}{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T{)}^{-1}\mathit{J}{\tilde{\mathit{M}}}^{-1}={\mathit{M}}_c^{-1}.$$

Both sides map a generalized impulse along the constraints, ${\mathit{J}}^T\mathit{y}$, to zero, and both map $\tilde{\mathit{M}}\mathit{N}\mathit{z}$ to $\mathit{N}\mathit{z}$. The two subspaces $\{{\mathit{J}}^T\mathit{y}\}$ and $\{\tilde{\mathit{M}}\mathit{N}\mathit{z}\}$ together span the whole space: $\tilde{\mathit{M}}\mathit{N}\mathit{z}={\mathit{J}}^T\mathit{y}$ implies ${\mathit{N}}^T\tilde{\mathit{M}}\mathit{N}\mathit{z}=\mathbf{0}$, so $\mathit{z}=\mathbf{0}$.

The right-hand form needs only $\mathit{J}$, ${\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T$ and a system with one equation per constraint row. No basis of the null space is built, the generalized coordinates stay those of the tree, and the number of constraint rows $|\mathit{\lambda }|$ is small compared with $|\mathit{u}|$.

$\mathit{P}=\mathit{I}-{\tilde{\mathit{M}}}^{-1}{\mathit{J}}^T{\mathit{D}}^{-1}\mathit{J}$ projects onto the allowed velocities, orthogonally in the kinetic-energy metric $\tilde{\mathit{M}}$: $\mathit{P}\mathit{u}$ is the allowed velocity closest to $\mathit{u}$ in $\parallel \cdot {\parallel }_{\tilde{\mathit{M}}}$. Moving ${\mathit{u}}^{\ast }$ to ${\mathit{u}}_0$ in step 4 is this projection, shifted so that $\mathit{J}{\mathit{u}}_0=-\beta \mathit{e}$ instead of zero. Physically, it is the impulse that a perfectly rigid and lossless constraint applies: of all velocities that satisfy the constraints, ${\mathit{u}}_0$ is the one closest to ${\mathit{u}}^{\ast }$ in kinetic energy (Gauss's principle of least constraint).

Integration Steps

Integration of an articulated system is performed in two stages: integrate1 and integrate2.

The following steps are performed in integrate1

1. If the time step, the control mode, the PD gains, the damping or the springs changed, update the diagonal $\mathit{w}$ added to the mass matrix (the effective inertia of the implicit springs, dampers and PD gains)
2. Update positions of the collision bodies
3. Detect collisions (called by the world instance)
4. The world assigns contacts on each object and computes the contact normal
5. Compute the mass matrix, nonlinear term, and inverse inertia matrix
6. Compute (Sparse) Jacobians of contacts

After this step, all kinematic/dynamic properties are computed. Users can access them if they are necessary for the controller. Next, integrate2 computes the rest of the simulation.

7. If the controller changed the control mode, the PD gains, the damping or the springs after integrate1, update $\mathit{w}$ as in step 1, then compute contact properties
8. Compute the PD torque (if used), add the feedforward generalized force, clamp this sum to the joint effort limits, and add the joint damping (never clipped, see Joint Damping and Friction)
9. Clip the actuator torques to the motor operating regions, map them to the joints and add them; then add the springs and the external forces/torques, and compute the velocity without contacts
10. Contact solver (called by the world instance). It also solves the joint limits and the joint friction
11. Integrate the velocity
12. Integrate the position with the integration scheme

Steps 8-12 are written out exactly in Joint torque of a simulation step.