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.

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -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;
 }

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,

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);
+			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)
+			{
+				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 + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
+					q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
-				qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
-				nu[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[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;
+				serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs+ baseDofQdot;
-				for (int i = 0; i < num_dofs; i++)
+
+				//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;
 			}

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];

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

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) &&
+			if (szObVel == szObAcc)
-				(szObAcc == dofCountOrg))
 			{
-				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* jointPositionsQ = (double*)malloc(szInBytesQ);
-				double* jointVelocitiesQdot = (double*)malloc(szInBytes);
+				double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
-				double* jointAccelerations = (double*)malloc(szInBytes);
+				double* jointAccelerations = (double*)malloc(szInBytesQdot);
-				double* jointForcesOutput = (double*)malloc(szInBytes);
+				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(
+						b3CalculateInverseDynamicsCommandInit2(
-							sm, bodyUniqueId, jointPositionsQ, jointVelocitiesQdot,
+							sm, bodyUniqueId, jointPositionsQ, szObPos, jointVelocitiesQdot,
-							jointAccelerations);
+							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], "
+								"positive and [joint velocities] and"
-								"[joint accelerations] need to match the number of "
+								"[joint accelerations] need to be equal and match the number of "
 								"degrees of freedom.");
 				return NULL;
 			}