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,13 +4369,47 @@ 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;
}

3
examples/SharedMemory/PhysicsClientC_API.h
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,

66
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -8493,23 +8493,55 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
if (bodyHandle && bodyHandle->m_multiBody)
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == baseDofQ &&
clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == baseDofQdot)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs + baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++)
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
if (baseDofQ)
{
q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
btScalar yawZ, pitchY, rollX;
orn.getEulerZYX(yawZ, pitchY, rollX);
q[0] = rollX;
q[1] = pitchY;
q[2] = yawZ;
q[3] = pos[0];
q[4] = pos[1];
q[5] = pos[2];
}
else
{
for (int i = 0; i < num_dofs; i++)
{
q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
}
}
for (int i = 0; i < num_dofs + baseDofQdot; i++)
{
qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
@@ -8517,10 +8549,22 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
for (int i = 0; i < num_dofs; i++)
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs+ baseDofQdot;
//inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
if (baseDofQdot)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i + baseDofs];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
}
for (int i = baseDofQdot; i < num_dofs+ baseDofQdot; i++)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
}
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -658,7 +658,8 @@ enum EnumSdfRequestInfoFlags
struct CalculateInverseDynamicsArgs
{
int m_bodyUniqueId;
int m_dofCountQ;
int m_dofCountQdot;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,7 +7,9 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 201810250
#define SHARED_MEMORY_MAGIC_NUMBER 201811260
//#define SHARED_MEMORY_MAGIC_NUMBER 201810250
//#define SHARED_MEMORY_MAGIC_NUMBER 201809030
//#define SHARED_MEMORY_MAGIC_NUMBER 201809010
//#define SHARED_MEMORY_MAGIC_NUMBER 201807040

32
examples/pybullet/pybullet.c
View File

@@ -9590,23 +9590,25 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations);
int dofCountOrg = b3ComputeDofCount(sm, bodyUniqueId);
if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
(szObAcc == dofCountOrg))
if (szObVel == szObAcc)
{
int szInBytes = sizeof(double) * dofCountOrg;
int szInBytesQ = sizeof(double) * szObPos;
int szInBytesQdot = sizeof(double) * szObVel;
int i;
PyObject* pylist = 0;
double* jointPositionsQ = (double*)malloc(szInBytes);
double* jointVelocitiesQdot = (double*)malloc(szInBytes);
double* jointAccelerations = (double*)malloc(szInBytes);
double* jointForcesOutput = (double*)malloc(szInBytes);
double* jointPositionsQ = (double*)malloc(szInBytesQ);
double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
double* jointAccelerations = (double*)malloc(szInBytesQdot);
double* jointForcesOutput = (double*)malloc(szInBytesQdot);
for (i = 0; i < dofCountOrg; i++)
for (i = 0; i < szObPos; i++)
{
jointPositionsQ[i] =
pybullet_internalGetFloatFromSequence(objPositionsQ, i);
}
for (i = 0; i < szObVel; i++)
{
jointVelocitiesQdot[i] =
pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
jointAccelerations[i] =
@@ -9617,9 +9619,9 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateInverseDynamicsCommandInit(
sm, bodyUniqueId, jointPositionsQ, jointVelocitiesQdot,
jointAccelerations);
b3CalculateInverseDynamicsCommandInit2(
sm, bodyUniqueId, jointPositionsQ, szObPos, jointVelocitiesQdot,
jointAccelerations, szObVel);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@@ -9650,7 +9652,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
else
{
PyErr_SetString(SpamError,
"Internal error in calculateInverseDynamics");
"Error in calculateInverseDynamics, please check arguments.");
}
}
free(jointPositionsQ);
@@ -9663,8 +9665,8 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
{
PyErr_SetString(SpamError,
"calculateInverseDynamics numDofs needs to be "
"positive and [joint positions], [joint velocities], "
"[joint accelerations] need to match the number of "
"positive and [joint velocities] and"
"[joint accelerations] need to be equal and match the number of "
"degrees of freedom.");
return NULL;
}