Dynamics Constraints

The rigid body dynamics of the robot are governed by the equations of motion from equations_of_motion_in_optvar. This constraint ultimately dictates the achievable dynamics of the system, and is formulated as the following equality constraint,

(1) $\underbrace{\bmat{-M(\q) & S^{\top} & \Je^{\top}(\q)}}_{A^{d}}\optvar = \underbrace{\bs{n}(\q, \jsr)}_{\bs{b}^{d}} \tp$

The terms $A^{d}$ and $\bs{b}^{d}$ are used to distinguish the equality constraint matrix and vector, respectively, for the dynamic constraints.

Important

To put this into ORCA standard form we have,

$\bs{b}^{d} \leq A^{d}\optvar \leq \bs{b}^{d}$