From fdd517e00f6db40ef573081599557f2be8e821e9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 30 Dec 2016 18:32:57 -0800 Subject: [PATCH] First step towards a MuJoCo MJCF importer for Bullet. It can load the humanoid.xml, ant.xml and some other OpenAI GYM asset files. Not all fields are converted, so it is work-in-progress. This is useful for Reinforcement Learning experiments, and would also help integration with DeepMind Lab. --- data/mjcf/ant.xml | 80 ++ data/mjcf/capsule.xml | 13 + data/mjcf/hello_mjcf.xml | 13 + data/mjcf/hopper.xml | 44 + data/mjcf/humanoid.xml | 130 ++ data/mjcf/reacher.xml | 39 + data/mjcf/swimmer.xml | 38 + examples/ExampleBrowser/CMakeLists.txt | 5 + examples/ExampleBrowser/ExampleEntries.cpp | 3 + .../ImportMJCFDemo/BulletMJCFImporter.cpp | 1098 +++++++++++++++++ .../ImportMJCFDemo/BulletMJCFImporter.h | 78 ++ .../ImportMJCFDemo/ImportMJCFSetup.cpp | 173 ++- .../ImportURDFDemo/ImportURDFSetup.cpp | 4 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 8 +- .../Importers/ImportURDFDemo/URDF2Bullet.h | 3 +- .../Importers/ImportURDFDemo/UrdfParser.h | 5 + 16 files changed, 1713 insertions(+), 21 deletions(-) create mode 100644 data/mjcf/ant.xml create mode 100644 data/mjcf/capsule.xml create mode 100644 data/mjcf/hello_mjcf.xml create mode 100644 data/mjcf/hopper.xml create mode 100644 data/mjcf/humanoid.xml create mode 100644 data/mjcf/reacher.xml create mode 100644 data/mjcf/swimmer.xml create mode 100644 examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp create mode 100644 examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h 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); + } + + //,