few fixes in inverse dynamics

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;