Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add initial SDF importer, work-in-progress (still too incomplete to be useful)

Browse Source
This commit is contained in:
Erwin Coumans
2016-05-09 17:25:07 -07:00
parent 2fee43b021
commit e9c6abff47
11 changed files with 991 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
data/two_cubes.sdf Normal file
View File

@@ -0,0 +1,239 @@
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<yunfei>99.2</yunfei>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>1 2 3</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode/>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>4 5 6</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='unit_box_0'>
<pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
<link name='unit_box_0::link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<model name='unit_box_0_clone'>
<pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
<link name='unit_box_0::link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<state world_name='default'>
<sim_time>0 0</sim_time>
<real_time>0 0</real_time>
<wall_time>1462824251 956472000</wall_time>
<iterations>0</iterations>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_0'>
<pose frame=''>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
<scale>1 1 1</scale>
<link name='unit_box_0::link'>
<pose frame=''>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
<velocity>0.004896 3e-06 -0.004891 -6e-06 0.009793 -0</velocity>
<acceleration>0.010615 0.006191 -9.78231 -0.012424 0.021225 -1.8e-05</acceleration>
<wrench>0.010615 0.006191 -9.78231 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_0_clone'>
<pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
<scale>1 1 1</scale>
<link name='unit_box_0::link'>
<pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>8.0562 -8.87312 3.07529 0 0.205021 2.5208</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

1
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -132,6 +132,7 @@ SET(BulletExampleBrowser_SRCS
../Importers/ImportObjDemo/LoadMeshFromObj.cpp ../Importers/ImportObjDemo/LoadMeshFromObj.cpp
../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp ../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../Importers/ImportSTLDemo/ImportSTLSetup.cpp ../Importers/ImportSTLDemo/ImportSTLSetup.cpp
../Importers/ImportSDFDemo/ImportSDFSetup.cpp
../Importers/ImportURDFDemo/ImportURDFSetup.cpp ../Importers/ImportURDFDemo/ImportURDFSetup.cpp
../Importers/ImportURDFDemo/URDF2Bullet.cpp ../Importers/ImportURDFDemo/URDF2Bullet.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

4
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -14,6 +14,7 @@
#include "../Importers/ImportColladaDemo/ImportColladaSetup.h" #include "../Importers/ImportColladaDemo/ImportColladaSetup.h"
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h" #include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h" #include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
#include "../Collision/CollisionTutorialBullet2.h" #include "../Collision/CollisionTutorialBullet2.h"
#include "../GyroscopicDemo/GyroscopicSetup.h" #include "../GyroscopicDemo/GyroscopicSetup.h"
#include "../Constraints/Dof6Spring2Setup.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 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(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(0,"Tutorial"),
ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY), 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), 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 (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).", ExampleEntry(1,"URDF (MultiBody)", "Import a URDF file and create a single multibody (btMultiBody) with tree hierarchy of links (mobilizers).",
ImportURDFCreateFunc, 1), ImportURDFCreateFunc, 1),
ExampleEntry(1,"SDF (MultiBody)", "Import an SDF file, create multiple multibodies etc", ImportSDFCreateFunc),
ExampleEntry(0,"Vehicles"), ExampleEntry(0,"Vehicles"),
ExampleEntry(1,"Hinge2 Vehicle", "A rigid body chassis with 4 rigid body wheels attached by a btHinge2Constraint",Hinge2VehicleCreateFunc), ExampleEntry(1,"Hinge2 Vehicle", "A rigid body chassis with 4 rigid body wheels attached by a btHinge2Constraint",Hinge2VehicleCreateFunc),

302
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp Normal file
View File

@@ -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<std::string* > 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<std::string> gFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportSDFInternalData
{
ImportSDFInternalData()
:m_numMotors(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
};
ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper)
{
m_data = new ImportSDFInternalData;
(void)option;
// if (option==1)
// {
m_useMultiBody = true;
//
// } else
// {
// m_useMultiBody = false;
// }
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
{
gFileNameArray.clear();
//load additional urdf file names from file
FILE* f = fopen("urdf_files.txt","r");
if (f)
{
int result;
//warning: we don't avoid string buffer overflow in this basic example in fscanf
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("sdf_files.txt entry %s",fileName);
if (result==1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
fclose(f);
}
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("two_cubes.sdf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
{
count=0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
}
}
ImportSDFSetup::~ImportSDFSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
}
static btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
static btVector3 selectColor()
{
static int curColor = 0;
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
return color;
}
void ImportSDFSetup::setFileName(const char* urdfFileName)
{
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
}
void ImportSDFSetup::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(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; m<u2b.getNumModels();m++)
{
u2b.activateModel(m);
btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
mb = creation.getBulletMultiBody();
}
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();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;i<m_data->m_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);
}

8
examples/Importers/ImportSDFDemo/ImportSDFSetup.h Normal file
View File

@@ -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

58
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -133,11 +133,69 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
} }
BulletErrorLogger loggie; BulletErrorLogger loggie;
m_data->m_urdfParser.setParseSDF(false);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase); bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase);
return result; 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() const char* BulletURDFImporter::getPathPrefix()
{ {
return m_data->m_pathPrefix; return m_data->m_pathPrefix;

6
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -4,6 +4,7 @@
#include "URDFImporterInterface.h" #include "URDFImporterInterface.h"
///BulletURDFImporter can deal with URDF and (soon) SDF files
class BulletURDFImporter : public URDFImporterInterface class BulletURDFImporter : public URDFImporterInterface
{ {
@@ -18,6 +19,11 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); 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(); const char* getPathPrefix();
void printTree(); //for debugging void printTree(); //for debugging

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -163,7 +163,7 @@ static btVector4 colors[4] =
}; };
btVector3 selectColor() static btVector3 selectColor()
{ {
static int curColor = 0; static int curColor = 0;

2
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -17,6 +17,8 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0; 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; virtual const char* getPathPrefix()=0;
///return >=0 for the root link index, -1 if there is no root link ///return >=0 for the root link index, -1 if there is no root link

370
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -5,14 +5,29 @@
#include "urdfLexicalCast.h" #include "urdfLexicalCast.h"
UrdfParser::UrdfParser() UrdfParser::UrdfParser()
:m_parseSDF(false),
m_activeSdfModel(-1)
{ {
} }
UrdfParser::~UrdfParser() UrdfParser::~UrdfParser()
{ {
cleanModel(&m_urdf2Model);
for (int i=0;i<m_model.m_materials.size();i++) for (int i=0;i<m_tmpModels.size();i++)
{ {
UrdfMaterial** matPtr = m_model.m_materials.getAtIndex(i); cleanModel(m_tmpModels[i]);
}
m_sdfModels.clear();
m_tmpModels.clear();
}
void UrdfParser::cleanModel(UrdfModel* model)
{
for (int i=0;i<model->m_materials.size();i++)
{
UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
if (matPtr) if (matPtr)
{ {
UrdfMaterial* mat = *matPtr; UrdfMaterial* mat = *matPtr;
@@ -20,26 +35,25 @@ UrdfParser::~UrdfParser()
} }
} }
for (int i=0;i<m_model.m_links.size();i++) for (int i=0;i<model->m_links.size();i++)
{ {
UrdfLink** linkPtr = m_model.m_links.getAtIndex(i); UrdfLink** linkPtr = model->m_links.getAtIndex(i);
if (linkPtr) if (linkPtr)
{ {
UrdfLink* link = *linkPtr; UrdfLink* link = *linkPtr;
delete link; delete link;
} }
} }
for (int i=0;i<m_model.m_joints.size();i++) for (int i=0;i<model->m_joints.size();i++)
{ {
UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i); UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
if (jointPtr) if (jointPtr)
{ {
UrdfJoint* joint = *jointPtr; UrdfJoint* joint = *jointPtr;
delete joint; delete joint;
} }
} }
} }
static bool parseVector4(btVector4& vec4, const std::string& vector_str) 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"); logger->reportError("Inertial element must have a mass element");
return false; return false;
} }
if (!mass_xml->Attribute("value")) if (m_parseSDF)
{ {
logger->reportError("Inertial: mass element must have value attribute"); inertia.m_mass = urdfLexicalCast<double>(mass_xml->GetText());
return false; } else
} {
if (!mass_xml->Attribute("value"))
{
logger->reportError("Inertial: mass element must have value attribute");
return false;
}
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value")); inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml) if (!inertia_xml)
@@ -207,21 +226,45 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have inertia element"); logger->reportError("Inertial element must have inertia element");
return false; return false;
} }
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && if (m_parseSDF)
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && {
inertia_xml->Attribute("izz"))) TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
{ TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
return false; TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
} TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx")); TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy")); if (ixx && ixy && ixz && iyy && iyz && izz)
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz")); {
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy")); inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz")); inertia.m_ixy = urdfLexicalCast<double>(ixy->GetText());
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz")); inertia.m_ixz = urdfLexicalCast<double>(ixz->GetText());
inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
inertia.m_iyz = urdfLexicalCast<double>(iyz->GetText());
inertia.m_izz = urdfLexicalCast<double>(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<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
}
return true; return true;
} }
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger) 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") else if (type_name == "box")
{ {
geom.m_type = URDF_GEOM_BOX; geom.m_type = URDF_GEOM_BOX;
if (!shape->Attribute("size")) if (m_parseSDF)
{ {
logger->reportError("box requires a size attribute"); TiXmlElement* size = shape->FirstChildElement("size");
} else if (0==size)
{ {
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger); 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") else if (type_name == "cylinder")
{ {
@@ -294,9 +351,29 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
} }
else else
{ {
logger->reportError("Unknown geometry type:"); if (this->m_parseSDF)
logger->reportError(type_name.c_str()); {
return false; 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; return true;
@@ -331,7 +408,7 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
return true; 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(); visual.m_linkLocalFrame.setIdentity();
@@ -358,7 +435,8 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
// Material // Material
TiXmlElement *mat = config->FirstChildElement("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 // get material name
if (!mat->Attribute("name")) if (!mat->Attribute("name"))
@@ -377,7 +455,7 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
if (parseMaterial(visual.m_localMaterial, mat,logger)) if (parseMaterial(visual.m_localMaterial, mat,logger))
{ {
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); 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; visual.m_hasLocalMaterial = true;
} }
} }
@@ -386,7 +464,7 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
return true; 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"); const char* linkName = config->Attribute("name");
if (!linkName) if (!linkName)
@@ -435,7 +513,7 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
{ {
UrdfVisual visual; UrdfVisual visual;
if (parseVisual(visual, vis_xml,logger)) if (parseVisual(model, visual, vis_xml,logger))
{ {
link.m_visualArray.push_back(visual); 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 // every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations // local convenience data structure for keeping child->parent relations
btHashMap<btHashString,btHashString> parentLinkTree; btHashMap<btHashString,btHashString> parentLinkTree;
// loop through all joints, for every link, assign children links and children joints // loop through all joints, for every link, assign children links and children joints
for (int i=0;i<m_model.m_joints.size();i++) for (int i=0;i<model.m_joints.size();i++)
{ {
UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i); UrdfJoint** jointPtr = model.m_joints.getAtIndex(i);
if (jointPtr) if (jointPtr)
{ {
UrdfJoint* joint = *jointPtr; UrdfJoint* joint = *jointPtr;
@@ -698,7 +776,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
return false; return false;
} }
UrdfLink** childLinkPtr = m_model.m_links.find(joint->m_childLinkName.c_str()); UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
if (!childLinkPtr) if (!childLinkPtr)
{ {
logger->reportError("Cannot find child link for joint "); logger->reportError("Cannot find child link for joint ");
@@ -708,7 +786,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
} }
UrdfLink* childLink = *childLinkPtr; 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) if (!parentLinkPtr)
{ {
logger->reportError("Cannot find parent link for a joint"); 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' //search for children that have no parent, those are 'root'
for (int i=0;i<m_model.m_links.size();i++) for (int i=0;i<model.m_links.size();i++)
{ {
UrdfLink** linkPtr = m_model.m_links.getAtIndex(i); UrdfLink** linkPtr = model.m_links.getAtIndex(i);
btAssert(linkPtr); btAssert(linkPtr);
if (linkPtr) if (linkPtr)
{ {
@@ -739,18 +817,18 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
if (!link->m_parentLink) if (!link->m_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"); 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"); logger->reportError("URDF without root link found");
return false; return false;
@@ -785,7 +863,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
logger->reportError("Expected a name for robot"); logger->reportError("Expected a name for robot");
return false; 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); 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) if (mat)
{ {
logger->reportWarning("Duplicate material"); logger->reportWarning("Duplicate material");
} else } 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; 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 name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str()); 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); UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) 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) if (mat && *mat)
{ {
vis.m_localMaterial = **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 } 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"); logger->reportWarning("No links found in URDF file");
return false; return false;
@@ -867,7 +945,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (parseJoint(*joint, joint_xml,logger)) 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 '%s' is not unique.");
logger->reportError(joint->m_name.c_str()); logger->reportError(joint->m_name.c_str());
@@ -875,7 +953,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
} }
else else
{ {
m_model.m_joints.insert(joint->m_name.c_str(),joint); m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
} }
} }
else 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) if (!ok)
{ {
return false; return false;
@@ -893,9 +971,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (forceFixedBase) if (forceFixedBase)
{ {
for (int i=0;i<m_model.m_rootLinks.size();i++) for (int i=0;i<m_urdf2Model.m_rootLinks.size();i++)
{ {
UrdfLink* link(m_model.m_rootLinks.at(i)); UrdfLink* link(m_urdf2Model.m_rootLinks.at(i));
link->m_inertia.m_mass = 0.0; link->m_inertia.m_mass = 0.0;
link->m_inertia.m_ixx = 0.0; link->m_inertia.m_ixx = 0.0;
link->m_inertia.m_ixy = 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; 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;i<link->m_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;
}

78
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -42,7 +42,9 @@ enum UrdfGeomTypes
URDF_GEOM_SPHERE=2, URDF_GEOM_SPHERE=2,
URDF_GEOM_BOX, URDF_GEOM_BOX,
URDF_GEOM_CYLINDER, URDF_GEOM_CYLINDER,
URDF_GEOM_MESH URDF_GEOM_MESH,
URDF_GEOM_PLANE,
}; };
@@ -57,6 +59,8 @@ struct UrdfGeometry
double m_cylinderRadius; double m_cylinderRadius;
double m_cylinderLength; double m_cylinderLength;
btVector3 m_planeNormal;
std::string m_meshFileName; std::string m_meshFileName;
btVector3 m_meshScale; btVector3 m_meshScale;
}; };
@@ -134,32 +138,88 @@ struct UrdfModel
class UrdfParser class UrdfParser
{ {
protected: protected:
UrdfModel m_urdf2Model;
btAlignedObjectArray<UrdfModel*> m_sdfModels;
btAlignedObjectArray<UrdfModel*> m_tmpModels;
bool m_parseSDF;
int m_activeSdfModel;
void cleanModel(UrdfModel* model);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger); bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, 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 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 parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger); bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger); bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
bool parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger); bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
UrdfModel m_model;
public: public:
UrdfParser(); UrdfParser();
virtual ~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() UrdfModel& getModel()
{ {
return m_model; if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
} }
}; };