- #1

Liferider

- 43

- 0

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 [tex] \mathbf{g} = \mathbf{\dot{G}\dot{x}} [/tex](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

[tex] \boldsymbol{\tau} = \mathbf{G}\boldsymbol{\tau_x} [/tex]

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!