2. Ensure physics:
$
\boldsymbol{M}(\boldsymbol{y}) \boldsymbol{\ddot{y}}
+ \boldsymbol{C}(\boldsymbol{y},\boldsymbol{\dot{y}}) \boldsymbol{\dot{y}}
+ \boldsymbol{g}(\boldsymbol{y}) =
\boldsymbol{S}^T \boldsymbol{\tau}
+ \sum_{i=0}^{N_c} \boldsymbol{J}^T_{c,i} \boldsymbol{W}_{c,i}
$
$ d_\mathrm{C,i,N} \cdot F_{C,i,N} + | \dot{d}_\mathrm{C,i,T} | \cdot F_\mathrm{C,i,N} = 0$