diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf
new file mode 100644
index 000000000..24c0854bd
--- /dev/null
+++ b/data/two_cubes.sdf
@@ -0,0 +1,239 @@
+
+
+
+ 99.2
+ 1
+ 0 0 10 0 -0 0
+ 0.8 0.8 0.8 1
+ 0.2 0.2 0.2 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+ 1
+
+
+
+
+ 1 2 3
+ 100 100
+
+
+
+
+ 65535
+
+
+
+
+ 100
+ 50
+
+
+
+
+
+
+
+ 10
+
+
+ 0
+
+
+ 4 5 6
+ 100 100
+
+
+
+
+
+
+ 0
+ 0
+ 1
+
+
+ 0 0 -9.8
+ 6e-06 2.3e-05 -4.2e-05
+
+
+ 0.001
+ 1
+ 1000
+
+
+ 0.4 0.4 0.4 1
+ 0.7 0.7 0.7 1
+ 1
+
+
+
+ EARTH_WGS84
+ 0
+ 0
+ 0
+ 0
+
+
+ 0.512455 -1.58317 0.5 0 -0 0
+
+
+ 1
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+
+
+
+
+ 1 1 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+
+ 0
+ 0
+ 1
+
+
+
+ 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159
+
+
+ 1
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+
+
+
+
+ 1 1 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+
+ 0
+ 0
+ 1
+
+
+
+ 0 0
+ 0 0
+ 1462824251 956472000
+ 0
+
+ 0 0 0 0 -0 0
+ 1 1 1
+
+ 0 0 0 0 -0 0
+ 0 0 0 0 -0 0
+ 0 0 0 0 -0 0
+ 0 0 0 0 -0 0
+
+
+
+ 0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159
+ 1 1 1
+
+ 0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159
+ 0.004896 3e-06 -0.004891 -6e-06 0.009793 -0
+ 0.010615 0.006191 -9.78231 -0.012424 0.021225 -1.8e-05
+ 0.010615 0.006191 -9.78231 0 -0 0
+
+
+
+ 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159
+ 1 1 1
+
+ 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159
+ 0 0 0 0 -0 0
+ 0 0 0 0 -0 0
+ 0 0 0 0 -0 0
+
+
+
+ 0 0 10 0 -0 0
+
+
+
+
+ 8.0562 -8.87312 3.07529 0 0.205021 2.5208
+ orbit
+ perspective
+
+
+
+
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index fef09597d..0a1c172f0 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -132,6 +132,7 @@ SET(BulletExampleBrowser_SRCS
../Importers/ImportObjDemo/LoadMeshFromObj.cpp
../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../Importers/ImportSTLDemo/ImportSTLSetup.cpp
+ ../Importers/ImportSDFDemo/ImportSDFSetup.cpp
../Importers/ImportURDFDemo/ImportURDFSetup.cpp
../Importers/ImportURDFDemo/URDF2Bullet.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 6c66e2716..6d8b86bea 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -14,6 +14,7 @@
#include "../Importers/ImportColladaDemo/ImportColladaSetup.h"
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
+#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
#include "../Collision/CollisionTutorialBullet2.h"
#include "../GyroscopicDemo/GyroscopicSetup.h"
#include "../Constraints/Dof6Spring2Setup.h"
@@ -126,7 +127,6 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
-
ExampleEntry(0,"Tutorial"),
ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY),
ExampleEntry(1,"Gravity Acceleration","Motion of a free falling rigid body under constant gravitational acceleration", TutorialCreateFunc,TUT_ACCELERATION),
@@ -210,7 +210,7 @@ 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,"SDF (MultiBody)", "Import an SDF file, create multiple multibodies etc", ImportSDFCreateFunc),
ExampleEntry(0,"Vehicles"),
ExampleEntry(1,"Hinge2 Vehicle", "A rigid body chassis with 4 rigid body wheels attached by a btHinge2Constraint",Hinge2VehicleCreateFunc),
diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
new file mode 100644
index 000000000..b95f528a9
--- /dev/null
+++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
@@ -0,0 +1,302 @@
+
+#include "ImportSDFSetup.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../../Utils/b3ResourcePath.h"
+
+#include "../ImportURDFDemo/BulletUrdfImporter.h"
+
+
+#include "../ImportURDFDemo/URDF2Bullet.h"
+
+
+//#include "urdf_samples.h"
+
+
+
+
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "../ImportURDFDemo/MyMultiBodyCreator.h"
+
+
+class ImportSDFSetup : public CommonMultiBodyBase
+{
+ char m_fileName[1024];
+
+ struct ImportSDFInternalData* m_data;
+ bool m_useMultiBody;
+
+ //todo(erwincoumans) we need a name memory for each model
+ btAlignedObjectArray m_nameMemory;
+
+public:
+ ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
+ virtual ~ImportSDFSetup();
+
+ virtual void initPhysics();
+ virtual void stepSimulation(float deltaTime);
+
+ void setFileName(const char* urdfFileName);
+
+ virtual void resetCamera()
+ {
+ float dist = 3.5;
+ float pitch = -136;
+ float yaw = 28;
+ float targetPos[3]={0.47,0,-0.64};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+};
+
+
+static btAlignedObjectArray gFileNameArray;
+
+
+#define MAX_NUM_MOTORS 1024
+
+struct ImportSDFInternalData
+{
+ ImportSDFInternalData()
+ :m_numMotors(0)
+ {
+ for (int i=0;i=numFileNames)
+ {
+ count=0;
+ }
+ sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
+ }
+}
+
+ImportSDFSetup::~ImportSDFSetup()
+{
+ for (int i=0;isetUpAxis(upAxis);
+
+ this->createEmptyDynamicsWorld();
+ //m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+ btIDebugDraw::DBG_DrawConstraints
+ +btIDebugDraw::DBG_DrawContactPoints
+ +btIDebugDraw::DBG_DrawAabb
+ );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+
+ btVector3 gravity(0,0,0);
+ gravity[upAxis]=-9.8;
+
+ m_dynamicsWorld->setGravity(gravity);
+
+ BulletURDFImporter u2b(m_guiHelper);
+
+ bool loadOk = u2b.loadSDF(m_fileName);
+
+
+ if (loadOk)
+ {
+ //printTree(u2b,u2b.getRootLinkIndex());
+
+ //u2b.printTree();
+
+ btTransform identityTrans;
+ identityTrans.setIdentity();
+
+
+
+ for (int m =0; mgetNumMultiBodyConstraints();i++)
+ {
+ m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
+ }
+
+
+
+ bool createGround=true;
+ if (createGround)
+ {
+ btVector3 groundHalfExtents(20,20,20);
+ groundHalfExtents[upAxis]=1.f;
+ btBoxShape* box = new btBoxShape(groundHalfExtents);
+ box->initializePolyhedralFeatures();
+
+ m_guiHelper->createCollisionShapeGraphicsObject(box);
+ btTransform start; start.setIdentity();
+ btVector3 groundOrigin(0,0,0);
+ groundOrigin[upAxis]=-2.5;
+ start.setOrigin(groundOrigin);
+ btRigidBody* body = createRigidBody(0,start,box);
+ //m_dynamicsWorld->removeRigidBody(body);
+ // m_dynamicsWorld->addRigidBody(body,2,1);
+ btVector3 color(0.5,0.5,0.5);
+ m_guiHelper->createRigidBodyGraphicsObject(body,color);
+ }
+
+ ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
+ m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
+ }
+
+
+}
+
+void ImportSDFSetup::stepSimulation(float deltaTime)
+{
+ if (m_dynamicsWorld)
+ {
+ for (int i=0;im_numMotors;i++)
+ {
+ if (m_data->m_jointMotors[i])
+ {
+ m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[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->
+ }
+ }
+
+ //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
+ m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+ }
+}
+
+class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options)
+{
+
+ return new ImportSDFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
+}
diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.h b/examples/Importers/ImportSDFDemo/ImportSDFSetup.h
new file mode 100644
index 000000000..eebd416be
--- /dev/null
+++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.h
@@ -0,0 +1,8 @@
+#ifndef IMPORT_SDF_SETUP_H
+#define IMPORT_SDF_SETUP_H
+
+
+class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //IMPORT_SDF_SETUP_H
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 77d11953b..a6cc8019f 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -133,11 +133,69 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
}
BulletErrorLogger loggie;
+ m_data->m_urdfParser.setParseSDF(false);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase);
return result;
}
+int BulletURDFImporter::getNumModels() const
+{
+ return m_data->m_urdfParser.getNumModels();
+}
+
+void BulletURDFImporter::activateModel(int modelIndex)
+{
+ m_data->m_urdfParser.activateModel(modelIndex);
+}
+
+
+bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
+{
+
+ m_data->m_linkColors.clear();
+
+
+ //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();
+ }
+
+ BulletErrorLogger loggie;
+ //todo: quick test to see if we can re-use the URDF parser for SDF or not
+ m_data->m_urdfParser.setParseSDF(true);
+ bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
+
+ return result;
+}
+
+
const char* BulletURDFImporter::getPathPrefix()
{
return m_data->m_pathPrefix;
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index 1f9972511..edfafc135 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
@@ -4,6 +4,7 @@
#include "URDFImporterInterface.h"
+///BulletURDFImporter can deal with URDF and (soon) SDF files
class BulletURDFImporter : public URDFImporterInterface
{
@@ -18,6 +19,11 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
+ //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
+ virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
+ virtual int getNumModels() const;
+ virtual void activateModel(int modelIndex);
+
const char* getPathPrefix();
void printTree(); //for debugging
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index cfedc3a9a..6aad57f3e 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -163,7 +163,7 @@ static btVector4 colors[4] =
};
-btVector3 selectColor()
+static btVector3 selectColor()
{
static int curColor = 0;
diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
index 8837a5a8f..91dc52f31 100644
--- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
+++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
@@ -17,6 +17,8 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0;
+ virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
+
virtual const char* getPathPrefix()=0;
///return >=0 for the root link index, -1 if there is no root link
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index d0b064bfb..25ca7461e 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -5,14 +5,29 @@
#include "urdfLexicalCast.h"
UrdfParser::UrdfParser()
+:m_parseSDF(false),
+m_activeSdfModel(-1)
{
}
+
+
UrdfParser::~UrdfParser()
{
+ cleanModel(&m_urdf2Model);
- for (int i=0;im_materials.size();i++)
+ {
+ UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
if (matPtr)
{
UrdfMaterial* mat = *matPtr;
@@ -20,26 +35,25 @@ UrdfParser::~UrdfParser()
}
}
- for (int i=0;im_links.size();i++)
{
- UrdfLink** linkPtr = m_model.m_links.getAtIndex(i);
+ UrdfLink** linkPtr = model->m_links.getAtIndex(i);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
delete link;
}
}
-
- for (int i=0;im_joints.size();i++)
{
- UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i);
+ UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
delete joint;
}
}
-
}
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
@@ -192,14 +206,19 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have a mass element");
return false;
}
- if (!mass_xml->Attribute("value"))
- {
- logger->reportError("Inertial: mass element must have value attribute");
- return false;
- }
+ if (m_parseSDF)
+ {
+ inertia.m_mass = urdfLexicalCast(mass_xml->GetText());
+ } else
+ {
+ if (!mass_xml->Attribute("value"))
+ {
+ logger->reportError("Inertial: mass element must have value attribute");
+ return false;
+ }
- inertia.m_mass = urdfLexicalCast(mass_xml->Attribute("value"));
-
+ inertia.m_mass = urdfLexicalCast(mass_xml->Attribute("value"));
+ }
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
@@ -207,21 +226,45 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have inertia element");
return false;
}
- if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
- inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
- inertia_xml->Attribute("izz")))
- {
- logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
- return false;
- }
- inertia.m_ixx = urdfLexicalCast(inertia_xml->Attribute("ixx"));
- inertia.m_ixy = urdfLexicalCast(inertia_xml->Attribute("ixy"));
- inertia.m_ixz = urdfLexicalCast(inertia_xml->Attribute("ixz"));
- inertia.m_iyy = urdfLexicalCast(inertia_xml->Attribute("iyy"));
- inertia.m_iyz = urdfLexicalCast(inertia_xml->Attribute("iyz"));
- inertia.m_izz = urdfLexicalCast(inertia_xml->Attribute("izz"));
-
+ if (m_parseSDF)
+ {
+ TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
+ TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
+ TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
+ TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
+ TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
+ TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
+ if (ixx && ixy && ixz && iyy && iyz && izz)
+ {
+ inertia.m_ixx = urdfLexicalCast(ixx->GetText());
+ inertia.m_ixy = urdfLexicalCast(ixy->GetText());
+ inertia.m_ixz = urdfLexicalCast(ixz->GetText());
+ inertia.m_iyy = urdfLexicalCast(iyy->GetText());
+ inertia.m_iyz = urdfLexicalCast(iyz->GetText());
+ inertia.m_izz = urdfLexicalCast(izz->GetText());
+ } else
+ {
+ logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
+ return false;
+ }
+ } else
+ {
+ if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
+ inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
+ inertia_xml->Attribute("izz")))
+ {
+ logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
+ return false;
+ }
+ inertia.m_ixx = urdfLexicalCast(inertia_xml->Attribute("ixx"));
+ inertia.m_ixy = urdfLexicalCast(inertia_xml->Attribute("ixy"));
+ inertia.m_ixz = urdfLexicalCast(inertia_xml->Attribute("ixz"));
+ inertia.m_iyy = urdfLexicalCast(inertia_xml->Attribute("iyy"));
+ inertia.m_iyz = urdfLexicalCast(inertia_xml->Attribute("iyz"));
+ inertia.m_izz = urdfLexicalCast(inertia_xml->Attribute("izz"));
+ }
return true;
+
}
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
@@ -251,13 +294,27 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
else if (type_name == "box")
{
geom.m_type = URDF_GEOM_BOX;
- if (!shape->Attribute("size"))
- {
- logger->reportError("box requires a size attribute");
- } else
- {
- parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
- }
+ if (m_parseSDF)
+ {
+ TiXmlElement* size = shape->FirstChildElement("size");
+ if (0==size)
+ {
+ logger->reportError("box requires a size child element");
+ return false;
+ }
+ parseVector3(geom.m_boxSize,size->GetText(),logger);
+ }
+ else
+ {
+ if (!shape->Attribute("size"))
+ {
+ logger->reportError("box requires a size attribute");
+ return false;
+ } else
+ {
+ parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
+ }
+ }
}
else if (type_name == "cylinder")
{
@@ -294,9 +351,29 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
else
{
- logger->reportError("Unknown geometry type:");
- logger->reportError(type_name.c_str());
- return false;
+ if (this->m_parseSDF)
+ {
+ if (type_name == "plane")
+ {
+ geom.m_type = URDF_GEOM_PLANE;
+
+ TiXmlElement *n = shape->FirstChildElement("normal");
+ TiXmlElement *s = shape->FirstChildElement("size");
+
+ if ((0==n)||(0==s))
+ {
+ logger->reportError("Plane shape must have both normal and size attributes");
+ return false;
+ }
+
+ parseVector3(geom.m_planeNormal,n->GetText(),logger);
+ }
+ } else
+ {
+ logger->reportError("Unknown geometry type:");
+ logger->reportError(type_name.c_str());
+ return false;
+ }
}
return true;
@@ -331,7 +408,7 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
return true;
}
-bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
+bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
@@ -358,7 +435,8 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
// Material
TiXmlElement *mat = config->FirstChildElement("material");
- if (mat)
+//todo(erwincoumans) skip materials in SDF for now (due to complexity)
+ if (mat && !m_parseSDF)
{
// get material name
if (!mat->Attribute("name"))
@@ -377,7 +455,7 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
- m_model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+ model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
@@ -386,7 +464,7 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
return true;
}
-bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
+bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
@@ -435,7 +513,7 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
{
UrdfVisual visual;
- if (parseVisual(visual, vis_xml,logger))
+ if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
@@ -676,16 +754,16 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
}
-bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
+bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* 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_childLinkName.c_str());
+ UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
if (!childLinkPtr)
{
logger->reportError("Cannot find child link for joint ");
@@ -708,7 +786,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
}
UrdfLink* childLink = *childLinkPtr;
- UrdfLink** parentLinkPtr = m_model.m_links.find(joint->m_parentLinkName.c_str());
+ UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
if (!parentLinkPtr)
{
logger->reportError("Cannot find parent link for a joint");
@@ -728,9 +806,9 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
}
//search for children that have no parent, those are 'root'
- for (int i=0;im_parentLink)
{
- m_model.m_rootLinks.push_back(link);
+ model.m_rootLinks.push_back(link);
}
}
}
- if (m_model.m_rootLinks.size()>1)
+ if (model.m_rootLinks.size()>1)
{
logger->reportWarning("URDF file with multiple root links found");
}
- if (m_model.m_rootLinks.size()==0)
+ if (model.m_rootLinks.size()==0)
{
logger->reportError("URDF without root link found");
return false;
@@ -785,7 +863,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
logger->reportError("Expected a name for robot");
return false;
}
- m_model.m_name = name;
+ m_urdf2Model.m_name = name;
@@ -797,13 +875,13 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
parseMaterial(*material, material_xml, logger);
- UrdfMaterial** mat =m_model.m_materials.find(material->m_name.c_str());
+ UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
- m_model.m_materials.insert(material->m_name.c_str(),material);
+ m_urdf2Model.m_materials.insert(material->m_name.c_str(),material);
}
}
@@ -817,9 +895,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
{
UrdfLink* link = new UrdfLink;
- if (parseLink(*link, link_xml,logger))
+ if (parseLink(m_urdf2Model,*link, link_xml,logger))
{
- if (m_model.m_links.find(link->m_name.c_str()))
+ if (m_urdf2Model.m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
@@ -832,7 +910,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
- UrdfMaterial** mat = m_model.m_materials.find(vis.m_materialName.c_str());
+ UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
@@ -844,7 +922,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
- m_model.m_links.insert(link->m_name.c_str(),link);
+ m_urdf2Model.m_links.insert(link->m_name.c_str(),link);
}
} else
{
@@ -854,7 +932,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
- if (m_model.m_links.size() == 0)
+ if (m_urdf2Model.m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
@@ -867,7 +945,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (parseJoint(*joint, joint_xml,logger))
{
- if (m_model.m_joints.find(joint->m_name.c_str()))
+ if (m_urdf2Model.m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
@@ -875,7 +953,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
else
{
- m_model.m_joints.insert(joint->m_name.c_str(),joint);
+ m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
@@ -885,7 +963,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
- bool ok(initTreeAndRoot(logger));
+ bool ok(initTreeAndRoot(m_urdf2Model,logger));
if (!ok)
{
return false;
@@ -893,9 +971,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (forceFixedBase)
{
- for (int i=0;im_inertia.m_mass = 0.0;
link->m_inertia.m_ixx = 0.0;
link->m_inertia.m_ixy = 0.0;
@@ -908,3 +986,161 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
return true;
}
+
+void UrdfParser::activateModel(int modelIndex)
+{
+ m_activeSdfModel = modelIndex;
+}
+
+
+bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
+{
+
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(sdfText);
+ if (xml_doc.Error())
+ {
+ logger->reportError(xml_doc.ErrorDesc());
+ xml_doc.ClearError();
+ return false;
+ }
+
+ TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
+ if (!sdf_xml)
+ {
+ logger->reportError("expected an sdf element");
+ return false;
+ }
+
+ TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
+ if (!world_xml)
+ {
+ logger->reportError("expected a world element");
+ return false;
+ }
+
+ // Get all model (robot) elements
+ for (TiXmlElement* robot_xml = world_xml->FirstChildElement("model"); robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
+ {
+ UrdfModel* localModel = new UrdfModel;
+ m_tmpModels.push_back(localModel);
+
+
+ // Get robot name
+ const char *name = robot_xml->Attribute("name");
+ if (!name)
+ {
+ logger->reportError("Expected a name for robot");
+ return false;
+ }
+ localModel->m_name = name;
+
+
+
+ // Get all Material elements
+ for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
+ {
+ UrdfMaterial* material = new UrdfMaterial;
+
+ parseMaterial(*material, material_xml, logger);
+
+
+ UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
+ if (mat)
+ {
+ logger->reportWarning("Duplicate material");
+ } else
+ {
+ localModel->m_materials.insert(material->m_name.c_str(),material);
+ }
+ }
+
+
+ // char msg[1024];
+ // sprintf(msg,"Num materials=%d", m_model.m_materials.size());
+ // logger->printMessage(msg);
+
+
+ for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
+ {
+ UrdfLink* link = new UrdfLink;
+
+ if (parseLink(*localModel, *link, link_xml,logger))
+ {
+ if (localModel->m_links.find(link->m_name.c_str()))
+ {
+ logger->reportError("Link name is not unique, link names in the same model have to be unique");
+ logger->reportError(link->m_name.c_str());
+ return false;
+ } else
+ {
+ //copy model material into link material, if link has no local material
+ for (int i=0;im_visualArray.size();i++)
+ {
+ UrdfVisual& vis = link->m_visualArray.at(i);
+ if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
+ {
+ UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
+ if (mat && *mat)
+ {
+ vis.m_localMaterial = **mat;
+ } else
+ {
+ //logger->reportError("Cannot find material with name:");
+ //logger->reportError(vis.m_materialName.c_str());
+ }
+ }
+ }
+
+ localModel->m_links.insert(link->m_name.c_str(),link);
+ }
+ } else
+ {
+ logger->reportError("failed to parse link");
+ delete link;
+ return false;
+ }
+
+ }
+ if (localModel->m_links.size() == 0)
+ {
+ logger->reportWarning("No links found in URDF file");
+ return false;
+ }
+
+ // Get all Joint elements
+ for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
+ {
+ UrdfJoint* joint = new UrdfJoint;
+
+ if (parseJoint(*joint, joint_xml,logger))
+ {
+ if (localModel->m_joints.find(joint->m_name.c_str()))
+ {
+ logger->reportError("joint '%s' is not unique.");
+ logger->reportError(joint->m_name.c_str());
+ return false;
+ }
+ else
+ {
+ localModel->m_joints.insert(joint->m_name.c_str(),joint);
+ }
+ }
+ else
+ {
+ logger->reportError("joint xml is not initialized correctly");
+ return false;
+ }
+ }
+
+ bool ok(initTreeAndRoot(*localModel,logger));
+ if (!ok)
+ {
+ return false;
+ }
+ m_sdfModels.push_back(localModel);
+ }
+
+ return true;
+}
+
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index cc0b7887a..b966c2761 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -42,7 +42,9 @@ enum UrdfGeomTypes
URDF_GEOM_SPHERE=2,
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
- URDF_GEOM_MESH
+ URDF_GEOM_MESH,
+ URDF_GEOM_PLANE,
+
};
@@ -57,6 +59,8 @@ struct UrdfGeometry
double m_cylinderRadius;
double m_cylinderLength;
+ btVector3 m_planeNormal;
+
std::string m_meshFileName;
btVector3 m_meshScale;
};
@@ -134,32 +138,88 @@ struct UrdfModel
class UrdfParser
{
protected:
+
+ UrdfModel m_urdf2Model;
+ btAlignedObjectArray m_sdfModels;
+ btAlignedObjectArray m_tmpModels;
+
+ bool m_parseSDF;
+ int m_activeSdfModel;
+
+
+ void cleanModel(UrdfModel* model);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
- bool parseVisual(UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
+ bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
- bool initTreeAndRoot(ErrorLogger* logger);
+ bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
- bool parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
- UrdfModel m_model;
+ bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
+
public:
UrdfParser();
virtual ~UrdfParser();
- bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+ void setParseSDF(bool useSDF)
+ {
+ m_parseSDF = useSDF;
+ }
+ bool getParseSDF() const
+ {
+ return m_parseSDF;
+ }
+ bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+ bool loadSDF(const char* sdfText, ErrorLogger* logger);
+
+ int getNumModels() const
+ {
+ //user should have loaded an SDF when calling this method
+ btAssert(m_parseSDF);
+ if (m_parseSDF)
+ {
+ return m_sdfModels.size();
+ }
+ }
+
+ void activateModel(int modelIndex);
+
+ UrdfModel& getModelByIndex(int index)
+ {
+ //user should have loaded an SDF when calling this method
+ btAssert(m_parseSDF);
- const UrdfModel& getModel() const
+ return *m_sdfModels[index];
+ }
+
+ const UrdfModel& getModelByIndex(int index) const
+ {
+ //user should have loaded an SDF when calling this method
+ btAssert(m_parseSDF);
+
+ return *m_sdfModels[index];
+ }
+
+ const UrdfModel& getModel() const
{
- return m_model;
+ if (m_parseSDF)
+ {
+ return *m_sdfModels[m_activeSdfModel];
+ }
+
+ return m_urdf2Model;
}
UrdfModel& getModel()
{
- return m_model;
+ if (m_parseSDF)
+ {
+ return *m_sdfModels[m_activeSdfModel];
+ }
+ return m_urdf2Model;
}
};