diff --git a/data/mjcf/ant.xml b/data/mjcf/ant.xml
new file mode 100644
index 000000000..18ad38b38
--- /dev/null
+++ b/data/mjcf/ant.xml
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/mjcf/capsule.xml b/data/mjcf/capsule.xml
new file mode 100644
index 000000000..f9fb5483a
--- /dev/null
+++ b/data/mjcf/capsule.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/data/mjcf/hello_mjcf.xml b/data/mjcf/hello_mjcf.xml
new file mode 100644
index 000000000..b434868d0
--- /dev/null
+++ b/data/mjcf/hello_mjcf.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/data/mjcf/hopper.xml b/data/mjcf/hopper.xml
new file mode 100644
index 000000000..7f94e64dc
--- /dev/null
+++ b/data/mjcf/hopper.xml
@@ -0,0 +1,44 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/data/mjcf/humanoid.xml b/data/mjcf/humanoid.xml
new file mode 100644
index 000000000..2ed2f55ec
--- /dev/null
+++ b/data/mjcf/humanoid.xml
@@ -0,0 +1,130 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/mjcf/reacher.xml b/data/mjcf/reacher.xml
new file mode 100644
index 000000000..28ebd6712
--- /dev/null
+++ b/data/mjcf/reacher.xml
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/data/mjcf/swimmer.xml b/data/mjcf/swimmer.xml
new file mode 100644
index 000000000..cda25da9d
--- /dev/null
+++ b/data/mjcf/swimmer.xml
@@ -0,0 +1,38 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 0096560b6..9b84597eb 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/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
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index f6af39031..ffcc2e14b 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/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"),
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
new file mode 100644
index 000000000..be7832408
--- /dev/null
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -0,0 +1,1098 @@
+#include "BulletMJCFImporter.h"
+#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include
+#include "../../Utils/b3ResourcePath.h"
+#include
+#include
+#include "../ImportURDFDemo/UrdfParser.h"
+#include "../ImportURDFDemo/urdfStringSplit.h"
+#include "../ImportURDFDemo/urdfLexicalCast.h"
+
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h""
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+
+
+
+enum ePARENT_LINK_ENUMS
+{
+ INVALID_LINK_INDEX=-2
+};
+
+
+
+static bool parseVector4(btVector4& vec4, const std::string& vector_str)
+{
+ vec4.setZero();
+ btArray pieces;
+ btArray rgba;
+ btAlignedObjectArray strArray;
+ urdfIsAnyOf(" ", strArray);
+ urdfStringSplit(pieces, vector_str, strArray);
+ for (int i = 0; i < pieces.size(); ++i)
+ {
+ if (!pieces[i].empty())
+ {
+ rgba.push_back(urdfLexicalCast(pieces[i].c_str()));
+ }
+ }
+ if (rgba.size() != 4)
+ {
+ return false;
+ }
+ vec4.setValue(rgba[0],rgba[1],rgba[2],rgba[3]);
+ return true;
+}
+
+static bool parseVector3(btVector3& vec3, const std::string& vector_str, MJCFErrorLogger* logger, bool lastThree = false)
+{
+ vec3.setZero();
+ btArray pieces;
+ btArray rgba;
+ btAlignedObjectArray strArray;
+ urdfIsAnyOf(" ", strArray);
+ urdfStringSplit(pieces, vector_str, strArray);
+ for (int i = 0; i < pieces.size(); ++i)
+ {
+ if (!pieces[i].empty())
+ {
+ rgba.push_back(urdfLexicalCast(pieces[i].c_str()));
+ }
+ }
+ if (rgba.size() < 3)
+ {
+ logger->reportWarning("Couldn't parse vector3");
+ return false;
+ }
+ if (lastThree) {
+ vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
+ }
+ else
+ {
+ vec3.setValue(rgba[0],rgba[1],rgba[2]);
+
+ }
+ return true;
+}
+
+
+static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector_str, MJCFErrorLogger* logger)
+{
+ v0.setZero();
+ v1.setZero();
+
+ btArray pieces;
+ btArray values;
+ btAlignedObjectArray strArray;
+ urdfIsAnyOf(" ", strArray);
+ urdfStringSplit(pieces, vector_str, strArray);
+ for (int i = 0; i < pieces.size(); ++i)
+ {
+ if (!pieces[i].empty())
+ {
+ values.push_back(urdfLexicalCast(pieces[i].c_str()));
+ }
+ }
+ if (values.size() < 6)
+ {
+ logger->reportWarning("Couldn't parse 6 floats");
+ return false;
+ }
+ v0.setValue(values[0],values[1],values[2]);
+ v1.setValue(values[3],values[4],values[5]);
+
+ return true;
+}
+
+
+
+struct BulletMJCFImporterInternalData
+{
+ GUIHelperInterface* m_guiHelper;
+ char m_pathPrefix[1024];
+
+ std::string m_fileModelName;
+
+ btAlignedObjectArray m_models;
+ int m_activeModel;
+
+ BulletMJCFImporterInternalData()
+ :m_activeModel(-1)
+ {
+ m_pathPrefix[0] = 0;
+ }
+
+ const UrdfLink* getLink(int modelIndex, int linkIndex) const
+ {
+ if (modelIndex>=0 && modelIndexm_links.getAtIndex(linkIndex);
+ if (linkPtrPtr && *linkPtrPtr)
+ {
+ UrdfLink* linkPtr = *linkPtrPtr;
+ return linkPtr;
+ }
+ }
+ return 0;
+ }
+
+ bool parseRootLevel(TiXmlElement* root_xml,MJCFErrorLogger* logger)
+ {
+ for (TiXmlElement* xml = root_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
+ {
+ bool handled = false;
+ std::string n = xml->Value();
+ if (n=="body")
+ {
+ int modelIndex = m_models.size();
+ UrdfModel* model = new UrdfModel();
+ m_models.push_back(model);
+ parseBody(xml,modelIndex, INVALID_LINK_INDEX,logger);
+ initTreeAndRoot(*model,logger);
+ handled = true;
+ }
+
+ if (n=="geom")
+ {
+ int modelIndex = m_models.size();
+ UrdfModel* modelPtr = new UrdfModel();
+ m_models.push_back(modelPtr);
+
+ UrdfLink* linkPtr = new UrdfLink();
+ linkPtr->m_name = "anonymous";
+ const char* namePtr = xml->Attribute("name");
+ if (namePtr)
+ {
+ linkPtr->m_name = namePtr;
+ }
+ int linkIndex = modelPtr->m_links.size();
+ linkPtr->m_linkIndex = linkIndex;
+ modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr);
+
+ btTransform linkTransform = parseTransform(xml,logger);
+ linkPtr->m_linkTransformInWorld = linkTransform;
+
+// modelPtr->m_rootLinks.push_back(linkPtr);
+
+ parseGeom(xml,modelIndex, linkIndex,logger);
+ initTreeAndRoot(*modelPtr,logger);
+
+ handled = true;
+ }
+
+
+ if (n=="site")
+ {
+ handled = true;
+ }
+ if (!handled)
+ {
+ logger->reportWarning("Unhandled root element");
+ logger->reportWarning(n.c_str());
+ }
+ }
+ return true;
+ }
+
+ bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, btTransform& jointTransOut)
+ {
+ const char* jType = link_xml->Attribute("type");
+ const char* limitedStr = link_xml->Attribute("limited");
+ const char* axisStr = link_xml->Attribute("axis");
+ const char* posStr = link_xml->Attribute("pos");
+ const char* ornStr = link_xml->Attribute("quat");
+ const char* nameStr = link_xml->Attribute("name");
+ btTransform jointTrans;
+ jointTrans.setIdentity();
+ if (posStr)
+ {
+ btVector3 pos;
+ std::string p=posStr;
+ if (parseVector3(pos,p,logger))
+ {
+ jointTrans.setOrigin(pos);
+ }
+ }
+ if (ornStr)
+ {
+ std::string o = ornStr;
+ btVector4 o4;
+ if (parseVector4(o4,o))
+ {
+ btQuaternion orn(o4[3],o4[0],o4[1],o4[2]);
+ jointTrans.setRotation(orn);
+ }
+ }
+ btVector3 jointAxis(1,0,0);
+
+ if (axisStr)
+ {
+ std::string ax = axisStr;
+ parseVector3(jointAxis,ax,logger);
+ } else
+ {
+ logger->reportWarning("joint without axis attribute");
+ }
+ bool isLimited = false;
+ if (limitedStr)
+ {
+ std::string lim = limitedStr;
+ if (lim=="true")
+ {
+ isLimited = true;
+ }
+ } else
+ {
+// logger->reportWarning("joint without limited field");
+ }
+
+ bool jointHandled = false;
+ const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
+
+ btTransform parentLinkToJointTransform;
+ parentLinkToJointTransform.setIdentity();
+ parentLinkToJointTransform = jointTrans*linkPtr->m_linkTransformInWorld;
+ jointTransOut = jointTrans;
+ UrdfJointTypes ejtype;
+ if (jType)
+ {
+ std::string jointType = jType;
+ if (jointType == "fixed")
+ {
+ ejtype = URDFFixedJoint;
+ jointHandled = true;
+ }
+ if (jointType == "hinge")
+ {
+ if (isLimited)
+ {
+ ejtype = URDFRevoluteJoint;
+ } else
+ {
+ ejtype = URDFContinuousJoint;
+ }
+ jointHandled = true;
+ }
+ } else
+ {
+ logger->reportWarning("Expected 'type' attribute for joint");
+ }
+
+ if (jointHandled)
+ {
+ //default to 'fixed' joint
+ UrdfJoint* jointPtr = new UrdfJoint();
+ jointPtr->m_childLinkName=linkPtr->m_name;
+ const UrdfLink* parentLink = getLink(modelIndex,parentLinkIndex);
+ jointPtr->m_parentLinkName =parentLink->m_name;
+ jointPtr->m_localJointAxis=jointAxis;
+ jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform;
+ jointPtr->m_type = ejtype;
+ int numJoints = m_models[modelIndex]->m_joints.size();
+ if (nameStr)
+ {
+ jointPtr->m_name =nameStr;
+ } else
+ {
+ char jointName[1024];
+ sprintf(jointName,"joint%d_%d",linkIndex,numJoints);
+ jointPtr->m_name =jointName;
+ }
+ m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr);
+ return true;
+ }
+ /*
+ URDFRevoluteJoint=1,
+ URDFPrismaticJoint,
+ URDFContinuousJoint,
+ URDFFloatingJoint,
+ URDFPlanarJoint,
+ URDFFixedJoint,
+ */
+ return false;
+ }
+ bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger)
+ {
+ UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
+ if (linkPtrPtr==0)
+ {
+ logger->reportWarning("Invalide linkindex");
+ return false;
+ }
+ UrdfLink* linkPtr = *linkPtrPtr;
+
+ btTransform linkLocalFrame;
+ linkLocalFrame.setIdentity();
+
+
+ bool handledGeomType = false;
+ UrdfGeometry geom;
+ const char* rgba = link_xml->Attribute("rgba");
+ const char* gType = link_xml->Attribute("type");
+ const char* sz = link_xml->Attribute("size");
+ const char* posS = link_xml->Attribute("pos");
+ if (posS)
+ {
+ btVector3 pos(0,0,0);
+ std::string p = posS;
+ if (parseVector3(pos,p,logger))
+ {
+ linkLocalFrame.setOrigin(pos);
+ }
+ }
+ const char* ornS = link_xml->Attribute("quat");
+ if (ornS)
+ {
+ std::string ornStr = ornS;
+ btQuaternion orn(0,0,0,1);
+ btVector4 o4;
+ if (parseVector4(o4,ornStr))
+ {
+ orn.setValue(o4[3],o4[0],o4[1],o4[2]);
+ linkLocalFrame.setRotation(orn);
+ }
+ }
+ if (gType)
+ {
+ std::string geomType = gType;
+
+
+ if (geomType == "plane")
+ {
+ geom.m_type = URDF_GEOM_PLANE;
+ geom.m_planeNormal.setValue(0,0,1);
+ btVector3 size(1,1,1);
+ if (sz)
+ {
+ std::string sizeStr = sz;
+ bool lastThree = false;
+ parseVector3(size,sizeStr,logger,lastThree);
+ }
+ geom.m_boxSize = size;
+ handledGeomType = true;
+ }
+ if (geomType == "box")
+ {
+ btVector3 size(1,1,1);
+ if (sz)
+ {
+ std::string sizeStr = sz;
+ bool lastThree = false;
+ parseVector3(size,sizeStr,logger,lastThree);
+ }
+ geom.m_type = URDF_GEOM_BOX;
+ geom.m_boxSize = size;
+ handledGeomType = true;
+ }
+
+ if (geomType == "sphere")
+ {
+ geom.m_type = URDF_GEOM_SPHERE;
+ if (sz)
+ {
+ geom.m_sphereRadius = urdfLexicalCast(sz);
+ } else
+ {
+ logger->reportWarning("Expected size field (scalar) in sphere geom");
+ }
+ handledGeomType = true;
+ }
+
+ //todo: capsule, cylinder, meshes or heightfields etc
+ if (geomType == "capsule")
+ {
+ geom.m_type = URDF_GEOM_CAPSULE;
+ geom.m_capsuleRadius = urdfLexicalCast(sz);
+ const char* fromtoStr = link_xml->Attribute("fromto");
+ if (fromtoStr)
+ {
+ std::string fromto = fromtoStr;
+ parseVector6(geom.m_capsuleFrom,geom.m_capsuleTo,fromto,logger);
+ handledGeomType = true;
+ } else
+ {
+ logger->reportWarning("capsule without fromto attribute");
+ }
+ }
+ #if 0
+ if (geomType == "cylinder")
+ {
+ geom.m_type = URDF_GEOM_CYLINDER;
+ handledGeomType = true;
+ }
+#endif
+ if (handledGeomType)
+ {
+
+ UrdfCollision col;
+ col.m_geometry = geom;
+ col.m_linkLocalFrame = linkLocalFrame;
+ linkPtr->m_collisionArray.push_back(col);
+
+ } else
+ {
+ char warn[1024];
+ sprintf(warn,"Unknown/unhandled geom type: %s", geomType.c_str());
+ logger->reportWarning(warn);
+ }
+ } else
+ {
+ logger->reportWarning("geom requires type");
+ }
+
+ return handledGeomType;
+ }
+
+ btTransform parseTransform(TiXmlElement* link_xml, MJCFErrorLogger* logger)
+ {
+ btTransform tr;
+ tr.setIdentity();
+
+ const char* p = link_xml->Attribute("pos");
+ if (p)
+ {
+ btVector3 pos(0,0,0);
+ std::string pstr = p;
+ if (parseVector3(pos,pstr,logger))
+ {
+ tr.setOrigin(pos);
+ }
+
+ } else
+ {
+// logger->reportWarning("body should have pos attribute");
+ }
+ const char* o = link_xml->Attribute("quat");
+ if (o)
+ {
+ std::string ornstr = o;
+ btVector4 o4;
+ btQuaternion orn(0,0,0,1);
+ if (parseVector4(o4,ornstr))
+ {
+ orn.setValue(o4[3],o4[0],o4[1],o4[2]);//MuJoCo quats are [w,x,y,z], Bullet uses [x,y,z,w]
+ tr.setRotation(orn);
+ }
+ } else
+ {
+// logger->reportWarning("body doesn't have quat (orientation) attribute");
+ }
+ return tr;
+ }
+
+ double computeVolume(const UrdfLink* linkPtr,MJCFErrorLogger* logger) const
+ {
+ double totalVolume = 0;
+
+ for (int i=0;im_collisionArray.size();i++)
+ {
+ const UrdfCollision* col = &linkPtr->m_collisionArray[i];
+ switch (col->m_geometry.m_type)
+ {
+ case URDF_GEOM_SPHERE:
+ {
+ double r = col->m_geometry.m_sphereRadius;
+ totalVolume += 4./3.*SIMD_PI*r*r*r;
+ break;
+ }
+ case URDF_GEOM_BOX:
+ {
+ totalVolume += 8. * col->m_geometry.m_boxSize[0]*
+ col->m_geometry.m_boxSize[1]*
+ col->m_geometry.m_boxSize[2];
+ break;
+ }
+
+ case URDF_GEOM_CYLINDER:
+ {
+ //todo
+ break;
+ }
+ case URDF_GEOM_MESH:
+ {
+ //todo (based on mesh bounding box?)
+ break;
+ }
+ case URDF_GEOM_PLANE:
+ {
+ //todo
+ break;
+ }
+ case URDF_GEOM_CAPSULE:
+ {
+ //one sphere
+ double r = col->m_geometry.m_capsuleRadius;
+ totalVolume += 4./3.*SIMD_PI*r*r*r;
+ //and one cylinder of 'height'
+ btScalar h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length();
+ totalVolume += SIMD_PI*r*r*h;
+
+ break;
+ }
+ default:
+ {
+ }
+ }
+ }
+
+ return totalVolume;
+ }
+
+ bool parseBody(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, MJCFErrorLogger* logger)
+ {
+ UrdfModel* modelPtr = m_models[modelIndex];
+ int linkIndex = modelPtr->m_links.size();
+ UrdfLink* linkPtr = new UrdfLink();
+ char uniqueLinkName[1024];
+ sprintf(uniqueLinkName,"link%d",linkIndex);
+ linkPtr->m_name = uniqueLinkName;
+ const char* namePtr = link_xml->Attribute("name");
+ if (namePtr)
+ {
+ linkPtr->m_name = namePtr;
+ }
+
+ linkPtr->m_linkIndex = linkIndex;
+ modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr);
+
+
+ btTransform linkTransform = parseTransform(link_xml,logger);
+
+ linkPtr->m_linkTransformInWorld = linkTransform;
+ //body/geom links with no parent are root links
+ if (parentLinkIndex==INVALID_LINK_INDEX)
+ {
+// modelPtr->m_rootLinks.push_back(linkPtr);
+ }
+
+ bool massDefined = false;
+ btVector3 inertialPos(0,0,0);
+ btQuaternion inertialOrn(0,0,0,1);
+ btScalar mass = 0.f;
+ btVector3 localInertiaDiag(0,0,0);
+ int thisLinkIndex = -2;
+ bool hasJoint = false;
+ btTransform jointTrans;
+ jointTrans.setIdentity();
+
+ for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
+ {
+ bool handled = false;
+ std::string n = xml->Value();
+ if (n=="inertial")
+ {
+ //
+
+ const char* p = xml->Attribute("pos");
+ if (p)
+ {
+ std::string posStr = p;
+ parseVector3(inertialPos,posStr,logger);
+ }
+ const char* m = xml->Attribute("mass");
+ if (m)
+ {
+ mass = urdfLexicalCast(m);
+ }
+ const char* i = xml->Attribute("diaginertia");
+ if (i)
+ {
+ std::string istr = i;
+ parseVector3(localInertiaDiag,istr,logger);
+ }
+
+ massDefined = true;
+
+ handled = true;
+ }
+
+ if (n=="joint")
+ {
+ //skip joints at the root for now
+ //MuJoCo supports more than just 'free' or 'fixed',
+ //so we will need to emulate this with extra root links+joints
+
+ //for now, only convert 1st joint
+ if (!hasJoint)
+ {
+ if (parentLinkIndex!=INVALID_LINK_INDEX)
+ {
+ parseJoint(xml,modelIndex,parentLinkIndex, linkIndex,logger,jointTrans);
+ }
+ }
+ hasJoint = true;
+ handled = true;
+ }
+ if (n == "geom")
+ {
+ parseGeom(xml,modelIndex, linkIndex, logger);
+ handled = true;
+ }
+
+ //recursive
+ if (n=="body")
+ {
+ parseBody(xml,modelIndex,linkIndex,logger);
+ handled = true;
+ }
+
+ if (n=="light")
+ {
+ handled = true;
+ }
+ if (!handled)
+ {
+ char warn[1024];
+ std::string n = xml->Value();
+ sprintf(warn,"Unknown/unhandled field: %s", n.c_str());
+ logger->reportWarning(warn);
+ }
+ }
+
+ if ((!hasJoint) && (parentLinkIndex != INVALID_LINK_INDEX))
+ {
+ //default to 'fixed' joint
+ UrdfJoint* jointPtr = new UrdfJoint();
+ jointPtr->m_childLinkName=linkPtr->m_name;
+ const UrdfLink* parentLink = getLink(modelIndex,parentLinkIndex);
+ jointPtr->m_parentLinkName =parentLink->m_name;
+ jointPtr->m_localJointAxis.setValue(1,0,0);
+ jointPtr->m_parentLinkToJointTransform = linkTransform;
+ jointPtr->m_type = URDFFixedJoint;
+ char jointName[1024];
+ sprintf(jointName,"joint%d",linkIndex);
+ jointPtr->m_name =jointName;
+ m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr);
+ }
+
+ //check mass/inertia
+ if (!massDefined)
+ {
+ double density = 1000;
+ double volume = computeVolume(linkPtr,logger);
+ mass = density * volume;
+ }
+ linkPtr->m_inertia.m_linkLocalFrame = jointTrans.inverse();
+ linkPtr->m_inertia.m_mass = mass;
+ return true;
+ }
+
+ bool initTreeAndRoot(UrdfModel& model, MJCFErrorLogger* logger)
+ {
+ // every link has children links and joints, but no parents, so we create a
+ // local convenience data structure for keeping child->parent relations
+ btHashMap parentLinkTree;
+
+ // loop through all joints, for every link, assign children links and children joints
+ for (int i=0;im_parentLinkName;
+ std::string child_link_name = joint->m_childLinkName;
+ if (parent_link_name.empty() || child_link_name.empty())
+ {
+ logger->reportError("parent link or child link is empty for joint");
+ logger->reportError(joint->m_name.c_str());
+ return false;
+ }
+
+ UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
+ if (!childLinkPtr)
+ {
+ logger->reportError("Cannot find child link for joint ");
+ logger->reportError(joint->m_name.c_str());
+
+ return false;
+ }
+ UrdfLink* childLink = *childLinkPtr;
+
+ UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
+ if (!parentLinkPtr)
+ {
+ logger->reportError("Cannot find parent link for a joint");
+ logger->reportError(joint->m_name.c_str());
+ return false;
+ }
+ UrdfLink* parentLink = *parentLinkPtr;
+
+ childLink->m_parentLink = parentLink;
+
+ childLink->m_parentJoint = joint;
+ parentLink->m_childJoints.push_back(joint);
+ parentLink->m_childLinks.push_back(childLink);
+ parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
+ }
+ }
+
+ //search for children that have no parent, those are 'root'
+ for (int i=0;im_linkIndex = i;
+
+ if (!link->m_parentLink)
+ {
+ model.m_rootLinks.push_back(link);
+ }
+ }
+
+ }
+
+ if (model.m_rootLinks.size()>1)
+ {
+ logger->reportWarning("URDF file with multiple root links found");
+ }
+
+ if (model.m_rootLinks.size()==0)
+ {
+ logger->reportError("URDF without root link found");
+ return false;
+ }
+ return true;
+
+ }
+
+};
+
+BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper)
+{
+ m_data = new BulletMJCFImporterInternalData();
+ m_data->m_guiHelper = helper;
+}
+
+BulletMJCFImporter::~BulletMJCFImporter()
+{
+ delete m_data;
+}
+
+bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase)
+{
+ if (strlen(fileName)==0)
+ return false;
+
+//int argc=0;
+ char relativeFileName[1024];
+
+ b3FileUtils fu;
+
+ //bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
+ bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
+
+ std::string xml_string;
+ m_data->m_pathPrefix[0] = 0;
+
+ if (!fileFound){
+ std::cerr << "URDF file not found" << std::endl;
+ return false;
+ } else
+ {
+
+ int maxPathLen = 1024;
+ fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
+
+
+ std::fstream xml_file(relativeFileName, std::fstream::in);
+ while ( xml_file.good())
+ {
+ std::string line;
+ std::getline( xml_file, line);
+ xml_string += (line + "\n");
+ }
+ xml_file.close();
+
+ if (parseMJCFString(xml_string.c_str(), logger))
+ {
+ return true;
+ }
+ }
+
+
+
+ return false;
+}
+
+bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* logger)
+{
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(xmlText);
+ if (xml_doc.Error())
+ {
+ logger->reportError(xml_doc.ErrorDesc());
+ xml_doc.ClearError();
+ return false;
+ }
+
+ TiXmlElement *mujoco_xml = xml_doc.FirstChildElement("mujoco");
+ if (!mujoco_xml)
+ {
+ logger->reportWarning("Cannot find root element");
+ return false;
+ }
+
+ const char* modelName = mujoco_xml->Attribute("model");
+ if (modelName)
+ {
+ m_data->m_fileModelName = modelName;
+ }
+
+
+ for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
+ {
+ m_data->parseRootLevel(link_xml,logger);
+ }
+
+ for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("worldbody"); link_xml; link_xml = link_xml->NextSiblingElement("worldbody"))
+ {
+ m_data->parseRootLevel(link_xml,logger);
+ }
+
+ //,