diff --git a/sdf/worlds/pr2.world b/sdf/worlds/pr2.world
index 01e261b0d8..846ae27f05 100644
--- a/sdf/worlds/pr2.world
+++ b/sdf/worlds/pr2.world
@@ -33,7 +33,8 @@
-
+
+
diff --git a/src/Server.cc b/src/Server.cc
index a6f4bb8c2d..f5d8fd6a30 100644
--- a/src/Server.cc
+++ b/src/Server.cc
@@ -93,7 +93,7 @@ bool Server::Load(const std::string &_filename)
gzthrow("Failed to load the World\n" << e);
}
- worldElem = sdf->root->GetNextElement("world", worldElem);
+ worldElem = worldElem->GetNextElement();
}
return true;
diff --git a/src/common/Mesh.cc b/src/common/Mesh.cc
index f402a1b618..a04458fd99 100644
--- a/src/common/Mesh.cc
+++ b/src/common/Mesh.cc
@@ -669,9 +669,16 @@ void SubMesh::RecalculateNormals()
math::Vector3 v2 = this->vertices[this->indices[i+1]];
math::Vector3 v3 = this->vertices[this->indices[i+2]];
math::Vector3 n = math::Vector3::GetNormal(v1, v2, v3);
- this->normals[this->indices[i]] += n;
- this->normals[this->indices[i+1]] += n;
- this->normals[this->indices[i+2]] += n;
+
+ for (unsigned int j=0; j< this->vertices.size(); j++)
+ {
+ if (this->vertices[j] == v1 ||
+ this->vertices[j] == v2 ||
+ this->vertices[j] == v3)
+ {
+ this->normals[j] += n;
+ }
+ }
}
// Normalize the results
diff --git a/src/gui/InsertModelWidget.cc b/src/gui/InsertModelWidget.cc
index 87699918d5..3b5c2b3685 100644
--- a/src/gui/InsertModelWidget.cc
+++ b/src/gui/InsertModelWidget.cc
@@ -186,10 +186,10 @@ void InsertModelWidget::OnModelSelection(QTreeWidgetItem *_item, int /*_column*/
this->visuals.push_back(visVisual);
- visualElem = linkElem->GetNextElement("visual", visualElem);
+ visualElem = visualElem->GetNextElement();
}
- linkElem = modelElem->GetNextElement("link", linkElem);
+ linkElem = linkElem->GetNextElement();
}
msgs::Selection selectMsg;
diff --git a/src/physics/Link.cc b/src/physics/Link.cc
index 45c013a3d1..d63576b9cf 100644
--- a/src/physics/Link.cc
+++ b/src/physics/Link.cc
@@ -123,7 +123,7 @@ void Link::Load( sdf::ElementPtr &_sdf )
this->visPub->Publish(msg);
this->visuals.push_back(msg.name());
- visualElem = this->sdf->GetNextElement("visual", visualElem);
+ visualElem = visualElem->GetNextElement();
}
}
@@ -135,7 +135,7 @@ void Link::Load( sdf::ElementPtr &_sdf )
{
// Create and Load a collision, which will belong to this body.
this->LoadCollision(collisionElem);
- collisionElem = this->sdf->GetNextElement("collision", collisionElem);
+ collisionElem = collisionElem->GetNextElement();
}
}
@@ -145,7 +145,7 @@ void Link::Load( sdf::ElementPtr &_sdf )
while (sensorElem)
{
this->LoadSensor(sensorElem);
- sensorElem = this->sdf->GetNextElement("sensor", sensorElem);
+ sensorElem = sensorElem->GetNextElement();
}
}
}
@@ -294,7 +294,7 @@ void Link::UpdateParameters( sdf::ElementPtr &_sdf )
this->visPub->Publish(msg);
- visualElem = this->sdf->GetNextElement("visual", visualElem);
+ visualElem = visualElem->GetNextElement();
}
}
@@ -308,7 +308,7 @@ void Link::UpdateParameters( sdf::ElementPtr &_sdf )
if (collision)
collision->UpdateParameters(collisionElem);
- collisionElem = this->sdf->GetNextElement("collision", collisionElem);
+ collisionElem = collisionElem->GetNextElement();
}
}
}
diff --git a/src/physics/Model.cc b/src/physics/Model.cc
index 33a6d9614a..4e9227bdcc 100644
--- a/src/physics/Model.cc
+++ b/src/physics/Model.cc
@@ -111,7 +111,7 @@ void Model::Load( sdf::ElementPtr &_sdf )
// bodies collisionetries
link->Load(linkElem);
- linkElem = _sdf->GetNextElement("link", linkElem);
+ linkElem = linkElem->GetNextElement();
}
}
@@ -122,7 +122,7 @@ void Model::Load( sdf::ElementPtr &_sdf )
while (jointElem)
{
this->LoadJoint(jointElem);
- jointElem = _sdf->GetNextElement("joint", jointElem);
+ jointElem = jointElem->GetNextElement();
}
}
@@ -133,7 +133,7 @@ void Model::Load( sdf::ElementPtr &_sdf )
while (pluginElem)
{
this->LoadPlugin(pluginElem);
- pluginElem = _sdf->GetNextElement("plugin", pluginElem);
+ pluginElem = pluginElem->GetNextElement();
}
}
}
@@ -304,7 +304,7 @@ void Model::UpdateParameters( sdf::ElementPtr &_sdf )
LinkPtr link = boost::shared_dynamic_cast(
this->GetChild(linkElem->GetValueString("name")));
link->UpdateParameters(linkElem);
- linkElem = _sdf->GetNextElement("link", linkElem);
+ linkElem = linkElem->GetNextElement();
}
}
/*
@@ -316,7 +316,7 @@ void Model::UpdateParameters( sdf::ElementPtr &_sdf )
{
JointPtr joint = boost::shared_dynamic_cast(this->GetChild(jointElem->GetValueString("name")));
joint->UpdateParameters(jointElem);
- jointElem = _sdf->GetNextElement("joint", jointElem);
+ jointElem = jointElem->GetNextElement();
}
}
*/
diff --git a/src/physics/World.cc b/src/physics/World.cc
index 310909ab97..89a49cd28c 100644
--- a/src/physics/World.cc
+++ b/src/physics/World.cc
@@ -438,7 +438,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent )
msgs::Light *lm = this->sceneMsg.add_light();
lm->CopyFrom( msgs::LightFromSDF(childElem) );
- childElem = _sdf->GetNextElement("light", childElem);
+ childElem = childElem->GetNextElement();
}
}
@@ -453,7 +453,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent )
// TODO : Put back in the ability to nest models. We should do this
// without requiring a joint.
- childElem = _sdf->GetNextElement("model", childElem);
+ childElem = childElem->GetNextElement();
}
}
@@ -464,7 +464,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent )
while (pluginElem)
{
this->LoadPlugin(pluginElem);
- pluginElem = _sdf->GetNextElement("plugin", pluginElem);
+ pluginElem = pluginElem->GetNextElement();
}
}
}
diff --git a/src/physics/server.cc b/src/physics/server.cc
index 45de436433..59f075d2a4 100644
--- a/src/physics/server.cc
+++ b/src/physics/server.cc
@@ -114,7 +114,7 @@ void Load()
gzthrow("Failed to load the World\n" << e);
}
- worldElem = sdf->root->GetNextElement("world", worldElem);
+ worldElem = worldElem->GetNextElement();
}
gazebo::physics::init_worlds();
diff --git a/src/sdf/interface/SDF.cc b/src/sdf/interface/SDF.cc
index 2af25e6b0f..648777ac41 100644
--- a/src/sdf/interface/SDF.cc
+++ b/src/sdf/interface/SDF.cc
@@ -428,21 +428,33 @@ ElementPtr Element::GetFirstElement() const
return this->elements.front();
}
-ElementPtr Element::GetNextElement(const std::string &_name,
- const ElementPtr &_elem) const
+ElementPtr Element::GetNextElement(const std::string &_name) const
{
- ElementPtr_V::const_iterator iter;
- iter = std::find(this->elements.begin(), this->elements.end(), _elem);
-
- for (iter++; iter != this->elements.end(); iter++)
+ if (this->parent)
{
- if ( (*iter)->GetName() == _name )
- return (*iter);
+ ElementPtr_V::const_iterator iter;
+ iter = std::find(this->parent->elements.begin(),
+ this->parent->elements.end(), shared_from_this());
+
+ if (iter == this->parent->elements.end())
+ return ElementPtr();
+
+ for (iter++; iter != this->parent->elements.end(); iter++)
+ {
+ if ( (*iter)->GetName() == _name )
+ return (*iter);
+ }
}
return ElementPtr();
}
+ElementPtr Element::GetNextElement() const
+{
+ return this->GetNextElement(this->name);
+}
+
+
boost::shared_ptr Element::GetOrCreateElement(const std::string &_name)
{
if (this->HasElement(_name))
diff --git a/src/sdf/interface/SDF.hh b/src/sdf/interface/SDF.hh
index 30e6a91a17..8c658e4560 100644
--- a/src/sdf/interface/SDF.hh
+++ b/src/sdf/interface/SDF.hh
@@ -106,8 +106,10 @@ namespace sdf
public: ElementPtr GetElement(const std::string &_name) const;
public: ElementPtr GetFirstElement() const;
- public: ElementPtr GetNextElement(const std::string &_name,
- const ElementPtr &_elem) const;
+
+ public: ElementPtr GetNextElement(const std::string &_name) const;
+ public: ElementPtr GetNextElement() const;
+
public: ElementPtr GetOrCreateElement(const std::string &_name);
public: ElementPtr AddElement(const std::string &_name);
diff --git a/src/sensors/ContactSensor.cc b/src/sensors/ContactSensor.cc
index 6c81cd6634..4cf85473c4 100644
--- a/src/sensors/ContactSensor.cc
+++ b/src/sensors/ContactSensor.cc
@@ -110,8 +110,7 @@ void ContactSensor::Load()
this->connections.push_back(collision->ConnectContact(
boost::bind(&ContactSensor::OnContact, this, _1, _2)));
}
- collisionElem = this->sdf->GetElement("contact")->GetNextElement(
- "collision", collisionElem);
+ collisionElem = collisionElem->GetNextElement();
}
}
diff --git a/src/sensors/Sensor.cc b/src/sensors/Sensor.cc
index c59dabee1f..ca5002dbe3 100644
--- a/src/sensors/Sensor.cc
+++ b/src/sensors/Sensor.cc
@@ -102,7 +102,7 @@ void Sensor::Init()
while (pluginElem)
{
this->LoadPlugin(pluginElem);
- pluginElem = this->sdf->GetNextElement("plugin", pluginElem);
+ pluginElem = pluginElem->GetNextElement();
}
}
//SensorManager::Instance()->AddSensor(shared_from_this());
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
index 89135cec96..88de4b9705 100644
--- a/tools/CMakeLists.txt
+++ b/tools/CMakeLists.txt
@@ -20,3 +20,7 @@ add_executable( gzstats gzstats.cc )
target_link_libraries(gzstats gazebo_msgs gazebo_common gazebo_transport )
INSTALL_EXECUTABLE( gzstats )
+add_executable( sdf2pov sdf2pov.cc )
+target_link_libraries(sdf2pov gazebo_common gazebo_sdf_parser)
+INSTALL_EXECUTABLE( sdf2pov )
+
diff --git a/tools/sdf2pov.cc b/tools/sdf2pov.cc
new file mode 100644
index 0000000000..aa4cb4c89b
--- /dev/null
+++ b/tools/sdf2pov.cc
@@ -0,0 +1,415 @@
+#include "sdf/sdf.h"
+#include "sdf/sdf_parser.h"
+#include "math/Pose.hh"
+#include "common/Console.hh"
+#include "common/MeshManager.hh"
+#include "common/Mesh.hh"
+#include "common/Material.hh"
+
+std::vector params;
+
+// Convert radians to degrees
+#define RTOD(r) ((r) * 180 / M_PI)
+
+// Convert degrees to radians
+#define DTOR(d) ((d) * M_PI / 180)
+
+void help()
+{
+ printf("help");
+}
+
+bool parse(int argc, char **argv)
+{
+ if (argc == 1 || std::string(argv[1]) == "help")
+ {
+ help();
+ return false;
+ }
+
+ // Get parameters from command line
+ for (int i=1; i < argc; i++)
+ {
+ std::string p = argv[i];
+ boost::trim(p);
+ params.push_back( p );
+ }
+
+ // Get parameters from stdin
+ if (!isatty(fileno(stdin)))
+ {
+ char str[1024];
+ while (!feof(stdin))
+ {
+ if (fgets(str, 1024, stdin)==NULL)
+ break;
+
+ if (feof(stdin))
+ break;
+ std::string p = str;
+ boost::trim(p);
+ params.push_back(p);
+ }
+ }
+
+ return true;
+}
+
+gazebo::math::Vector3 Convert(const gazebo::math::Vector3 &_vec)
+{
+ gazebo::math::Vector3 result;
+ gazebo::math::Quaternion rot1(0,M_PI*.5,0);
+ gazebo::math::Quaternion rot2(0,-M_PI*.5,0);
+
+ result = rot1.RotateVector(_vec);
+ result = rot2.RotateVector(result);
+
+ return result;
+}
+
+void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
+{
+ const gazebo::common::Mesh *mesh;
+
+ mesh = gazebo::common::MeshManager::Instance()->Load(
+ _elem->GetValueString("filename"));
+
+ const_cast(mesh)->RecalculateNormals();
+
+
+ for (unsigned int i=0; i < mesh->GetSubMeshCount(); i++)
+ {
+ const gazebo::common::SubMesh *subMesh = mesh->GetSubMesh(i);
+ printf("mesh2 {\n");
+
+ printf(" vertex_vectors {\n");
+ printf(" %d,\n ", subMesh->GetVertexCount());
+ for (unsigned int v=0; v < subMesh->GetVertexCount(); v++)
+ {
+ gazebo::math::Vector3 vert = subMesh->GetVertex(v);
+ //vert = _pose.CoordPositionAdd(vert);
+ printf("<%f, %f, %f>, ", vert.x, vert.y, vert.z);
+ }
+ printf(" }\n");
+
+ printf(" normal_vectors {\n");
+ printf(" %d,\n ", subMesh->GetNormalCount());
+ for (unsigned int n=0; n < subMesh->GetNormalCount(); n++)
+ {
+ gazebo::math::Vector3 norm = subMesh->GetNormal(n);
+ printf("<%f, %f, %f>, ", norm.x, norm.y, norm.z);
+ }
+ printf(" }\n");
+
+ printf(" uv_vectors {\n");
+ printf(" %d,\n", subMesh->GetTexCoordCount());
+ for (unsigned int j=0; j < subMesh->GetTexCoordCount(); j++)
+ {
+ printf(" <%f, %f>,\n", subMesh->GetTexCoord(j).x,
+ 1.0 - subMesh->GetTexCoord(j).y);
+ }
+ printf(" }\n");
+
+ const gazebo::common::Material *mat = mesh->GetMaterial(
+ subMesh->GetMaterialIndex());
+ if (mat)
+ {
+ printf(" texture_list {\n");
+ printf(" 1,\n");
+ printf(" texture {\n");
+ if (!mat->GetTextureImage().empty())
+ {
+ printf(" uv_mapping pigment { image_map { tiff \"/home/nkoenig/%s\" } }\n",
+ mat->GetTextureImage().c_str());
+ }
+ else
+ {
+ printf(" pigment { color rgb <%f, %f, %f> }\n",
+ mat->GetDiffuse().R(),mat->GetDiffuse().G(),mat->GetDiffuse().B());
+ }
+
+ printf(" finish {\n");
+ printf(" ambient color rgb <%f, %f, %f>\n",
+ mat->GetAmbient().R(), mat->GetAmbient().G(), mat->GetAmbient().B());
+ printf(" specular %f\n", 1.0);//mat->GetSpecular().R());
+ printf(" }\n");
+
+ printf(" }\n");
+ printf(" }\n");
+
+ }
+
+ printf(" face_indices {\n");
+ printf(" %d,\n", subMesh->GetIndexCount() / 3);
+ for (unsigned int j=0; j < subMesh->GetIndexCount(); j+=3)
+ {
+ if (mat)
+ {
+ printf(" <%d, %d, %d>,0\n", subMesh->GetIndex(j),
+ subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
+ }
+ else
+ {
+ printf(" <%d, %d, %d>\n", subMesh->GetIndex(j),
+ subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
+ }
+
+ }
+ printf(" }\n");
+
+ printf(" normal_indices {\n");
+ printf(" %d,\n", subMesh->GetIndexCount() / 3);
+ for (unsigned int j=0; j < subMesh->GetIndexCount(); j+=3)
+ {
+ printf(" <%d, %d, %d>,\n", subMesh->GetIndex(j),
+ subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
+ }
+ printf(" }\n");
+/*
+ printf(" uv_indices {\n");
+ printf(" %d,\n", subMesh->GetIndexCount() / 3);
+ for (unsigned int j=0; j < subMesh->GetIndexCount(); j+=3)
+ {
+ printf(" <%d, %d, %d>,\n", subMesh->GetIndex(j),
+ subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
+ }
+ printf(" }\n");
+ */
+
+ gazebo::math::Vector3 rpy = _pose.rot.GetAsEuler();
+ printf(" translate <%f, %f, %f>\n",_pose.pos.x, _pose.pos.y, _pose.pos.z);
+ printf(" rotate <%f, %f, %f>\n",RTOD(rpy.x), RTOD(rpy.y), RTOD(rpy.z));
+
+ printf("}\n");
+ }
+}
+
+void ProcessLight(sdf::ElementPtr _elem)
+{
+ gazebo::math::Pose pose;
+ gazebo::common::Color diffuse, specular;
+
+ pose = _elem->GetOrCreateElement("origin")->GetValuePose("pose");
+ diffuse = _elem->GetElement("diffuse")->GetValueColor("rgba");
+ specular = _elem->GetElement("specular")->GetValueColor("rgba");
+ //double fadeDist = _elem->GetElement("attenuation")->GetValueDouble("range");
+ //double constant = _elem->GetElement("attenuation")->GetValueDouble("constant");
+ //double linear = _elem->GetElement("attenuation")->GetValueDouble("linear");
+ //double quadratic = _elem->GetElement("attenuation")->GetValueDouble("quadratic");
+
+ printf("light_source {\n");
+ printf(" <%f, %f, %f>, rgb <%f, %f, %f>\n",
+ pose.pos.x, pose.pos.y, pose.pos.z,
+ diffuse.R(), diffuse.G(), diffuse.B());
+
+ std::string type = _elem->GetValueString("type");
+ if (type == "point")
+ {
+ //printf(" pointlight\n");
+ }
+ else if (type == "directional")
+ {
+ printf(" parallel\n");
+ }
+ else if (type == "spot")
+ {
+ double innerAngle, outerAngle, falloff;
+ innerAngle = _elem->GetElement("spot")->GetValueDouble("inner_angle");
+ outerAngle = _elem->GetElement("spot")->GetValueDouble("outer_angle");
+ falloff = _elem->GetElement("spot")->GetValueDouble("falloff");
+ printf(" spotlight\n");
+ printf(" radius %f\n", RTOD(innerAngle));
+ printf(" falloff %f\n", RTOD(outerAngle));
+ printf(" tightness %f\n", falloff);
+ }
+
+ if (_elem->HasElement("direction"))
+ {
+ gazebo::math::Vector3 dir =
+ _elem->GetElement("direction")->GetValueVector3("xyz");
+ double d = pose.pos.GetDistToPlane(dir, gazebo::math::Vector3(0,0,1), 0);
+ double t;
+ t = atan2(dir.x, dir.z*-1);
+ double x = sin(t) * d;
+ t = atan2(dir.y, dir.z*-1);
+ double y = sin(t) * d;
+ printf(" point_at <%f, %f, 0.0>\n",x,y);
+ }
+
+ printf("}\n");
+}
+
+void ProcessScene(sdf::ElementPtr _elem)
+{
+ gazebo::common::Color color;
+ if (_elem->HasElement("background"))
+ {
+ color = _elem->GetElement("background")->GetValueColor("rgba");
+ printf("background { rgb <%f, %f, %f> }\n", color.R(), color.G(), color.B());
+ }
+
+ if (_elem->HasElement("ambient"))
+ {
+ color = _elem->GetElement("ambient")->GetValueColor("rgba");
+ //printf("global_settings { ambient_light rgb <%f, %f, %f> }\n",
+ //color.R(), color.G(), color.B());
+ }
+
+ int count = 35;
+ //int count = 1600;
+
+ int recursionLimit = 3;
+ //int recursionLimit = 20;
+
+ float errorBound = 1.8;
+ //float errorBound = 1.0;
+
+ // Note: Extreme quality
+ printf("global_settings { radiosity{\n");
+ printf(" count %d\n", count);
+ printf(" recursion_limit %d\n", recursionLimit);
+ printf(" error_bound %f\n", errorBound);
+
+ printf("} }\n");
+}
+
+void ProcessGeometry(sdf::ElementPtr _elem, const gazebo::math::Pose &_pose)
+{
+ if (_elem->HasElement("plane"))
+ {
+ sdf::ElementPtr planeElem = _elem->GetElement("plane");
+ gazebo::math::Vector3 normal = planeElem->GetValueVector3("normal");
+ printf("plane {\n");
+ printf(" <%f, %f, %f>, 0\n", normal.x, normal.y, normal.z);
+ printf(" texture {pigment { color Yellow } }\n");
+ printf("}\n");
+ }
+ else if (_elem->HasElement("box"))
+ {
+ sdf::ElementPtr boxElem = _elem->GetElement("box");
+ gazebo::math::Vector3 size = boxElem->GetValueVector3("size");
+ printf("box {\n");
+ gazebo::math::Vector3 corner1 = _pose.pos - (size/2.0);
+ gazebo::math::Vector3 corner2 = _pose.pos + (size/2.0);
+ corner1 = _pose.rot.RotateVector(corner1);
+ corner2 = _pose.rot.RotateVector(corner2);
+ printf(" <%f, %f, %f,>, <%f, %f, %f>\n",corner1.x, corner1.y, corner1.z,
+ corner2.x, corner2.y, corner2.z);
+ printf("}\n");
+ }
+ else if (_elem->HasElement("cylinder"))
+ {
+ sdf::ElementPtr cylinderElem = _elem->GetElement("cylinder");
+ double radius = cylinderElem->GetValueDouble("radius");
+ double length = cylinderElem->GetValueDouble("length");
+ gazebo::math::Vector3 capPoint = _pose.pos;
+ capPoint.z += length;
+ capPoint = _pose.rot.RotateVector(capPoint);
+ printf("cylinder {\n");
+ printf(" <%f, %f, %f>, <%f, %f, %f>, %f\n",
+ _pose.pos.x, _pose.pos.y, _pose.pos.z,
+ capPoint.x, capPoint.y, capPoint.z, radius);
+ printf("}\n");
+ }
+ else if (_elem->HasElement("sphere"))
+ {
+ sdf::ElementPtr sphereElem = _elem->GetElement("sphere");
+ double radius = sphereElem->GetValueDouble("radius");
+
+ printf("sphere {\n");
+ printf(" <%f, %f, %f> %f\n",_pose.pos.x, _pose.pos.y, _pose.pos.z, radius);
+ printf("}\n");
+ }
+ else if (_elem->HasElement("mesh"))
+ {
+ ProcessMesh(_elem->GetElement("mesh"), _pose);
+ }
+
+}
+
+
+int main(int argc, char **argv)
+{
+ if (!parse(argc, argv))
+ return 0;
+
+ // Load the world file
+ sdf::SDFPtr sdf(new sdf::SDF);
+ if (!sdf::init(sdf))
+ {
+ gzerr << "Unable to initialize sdf\n";
+ return false;
+ }
+
+ if (!sdf::readFile(params[0], sdf))
+ {
+ gzerr << "Unable to read sdf file[" << params[0] << "]\n";
+ return false;
+ }
+
+ printf("#include \"colors.inc\"\n");
+ printf("#include \"shapes.inc\"\n");
+ printf("camera {\n");
+ printf(" location <-2, 0, 2>\n");
+ printf(" look_at <0, 0, 1>\n");
+ printf(" sky <0, 0, 1>\n");
+ printf(" direction <1, 0, 0>\n");
+ printf(" up <0, 0, -1>\n");
+ printf(" right <0, 1, 0>\n");
+ printf("}\n");
+
+ sdf::ElementPtr root = sdf->root;
+
+ gazebo::math::Pose modelPose, linkPose, visualPose;
+
+ sdf::ElementPtr worldElem = root->GetElement("world");
+ while (worldElem)
+ {
+ if (worldElem->HasElement("scene"))
+ ProcessScene(worldElem->GetElement("scene"));
+
+ if (worldElem->HasElement("light"))
+ {
+ sdf::ElementPtr lightElem = worldElem->GetElement("light");
+ while(lightElem)
+ {
+ ProcessLight(lightElem);
+ lightElem = lightElem->GetNextElement();
+ }
+ }
+
+ sdf::ElementPtr modelElem = worldElem->GetElement("model");
+ while (modelElem)
+ {
+ modelPose = modelElem->GetOrCreateElement("origin")->GetValuePose("pose");
+
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ while (linkElem)
+ {
+ linkPose = linkElem->GetOrCreateElement("origin")->GetValuePose("pose");
+
+ if (linkElem->HasElement("visual"))
+ {
+ sdf::ElementPtr visualElem = linkElem->GetElement("visual");
+ while (visualElem)
+ {
+ visualPose =
+ visualElem->GetOrCreateElement("origin")->GetValuePose("pose");
+ //visualPose = (visualPose + linkPose) + modelPose;
+ visualPose = modelPose + (linkPose + visualPose);
+ //visualPose.pos = modelPose.pos + linkPose.pos + visualPose.pos;
+ //visualPose.rot = visualPose.rot * linkPose.rot * modelPose.rot;
+ sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
+ ProcessGeometry(geomElem, visualPose);
+
+ visualElem = visualElem->GetNextElement();
+ }
+ }
+ linkElem = linkElem->GetNextElement();
+ }
+ modelElem = modelElem->GetNextElement();
+ }
+ worldElem = worldElem->GetNextElement();
+ }
+
+}