# 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

### Greg Bernhardt

Thanks for the thread! This is an automated courtesy bump. Sorry you aren't generating responses at the moment. Do you have any further information, come to any new conclusions or is it possible to reword the post? The more details the better.