make setJointPosMultiDof and setJointVelMultiDof argument const.

add PyBullet.resetJointStateMultiDof / getJointStateMultiDof, for preliminary support for spherical and planar joints

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -383,8 +383,65 @@ void ConvertURDF2BulletInternal(
 
 			switch (jointType)
 			{
-				case URDFFloatingJoint:
+				case URDFSphericalJoint:
+				{
+					if (createMultiBody)
+					{
+						creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+						cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+							parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
+							disableParentCollision);
+					}
+					else
+					{
+						btAssert(0);
+					}
+					break;
+				}
 				case URDFPlanarJoint:
+				{
+					
+					if (createMultiBody)
+					{
+#if 0
+						void setupPlanar(int i,  // 0 to num_links-1
+							btScalar mass,
+							const btVector3 &inertia,
+							int parent,
+							const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
+							const btVector3 &rotationAxis,
+							const btVector3 &parentComToThisComOffset,  // vector from parent COM to this COM, in PARENT frame
+							bool disableParentCollision = false);
+#endif
+						creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+						cache.m_bulletMultiBody->setupPlanar(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+							parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),
+							disableParentCollision);
+					}
+					else
+					{
+#if 0
+						//b3Printf("Fixed joint\n");
+
+						btGeneric6DofSpring2Constraint* dof6 = 0;
+
+						//backward compatibility
+						if (flags & CUF_RESERVED)
+						{
+							dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+						}
+						else
+						{
+							dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
+						}
+						if (enableConstraints)
+							world1->addConstraint(dof6, true);
+#endif
+					}
+					break;
+				}
+				case URDFFloatingJoint:
+				
 				case URDFFixedJoint:
 				{
 					if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
@@ -426,7 +483,9 @@ void ConvertURDF2BulletInternal(
 					if (createMultiBody)
 					{
 						cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
-															   parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),  //parent2joint.getOrigin(),
+															   parentRotToThis, quatRotate(offsetInB.getRotation(), 
+																   jointAxisInJointSpace), 
+																offsetInA.getOrigin(),
 															   -offsetInB.getOrigin(),
 															   disableParentCollision);
 

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -11,6 +11,8 @@ enum UrdfJointTypes
 	URDFFloatingJoint,
 	URDFPlanarJoint,
 	URDFFixedJoint,
+	URDFSphericalJoint,
+
 };
 
 enum URDF_LinkContactFlags

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -1280,7 +1280,9 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l
 	}
 
 	std::string type_str = type_char;
-	if (type_str == "planar")
+	if (type_str == "spherical")
+		joint.m_type = URDFSphericalJoint;
+	else if (type_str == "planar")
 		joint.m_type = URDFPlanarJoint;
 	else if (type_str == "floating")
 		joint.m_type = URDFFloatingJoint;