enable pybullet.calculateInverseDynamics for floating bodies

Using calculateInverseDynamics with zero target acceleration allows to compute the non-linear dynamics forces (coriolis/gyroscopic) and/or gravity force.
This commit is contained in:
erwincoumans
2018-11-27 08:49:56 -08:00
parent 2e30a9565b
commit 192d27743a
6 changed files with 117 additions and 31 deletions

View File

@@ -384,6 +384,9 @@ extern "C"
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot);
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,