few fixes in inverse dynamics

Extras/InverseDynamics/btMultiBodyTreeCreator.cpp

@@ -202,8 +202,16 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
 				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
 				break;
 			case btMultibodyLink::eSpherical:
-				bt_id_error_message("spherical joints not implemented\n");
+				
-				return -1;
+				link.joint_type = SPHERICAL;
+				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
+				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
+				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
+				// random unit vector
+				link.body_axis_of_motion(0) = 0;
+				link.body_axis_of_motion(1) = 0;
+				link.body_axis_of_motion(2) = 1;
+				break;
 			case btMultibodyLink::ePlanar:
 				bt_id_error_message("planar joints not implemented\n");
 				return -1;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -9756,7 +9756,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
 											   numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
 											   &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
 					}
-					serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
+					serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId;
 					for (int i = 0; i < numDofs; i++)
 					{
 						serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -134,6 +134,15 @@ public:
 		return m_baseCollider;
 	}
 
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
 	btMultiBodyLinkCollider *getLinkCollider(int index)
 	{
 		if (index >= 0 && index < getNumLinks())

src/BulletInverseDynamics/MultiBodyTree.cpp

@@ -281,6 +281,8 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 			break;
 		case FLOATING:
 			break;
+		case SPHERICAL:
+			break;
 		default:
 			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
@@ -437,6 +439,16 @@ int MultiBodyTree::finalize()
 				rigid_body.m_Jac_JT(1) = 0.0;
 				rigid_body.m_Jac_JT(2) = 0.0;
 				break;
+			case SPHERICAL:
+				// NOTE/TODO: this is not really correct.
+				// the Jacobians should be 3x3 matrices here !
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
 			case FLOATING:
 				// NOTE/TODO: this is not really correct.
 				// the Jacobians should be 3x3 matrices here !

src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

@@ -28,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 			// does not add a degree of freedom
 			// m_num_dofs+=0;
 			break;
+		case SPHERICAL:
+			m_num_dofs += 3;
+			break;
 		case FLOATING:
 			m_num_dofs += 6;
 			break;