How to get the dynamic equation?

[foolyc]

how to get the matrix M/N/S/Jc etc. after loading a robot xml file?

[erwincoumans]

As described in the PyBullet Quickstart Guide, PyBullet provides a function to calculate the mass matrix M, calculateMassMatrix, and Jacobian, calculateJacobian. There is an example for calculateJacobian here:
https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/jacobian.py

There is also a calculateInverseDynamics and a calculateInverseKinematics with example:

Presumably the 'N' stands for Coriolis and centrifugal terms? Those are not exposed at the moment. Does 'S' stand for user forces? Some link/info about the variable names and what/why you need it would help.

[foolyc]

well, 'N' stands for centripetal, Coriolis and gravity forces, '\tao' stands for user force, '\lamda' stands for constraint forces
i want to use this matrix to do inverse dynamic QP optimization for control purpose.

[haudren]

Indeed, those terms are not exposed yet. I've had success using RBDL and RBDyn (and maybe even Tasks if you want to do QP-based control) to compute the missing terms, treating pybullet as a pure simulator.

[erwincoumans]

It would be good to expose the missing terms in PyBullet as well.

1. Exposing the N term (gravity, coriolis and centrifugal) should be easy.

2. Why would you need the \tao / user force? Didn't you apply it yourself?

3. Do you need the constaint forces, as computed by the LCP solver, such as contact, joint limits term too?

How does RBDL or RBDyn help with (2) and (3)?

Any other term(s)?

[haudren]

Sorry, I did not come back to this issue:

RBDL/RBDyn only help with your point 1) AFAIK. However, RBDyn especially has a suite of associated control tools.

2. In my mind, \tau is the control signal, so it is provided by the user.

3. In typical control schemes, the contact forces are either sensed or estimated. I think it would be important to be able to retrieve that information from the simulator to implement and / or verify algorithms. An extra term might be the contact locations.

[elkmug]

1. Exposing the N term (gravity, coriolis and centrifugal) should be easy.

If we're voting, heck even if we're not, I'd love to see this exposed in pybullet.

[foolyc]

Thanks for your reply. I choose RBDL in my project.

[benelot]

Constraint forces might be something I need at some point for openai gym.

…

On Mon, Jun 4, 2018, 12:01 chao ***@***.***> wrote: Thanks for your reply. I choose RBDL in my project. — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#1529 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AC97q4zRJHS2J3GaYZo4CkFzm-K19ga2ks5t5QVwgaJpZM4Rt5rC> .

[erwincoumans]

Re-opened for others who may be interested:

I'm currently re-implementing DeepMimic in PyBullet and for stable PD control also need the N (or C, gravity) term. DeepMimic also uses Bullet, but it implements its own inverse dynamics and stable PD control, using those terms.
You should be able to get the N term using pybullet.calculateInverseDynamics and setting the target acceleration to zero vector:

N(q,qdot) = p.calculateInverseDynamics(objUid, q, qdot, zeroAcc)
I just added a PR fix to allow calculateInverseDynamics for floating bodies (it was only supporting fixed-base bodies with joints).

The contact forces are available too, after stepSimulation. Use pybullet.getContactPoints, and see the quickstart guide for the applied force field.

The only remaining thing would be to expose the non-contact constraint forces (motors, limits etc) as well.

[elkmug]

This is cool. My interest in the N term was because I wanted to experiment with stable PD control, as opposed to regular PD control, in PyBullet. And, that interest was due to reading about them in the DeepMimic paper.

So I'm doubly happy.

[erwincoumans]

I'm currently looking to extend BulletInverseDynamics with spherical joints and add stable PD control, but it is takes some time.

DeepMimic Stable PD control is here:
https://github.com/xbpeng/DeepMimic/blob/master/DeepMimicCore/sim/ImpPDController.cpp#L136

Its inverse dynamics is here:
https://github.com/xbpeng/DeepMimic/blob/master/DeepMimicCore/sim/RBDUtil.cpp

[soshishimada]

In the presence of contact force from a floor, calculateInverseDynamics() function with zero acceleration will return:

N(q,qdot) - '\lamda' = p.calculateInverseDynamics(objUid, q, qdot, zeroAcc).

meaning, the returned value includes centripetal, Coriolis, gravity and force from a floor. Am I right?