Equations of Motion: Constrained Hybrid Dynamics

[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!

 

[eljose79]

Hi there,

It seems like you have a good understanding of the mathematical principles involved in your model-based dynamics software. However, without seeing your code or knowing more details about your specific system, it's difficult for me to give you specific feedback on where you may have gone wrong.

One thing I would suggest is to double check your constraint equations and make sure they are accurately representing the constraints in your system. It's possible that there may be an error in how you are constraining the matrices H and C.

Another thing to consider is the stability of your system. If your system becomes unstable when you try to solve for the inverse dynamics of constrained joints, it could be an indication that something is not quite right in your implementation.

I would also recommend reaching out to other experts in the field or seeking help from online communities where you can share your code and get more specific feedback on potential errors.

Overall, it seems like you have a good understanding of the principles involved and are on the right track. I wish you luck in finding the solution to your problem. Keep persevering and seeking help when needed.