Merge pull request #900 from erwincoumans/master

First step towards a MuJoCo MJCF importer for Bullet. It can load the…

examples/ExampleBrowser/CMakeLists.txt

@@ -295,6 +295,11 @@ SET(BulletExampleBrowser_SRCS
   ../Importers/ImportURDFDemo/ImportURDFSetup.h
   ../Importers/ImportURDFDemo/URDF2Bullet.h
   ../Importers/ImportURDFDemo/urdf_samples.h
+  ../Importers/ImportURDFDemo/urdf_samples.h
+	../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+	../Importers/ImportMJCFDemo/BulletMJCFImporter.h
+	../Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
+	../Importers/ImportMJCFDemo/ImportMJCFSetup.h
   ../Importers/ImportBsp/BspConverter.cpp
   ../Importers/ImportBsp/BspLoader.cpp
   ../Importers/ImportBsp/ImportBspExample.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -18,6 +18,7 @@
 #include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
 #include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
 #include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
+#include "../Importers/ImportMJCFDemo/ImportMJCFSetup.h"
 #include "../Collision/CollisionTutorialBullet2.h"
 #include "../GyroscopicDemo/GyroscopicSetup.h"
 #include "../Constraints/Dof6Spring2Setup.h"
@@ -231,6 +232,8 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"URDF (RigidBody)", "Import a URDF file, and create rigid bodies (btRigidBody) connected by constraints.", ImportURDFCreateFunc, 0),
 	ExampleEntry(1,"URDF (MultiBody)", "Import a URDF file and create a single multibody (btMultiBody) with tree hierarchy of links (mobilizers).",
 					ImportURDFCreateFunc, 1),
+	ExampleEntry(1,"MJCF (MultiBody)", "Import a MJCF xml file, create multiple multibodies etc", ImportMJCFCreateFunc),
+
 	ExampleEntry(1,"SDF (MultiBody)", "Import an SDF file, create multiple multibodies etc", ImportSDFCreateFunc),
 
 	ExampleEntry(0,"Vehicles"),

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -0,0 +1,78 @@
+#ifndef BULLET_MJCF_IMPORTER_H
+#define BULLET_MJCF_IMPORTER_H
+
+#include "../ImportURDFDemo/URDFImporterInterface.h"
+#include "../ImportURDFDemo/LinkVisualShapesConverter.h"
+
+
+struct MJCFErrorLogger
+{
+	virtual void reportError(const char* error)=0;
+	virtual void reportWarning(const char* warning)=0;
+	virtual void printMessage(const char* msg)=0;
+};
+
+
+
+class BulletMJCFImporter : public URDFImporterInterface
+{
+	struct BulletMJCFImporterInternalData* m_data;
+
+
+public:
+	BulletMJCFImporter(struct GUIHelperInterface* helper);
+	virtual ~BulletMJCFImporter();
+	
+	virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
+
+	virtual bool loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase = false);
+
+	int getNumModels() const;
+
+	void activateModel(int modelIndex);
+
+	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
+	{
+		return false;
+	}
+
+    virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
+
+    virtual const char* getPathPrefix();
+    
+    ///return >=0 for the root link index, -1 if there is no root link
+    virtual int getRootLinkIndex() const;
+    
+    ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
+    virtual std::string getLinkName(int linkIndex) const;
+
+	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
+	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+
+	///this API will likely change, don't override it!
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
+    
+    virtual std::string getJointName(int linkIndex) const;
+
+    //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
+    virtual void  getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
+    
+    ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
+    virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
+    
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
+    
+    virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
+    
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
+    
+    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
+    virtual void setBodyUniqueId(int bodyId);
+    virtual int getBodyUniqueId() const;
+    
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const ;
+
+
+};
+
+#endif //BULLET_MJCF_IMPORTER_H

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -14,6 +14,8 @@
 #include "../CommonInterfaces/CommonMultiBodyBase.h"
 
 #include "../ImportURDFDemo/MyMultiBodyCreator.h"
+#include "BulletMJCFImporter.h"
+#include "../ImportURDFDemo/URDF2Bullet.h"
 
 class ImportMJCFSetup : public CommonMultiBodyBase
 {
@@ -63,7 +65,7 @@ struct ImportMJCFInternalData
     }
 
     
-    btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
+    btScalar m_motorTargetPositions[MAX_NUM_MOTORS];
     btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
 	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
     int m_numMotors;
@@ -75,19 +77,13 @@ struct ImportMJCFInternalData
 
 ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
 	:CommonMultiBodyBase(helper),
-	m_grav(0),
+	m_grav(-10),
 	m_upAxis(2)
 {
 	m_data = new ImportMJCFInternalData;
 
-	if (option==1)
-	{
-		m_useMultiBody = true;
-	} else
-	{
-		m_useMultiBody = false;
-	}
-
+	m_useMultiBody = true;
+	
 	static int count = 0;
 	if (fileName)
 	{
@@ -121,8 +117,13 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gMCFJFileNameArray.size()==0)
 		{
-			gMCFJFileNameArray.push_back("quadruped/quadruped.mjcf");
-
+			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
+			gMCFJFileNameArray.push_back("mjcf/hello_mjcf2.xml");
+			gMCFJFileNameArray.push_back("mjcf/capsule.xml");
+			gMCFJFileNameArray.push_back("mjcf/ant.xml");
+//			gMCFJFileNameArray.push_back("mjcf/hopper.xml");
+//			gMCFJFileNameArray.push_back("mjcf/swimmer.xml");
+//			gMCFJFileNameArray.push_back("mjcf/reacher.xml");
 		}
 
 		int numFileNames = gMCFJFileNameArray.size();
@@ -170,7 +171,21 @@ void ImportMJCFSetup::setFileName(const char* mjcfFileName)
 }
 
 
-
+struct MyMJCFLogger : public MJCFErrorLogger
+{
+	virtual void reportError(const char* error)
+	{
+		b3Error(error);
+	}
+	virtual void reportWarning(const char* warning)
+	{
+		b3Warning(warning);
+	}
+	virtual void printMessage(const char* msg)
+	{
+		b3Printf(msg);
+	}
+};
 
 void ImportMJCFSetup::initPhysics()
 {
@@ -196,7 +211,132 @@ void ImportMJCFSetup::initPhysics()
 		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
 	
+	BulletMJCFImporter importer(m_guiHelper);
+	MyMJCFLogger logger;
+	bool result = importer.loadMJCF(m_fileName,&logger);
+	if (result)
+	{
+		 btTransform rootTrans;
+        rootTrans.setIdentity();
+        
+        for (int m =0; m<importer.getNumModels();m++)
+		{
 
+            importer.activateModel(m);
+            
+			btMultiBody* mb = 0;
+
+
+			//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+			int rootLinkIndex = importer.getRootLinkIndex();
+			//b3Printf("urdf root link index = %d\n",rootLinkIndex);
+			MyMultiBodyCreator creation(m_guiHelper);
+
+			rootTrans.setIdentity();
+           importer.getRootTransformInWorld(rootTrans);
+
+			ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
+
+			mb = creation.getBulletMultiBody();
+			if (mb)
+			{
+				printf("first MJCF file converted!\n");
+				std::string*   name = new std::string(importer.getLinkName(importer.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
+				s->registerNameForPointer(name->c_str(),name->c_str());
+#endif//TEST_MULTIBODY_SERIALIZATION
+				mb->setBaseName(name->c_str());
+				//create motors for each btMultiBody joint
+				int numLinks = mb->getNumLinks();
+				for (int i=0;i<numLinks;i++)
+				{
+					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(importer.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(importer.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION					
+					s->registerNameForPointer(jointName->c_str(),jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(),linkName->c_str());
+#endif//TEST_MULTIBODY_SERIALIZATION
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+							
+					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
+					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
+					)
+					{
+						if (m_data->m_numMotors<MAX_NUM_MOTORS)
+						{
+							
+							char motorName[1024];
+							sprintf(motorName,"%s q ", jointName->c_str());
+							btScalar* motorPos = &m_data->m_motorTargetPositions[m_data->m_numMotors];
+							*motorPos = 0.f;
+							SliderParams slider(motorName,motorPos);
+							slider.m_minVal=-4;
+							slider.m_maxVal=4;
+							slider.m_clampToIntegers = false;
+							slider.m_clampToNotches = false;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+							float maxMotorImpulse = 5.f;
+							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
+							motor->setErp(0.1);
+							//motor->setMaxAppliedImpulse(0);
+							m_data->m_jointMotors[m_data->m_numMotors]=motor;
+							m_dynamicsWorld->addMultiBodyConstraint(motor);
+							m_data->m_numMotors++;
+						}
+					}
+
+				}
+			} else
+			{
+				if (1)
+				{
+					//create motors for each generic joint
+					int num6Dof = creation.getNum6DofConstraints();
+					for (int i=0;i<num6Dof;i++)
+					{
+						btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
+						if (c->getUserConstraintPtr())
+						{
+							GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
+							if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) || 
+								(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
+								(jointInfo->m_urdfJointType ==URDFContinuousJoint))
+							{
+								int urdfLinkIndex = jointInfo->m_urdfIndex;
+								std::string jointName = importer.getJointName(urdfLinkIndex);
+								char motorName[1024];
+								sprintf(motorName,"%s q'", jointName.c_str());
+								btScalar* motorVel = &m_data->m_motorTargetPositions[m_data->m_numMotors];
+
+								*motorVel = 0.f;
+								SliderParams slider(motorName,motorVel);
+								slider.m_minVal=-4;
+								slider.m_maxVal=4;
+								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+								m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
+								bool motorOn = true;
+								c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
+								c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
+								c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
+								
+								m_data->m_numMotors++;
+							}
+						}
+					}
+				}
+			
+			}
+		}
+		m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+	}
 		
 }
 
@@ -206,7 +346,7 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
 {
 	if (m_dynamicsWorld)
 	{
-		btVector3 gravity(0, 0, 0);
+		btVector3 gravity(0, 0, -10);
 		gravity[m_upAxis] = m_grav;
 		m_dynamicsWorld->setGravity(gravity);
 
@@ -214,13 +354,12 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
         {
 			if (m_data->m_jointMotors[i])
 			{
-				m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
+				m_data->m_jointMotors[i]->setPositionTarget(m_data->m_motorTargetPositions[i]);
 			}
 			if (m_data->m_generic6DofJointMotors[i])
 			{
 				GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
-				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
-				//jointInfo->
+				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetPositions[i]);
 			}
         }
 

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -89,7 +89,7 @@ struct ImportUrdfInternalData
 
 ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
 	:CommonMultiBodyBase(helper),
-	m_grav(0),
+	m_grav(-10),
 	m_upAxis(2)
 {
 	m_data = new ImportUrdfInternalData;
@@ -135,7 +135,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gFileNameArray.size()==0)
 		{
-			gFileNameArray.push_back("quadruped/quadruped.urdf");
+			gFileNameArray.push_back("r2d2.urdf");
 
 		}
 

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -237,7 +237,13 @@ void ConvertURDF2BulletInternal(
     }
     else
     {
-        linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+		if (flags & CUF_USE_MJCF)
+		{
+			linkTransformInWorldSpace =parentTransformInWorldSpace*linkTransformInWorldSpace;
+		} else
+		{
+	        linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+		}
     }
     
     

examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -17,7 +17,8 @@ class MultiBodyCreationInterface;
 enum ConvertURDFFlags {
   CUF_USE_SDF = 1,
   // Use inertia values in URDF instead of recomputing them from collision shape.
-  CUF_USE_URDF_INERTIA = 2
+  CUF_USE_URDF_INERTIA = 2,
+  CUF_USE_MJCF = 4,
 };
 
 void ConvertURDF2Bullet(const URDFImporterInterface& u2b,

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -44,6 +44,7 @@ enum UrdfGeomTypes
 	URDF_GEOM_CYLINDER,
 	URDF_GEOM_MESH,
     URDF_GEOM_PLANE,
+	URDF_GEOM_CAPSULE//non-standard URDF?
     
 };
 
@@ -56,6 +57,10 @@ struct UrdfGeometry
 	
 	btVector3 m_boxSize;
 	
+	double m_capsuleRadius;
+	btVector3 m_capsuleFrom;
+	btVector3 m_capsuleTo;
+
 	double m_cylinderRadius;
 	double m_cylinderLength;