Merge pull request #2007 from erwincoumans/master

enable pybullet.calculateInverseDynamics for floating bodies

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

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,56 @@ 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;
+		const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
+
+		if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ+ num_dofs) &&
+			clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot+ num_dofs))
 		{
-			int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+			
-			const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
+			btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
-			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++)
+			//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];
+				btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
-				qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
+					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
-				nu[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
+					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 +8550,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 (szObVel == szObAcc)
-			if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
-				(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;
 			}

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -296,7 +296,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 		btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
 		btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
 
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
 											   capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(),
@@ -318,7 +318,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 		btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
 		btSphereShape* capsuleB = (btSphereShape*)min1;
 
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
 											   0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1,
@@ -340,7 +340,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 		btSphereShape* capsuleA = (btSphereShape*)min0;
 		btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
 
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(),
 											   capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(),
@@ -480,7 +480,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 			btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1;
 			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
 			{
-				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 				btScalar minDist = -1e30f;
 				btVector3 sepNormalWorldSpace;
@@ -547,7 +547,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 
 					//tri->initializePolyhedralFeatures();
 
-					btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+					btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 					btVector3 sepNormalWorldSpace;
 					btScalar minDist = -1e30f;

src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

@@ -116,7 +116,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWr
 	btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
 	btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
 
-	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 	resultOut->setPersistentManifold(m_manifoldPtr);
 	if (hasCollision)
 	{

src/BulletInverseDynamics/MultiBodyTree.hpp

@@ -16,7 +16,9 @@ enum JointType
 	/// one translational degree of freedom relative to parent
 	PRISMATIC,
 	/// six degrees of freedom relative to parent
-	FLOATING
+	FLOATING,
+	/// three degrees of freedom, relative to parent
+	SPHERICAL
 };
 
 /// Interface class for calculating inverse dynamics for tree structured
@@ -31,12 +33,14 @@ enum JointType
 ///	- PRISMATIC: displacement [m]
 ///	- FLOATING:  Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
 ///				 (in that order)
+/// - SPHERICAL: Euler x-y-z angles [rad]
 /// The u vector contains the generalized speeds, which are
 ///	- FIXED:	 none
 ///	- REVOLUTE:  time derivative of angle of rotation [rad/s]
 ///	- PRISMATIC: time derivative of displacement [m/s]
 ///	- FLOATING:  angular velocity [rad/s] (*not* time derivative of rpy angles)
 ///				 and time derivative of displacement in parent frame [m/s]
+//	- SPHERICAL:  angular velocity [rad/s]
 ///
 /// The q and u vectors are obtained by stacking contributions of all bodies in one
 /// vector in the order of body indices.
@@ -47,7 +51,7 @@ enum JointType
 ///	 - PRISMATIC: force  [N], along joint axis
 ///	 - FLOATING:  moment vector [Nm] and force vector [N], both in body-fixed frame
 ///				  (in that order)
-///
+///  - SPHERICAL: moment vector [Nm] 
 /// TODO - force element interface (friction, springs, dampers, etc)
 ///	  - gears and motor inertia
 class MultiBodyTree

src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -35,6 +35,8 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ
 			return "prismatic";
 		case FLOATING:
 			return "floating";
+		case SPHERICAL:
+			return "spherical";
 	}
 	return "error: invalid";
 }
@@ -88,6 +90,8 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
 			return 1;
 		case FLOATING:
 			return 6;
+		case SPHERICAL:
+			return 3;
 	}
 	bt_id_error_message("unknown joint type %d\n", type);
 	return 0;
@@ -150,6 +154,11 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets()
 				body.m_q_index = q_index;
 				q_index += 6;
 				break;
+			case SPHERICAL:
+				m_body_spherical_list.push_back(i);
+				body.m_q_index = q_index;
+				q_index += 3;
+				break;
 			default:
 				bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
 				return -1;
@@ -238,6 +247,16 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData()
 			case FLOATING:
 				// no static data
 				break;
+			case SPHERICAL:
+				//todo: review
+				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
+				body.m_parent_vel_rel(0) = 0;
+				body.m_parent_vel_rel(1) = 0;
+				body.m_parent_vel_rel(2) = 0;
+				body.m_parent_acc_rel(0) = 0;
+				body.m_parent_acc_rel(1) = 0;
+				body.m_parent_acc_rel(2) = 0;
+				break;
 		}
 
 			// resize & initialize jacobians to zero.
@@ -352,6 +371,15 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 		(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
 	}
 
+	// 4.4 spherical bodies (3-DoF joints)
+	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+	{
+		//todo: review
+		RigidBody &body = m_body_list[m_body_spherical_list[i]];
+		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
+		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
+		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
+	}
 	return 0;
 }
 
@@ -413,7 +441,8 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
 		RigidBody &body = m_body_list[m_body_floating_list[i]];
 
 		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-							   transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
+							   transformY(q(body.m_q_index + 1)) *
+							   transformX(q(body.m_q_index));
 		body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
 		body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
 		body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
@@ -444,6 +473,32 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
 			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
 		}
 	}
+	
+	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+	{
+		//todo: review
+		RigidBody &body = m_body_list[m_body_spherical_list[i]];
+
+		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
+							   transformY(q(body.m_q_index + 1)) *
+							   transformX(q(body.m_q_index));
+		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+
+		if (type >= POSITION_VELOCITY)
+		{
+			body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
+			body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
+			body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
+			body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+		}
+		if (type >= POSITION_VELOCITY_ACCELERATION)
+		{
+			body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
+			body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
+			body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
+			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+		}
+	}
 
 	// 2. absolute kinematic quantities (vector valued)
 	// NOTE: this should be optimized by specializing for different body types
@@ -560,6 +615,12 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
 			setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
 			setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
 
+			break;
+		case SPHERICAL:
+			//todo: review
+			setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
+			setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
+			setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
 			break;
 	}
 }
@@ -608,6 +669,32 @@ int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &
 }
 #endif
 
+static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
+{
+	switch (dof)
+	{
+		// rotational part
+		case 0:
+			Jac_JR(0) = 1;
+			Jac_JR(1) = 0;
+			Jac_JR(2) = 0;
+			setZero(Jac_JT);
+			break;
+		case 1:
+			Jac_JR(0) = 0;
+			Jac_JR(1) = 1;
+			Jac_JR(2) = 0;
+			setZero(Jac_JT);
+			break;
+		case 2:
+			Jac_JR(0) = 0;
+			Jac_JR(1) = 0;
+			Jac_JR(2) = 1;
+			setZero(Jac_JT);
+			break;
+	}
+}
+
 static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
 {
 	switch (dof)
@@ -664,6 +751,8 @@ static inline int jointNumDoFs(const JointType &type)
 			return 1;
 		case FLOATING:
 			return 6;
+		case SPHERICAL:
+			return 3;
 	}
 	// this should never happen
 	bt_id_error_message("invalid joint type\n");
@@ -798,6 +887,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 			{
 				setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
 			}
+			if (SPHERICAL == body.m_joint_type)
+			{
+				//todo: review
+				setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
+			}
 
 			vec3 body_eom_rot =
 				body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
@@ -810,14 +904,19 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 				// 1. for multi-dof joints, rest of the dofs of this body
 				for (int row = col - 1; row >= q_index_min; row--)
 				{
-					if (FLOATING != body.m_joint_type)
+					if (SPHERICAL == body.m_joint_type)
 					{
-						bt_id_error_message("??\n");
+						//todo: review
-						return -1;
+						setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
+						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
+						setMatxxElem(col, row, Mrc, mass_matrix);
+					}
+					if (FLOATING == body.m_joint_type)
+					{
+						setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
+						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
+						setMatxxElem(col, row, Mrc, mass_matrix);
 					}
-					setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
-					const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-					setMatxxElem(col, row, Mrc, mass_matrix);
 				}
 				// 2. ancestor dofs
 				int child_idx = i;
@@ -839,6 +938,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 					vec3 Jac_JT = parent_body.m_Jac_JT;
 					for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
 					{
+						if (SPHERICAL == parent_body.m_joint_type)
+						{
+							//todo: review
+							setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
+						}
 						// set jacobians for 6-DoF joints
 						if (FLOATING == parent_body.m_joint_type)
 						{

src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp

@@ -274,6 +274,8 @@ private:
 	idArray<int>::type m_body_prismatic_list;
 	// Indices of floating joints
 	idArray<int>::type m_body_floating_list;
+	// Indices of spherical joints
+	idArray<int>::type m_body_spherical_list;
 	// a user-provided integer
 	idArray<int>::type m_user_int;
 	// a user-provided pointer