Dynamical Model From Dart

#1

Hi,
I have a question regarding dynamical modeling. So when we request getMassMatrix() and getCoriolisAndGravityForces(), what we get is is in generalized coordinates. Do it take into effect the holonomic and non-holonomic constraints?

What I mean is in general, system dynamics for robots are given as
M(q)\ddot{q}+H(\dot{q},q)+V\dot{q}+G=E\tau+A(q)^{T}λ

where A(q) represents the holonomic and nonholonomic constraints and lambda is the langrange multiplier. So do Dart gives us just M and we have to multiple A ourself or its gives us complete dynamics.

Guidance will be highly appreciated.

Thanks

#2

Hi @mamurtaza,

I rewrite your dynamic equation :
M(q)\ddot{q}+H(\dot{q},q)+V\dot{q}+G=E\tau+A(q)^{T}λ

to a simpler form but still the same :

M(q)\ddot{q}+C(q,\dot{q}) = \tau_{int} + \tau_{ext}

For computing :
M(q) : getMassMatrix()
C(q,\dot{q}) : getCoriolisAndGravityForces()
\tau_{ext} : getConstraintForces()

Then you are able to compute \tau_{int} .

\tau_{int} = -K_p (q^n + \dot{q}^n \Delta t - \bar{q}^{n+1}) - K_d \dot{q}^n + \ddot{q}^n \Delta t)

I don’t know if my answer could help you, I took the formula from the humanoid tutorial (SPD controller). let me in touch.

#3

Thank you Pedro for the reply. I agree that we can write the controller in that form because of its a PD controller based on error. However, if we want to write a sophisticated controller that utilizes the dynamical equation than I think it wouldn’t work.

I believe the dart getMass matrix gives us a unconstrainted mass matrix, considering the base frame is 6 dimensional(3 for orientation and 3 for the position). In order to write it in minimal coordinates, we need to figure out the rest of the 5 constraints( holonomic and non-holonomic constraints).
Once computed, we need to rewrite the dynamical equation as
M_hat(q)\ddot{q}+H_hat(\dot{q},q)+V_hat\dot{q}+G_hat=E_hat\tau+A(q)^{T}λ

where
M_hat=S(q)^{T}M(q)S(q),H_hat=S(q)^{T}[M(q) \dot{S}(q)ν+H(q, ̇q)],V_hat=S(q)^{T}VS(q),G_hat=S(q)^{T}G, E_hat=S(q)^{T}E and ν are your minimal coordinates. S is in the null space of A matrix.

I was hoping for the confirmation but I think my observation is correct. I got the idea from the paper https://link.springer.com/article/10.1007/s11370-008-0024-5

#4

Skeleton::getMassMatrix() does return the mass matrix in generalized (a.k.a. minimal) coordinates.

If the base frame is 6 dimensional, that means your root BodyNode has a FreeJoint as its parent joint. You can use root_body_node->moveTo<WeldJoint>(nullptr) if you want the base to be rigidly attached to the world. Or you can do the same with any other joint type.

For example, if you want to model a wheeled inverted pendulum, then you could model the dynamics as though the base joint is attached to the world using a prismatic joint. The axis of the prismatic joint would be parallel to the ground at the same height as the wheel axis of the inverted pendulum mechanism. This model would not take into account factors like wheel slip, but that’s how you would reduce it to the kind of dynamic model that you’re looking for.

#5

Thank you Grey for the help and guidance. So I am loading my model via URDF file and currently I am not defining my base node joint in the urdf. So technically speaking, my base is connected to the world via a free joint. is it equivalent if I define it in my urdf and describe joint type to be prismatic or is it explicitly need to be defined in code.

Thanks a lot for the help.

#6

You can do this with a URDF, but you have to use a DART-specific workaround, because attaching the base link to the world is not something that’s canonically supported by URDF.

If you look at DART’s URDF loader implementation here it explains that you can create a root link in your URDF with the name "world", and anything you attach to that root link will be connected to the world.

So in your case, you should create a dummy root link in your URDF with the name “world” and attach your actual base link to it using a prismatic joint.