Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

enable pybullet.calculateInverseDynamics for floating bodies

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

38
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -4369,17 +4369,51 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints; i++)
int dofCount = b3ComputeDofCount(physClient, bodyUniqueId);
for (int i = 0; i < dofCount; i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCount;
command->m_calculateInverseDynamicsArguments.m_dofCountQdot = dofCount;
return (b3SharedMemoryCommandHandle)command;
}
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCountQ;
for (int i = 0; i < dofCountQ; i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
command->m_calculateInverseDynamicsArguments.m_dofCountQdot = dofCountQdot;
for (int i=0;i<dofCountQdot;i++)
{
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,