# I Equations of Motion: Constrained Hybrid Dynamics

Tags:
1. Jan 28, 2017

### Liferider

Hi all, I've tried to figure this out for some time without luck. Hope you might be able to give some input.

I've implemented a model-based dynamics software in MATLAB based on the works of Roy Featherstone's Springer book "Rigid Body Dynamics Algorithms".

So, I have the EoM of an unconstrained system:
\begin{equation*} \mathbf{H}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) = \boldsymbol{\tau}
\end{equation*}
where i used the composite rigid body algorithm to find H and C. The system is constrained so I supplied the following state vector and its derivative:
\begin{align*} \mathbf{q} &= \boldsymbol{\gamma}(\mathbf{x}) \\
\mathbf{\dot{q}} &= \mathbf{G}\mathbf{\dot{x}}
\end{align*}
So far so good, but now I want to eliminate the constrained variables and solve for accelerations only for some joints while solving the inverse, ie, for the generalized forces for the other variables. To do this I constrain the matrices H and C with G and $$\mathbf{g} = \mathbf{\dot{G}\dot{x}}$$(still following Featherstone):
\begin{align*}
\mathbf{H_G} &= \mathbf{G^T}\mathbf{H}\mathbf{G} \\
\mathbf{C_G} &= \mathbf{G^T}(\mathbf{C} + \mathbf{H}\mathbf{g}) \\
\boldsymbol{\tau_G} &= \mathbf{G^T}\boldsymbol{\tau}
\end{align*}
to obtain the new constrained EoM:
\begin{equation*} \mathbf{H_G}\ddot{\mathbf{x}} + \mathbf{C_G} = \boldsymbol{\tau_G}
\end{equation*}
Note that I got the unconstrained generalized force via
$$\boldsymbol{\tau} = \mathbf{G}\boldsymbol{\tau_x}$$
where \tau_x is the generalized force vector of my independant variables.

I then proceed "by the book" to separate inverse and forward dynamics variables, solve first for the forward dynamics accelerations and then inversely for the remaining generalized forces. I just have a feeling that I ****ed up somewhere here. Maybe its somwhere else in my code but will this work? When I simulate the system it seems to work fine as long as I don't choose inverse dynamics for joints that are part of the constraint function.. when I do the motion becomes obviously wrong.

Anyways, thank you for any replies!

2. Feb 12, 2017