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.


To put this into ORCA standard form we have,

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