diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f8ee5b4aa..4fdbbdc05 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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;im_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, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index cb7c40f16..18f8f81f6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6c8e8c496..a89073bb9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 50db5777b..27d65b1da 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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]; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 47ee7f531..cc522e754 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1086614c3..e9dd48a47 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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; }