bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: trimming collada by not writing null transforms; shifted away from deprecated createAndPlace API; added option to add transform before an existing element

This commit is contained in:
tfield 2010-05-17 04:52:20 +00:00
parent c8db59bef0
commit 01196304b6
2 changed files with 126 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
collada_urdf/include/collada_urdf/collada_writer.h
View File

@ -110,8 +110,8 @@ private:
void addBindings(SCENE const& scene); void addBindings(SCENE const& scene);
domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position); domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position, daeElementRef before = NULL, bool ignore_zero_translations = false);
domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r); domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r, daeElementRef before = NULL, bool ignore_zero_rotations = false);
std::string getTimeStampString() const; std::string getTimeStampString() const;

232
collada_urdf/src/collada_writer.cpp
View File

@ -106,45 +106,45 @@ void ColladaWriter::initDocument(string const& documentName) {
dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML");
// Create asset elements // Create asset elements
domAssetRef asset = daeSafeCast<domAsset>(dom_->createAndPlace(COLLADA_ELEMENT_ASSET)); domAssetRef asset = daeSafeCast<domAsset>(dom_->add(COLLADA_ELEMENT_ASSET));
{ {
string date = getTimeStampString(); string date = getTimeStampString();
domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>(asset->createAndPlace(COLLADA_ELEMENT_CREATED)); domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>(asset->add(COLLADA_ELEMENT_CREATED));
created->setValue(date.c_str()); created->setValue(date.c_str());
domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>(asset->createAndPlace(COLLADA_ELEMENT_MODIFIED)); domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>(asset->add(COLLADA_ELEMENT_MODIFIED));
modified->setValue(date.c_str()); modified->setValue(date.c_str());
domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>(asset->createAndPlace(COLLADA_TYPE_CONTRIBUTOR)); domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>(asset->add(COLLADA_ELEMENT_CONTRIBUTOR));
domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>(contrib->createAndPlace(COLLADA_ELEMENT_AUTHORING_TOOL)); domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>(contrib->add(COLLADA_ELEMENT_AUTHORING_TOOL));
authoringtool->setValue("URDF Collada Writer"); authoringtool->setValue("URDF Collada Writer");
domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>(asset->createAndPlace(COLLADA_ELEMENT_UNIT)); domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>(asset->add(COLLADA_ELEMENT_UNIT));
units->setMeter(1); units->setMeter(1);
units->setName("meter"); units->setName("meter");
domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>(asset->createAndPlace(COLLADA_ELEMENT_UP_AXIS)); domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>(asset->add(COLLADA_ELEMENT_UP_AXIS));
zup->setValue(UP_AXIS_Z_UP); zup->setValue(UP_AXIS_Z_UP);
} }
// Create top-level elements // Create top-level elements
scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->add(COLLADA_ELEMENT_SCENE));
visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
visualScenesLib_->setId("vscenes"); visualScenesLib_->setId("vscenes");
geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
geometriesLib_->setId("geometries"); geometriesLib_->setId("geometries");
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kscenes"); kinematicsScenesLib_->setId("kscenes");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kmodels"); kinematicsModelsLib_->setId("kmodels");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->add(COLLADA_ELEMENT_LIBRARY_JOINTS));
jointsLib_->setId("joints"); jointsLib_->setId("joints");
physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
physicsScenesLib_->setId("physics_scenes"); physicsScenesLib_->setId("physics_scenes");
effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS)); effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
effectsLib_->setId("effects"); effectsLib_->setId("effects");
materialsLib_ = daeSafeCast<domLibrary_materials>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS)); materialsLib_ = daeSafeCast<domLibrary_materials>(dom_->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
materialsLib_->setId("materials"); materialsLib_->setId("materials");
} }
@ -152,30 +152,30 @@ ColladaWriter::SCENE ColladaWriter::createScene() {
SCENE s; SCENE s;
// Create visual scene // Create visual scene
s.vscene = daeSafeCast<domVisual_scene>(visualScenesLib_->createAndPlace(COLLADA_ELEMENT_VISUAL_SCENE)); s.vscene = daeSafeCast<domVisual_scene>(visualScenesLib_->add(COLLADA_ELEMENT_VISUAL_SCENE));
s.vscene->setId("vscene"); s.vscene->setId("vscene");
s.vscene->setName("URDF Visual Scene"); s.vscene->setName("URDF Visual Scene");
// Create instance visual scene // Create instance visual scene
s.viscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE)); s.viscene = daeSafeCast<domInstance_with_extra>(scene_->add(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE));
s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str()); s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str());
// Create kinematics scene // Create kinematics scene
s.kscene = daeSafeCast<domKinematics_scene>(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE)); s.kscene = daeSafeCast<domKinematics_scene>(kinematicsScenesLib_->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
s.kscene->setId("kscene"); s.kscene->setId("kscene");
s.kscene->setName("URDF Kinematics Scene"); s.kscene->setName("URDF Kinematics Scene");
// Create instance kinematics scene // Create instance kinematics scene
s.kiscene = daeSafeCast<domInstance_kinematics_scene>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE)); s.kiscene = daeSafeCast<domInstance_kinematics_scene>(scene_->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE));
s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str()); s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str());
// Create physics scene // Create physics scene
s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE)); s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->add(COLLADA_ELEMENT_PHYSICS_SCENE));
s.pscene->setId("pscene"); s.pscene->setId("pscene");
s.pscene->setName("URDF Physics Scene"); s.pscene->setName("URDF Physics Scene");
// Create instance physics scene // Create instance physics scene
s.piscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE)); s.piscene = daeSafeCast<domInstance_with_extra>(scene_->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE));
s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str()); s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str());
return s; return s;
@ -183,10 +183,10 @@ ColladaWriter::SCENE ColladaWriter::createScene() {
void ColladaWriter::setupPhysics(SCENE const& scene) { void ColladaWriter::setupPhysics(SCENE const& scene) {
// <technique_common> // <technique_common>
domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{ {
// <gravity>0 0 0 // <gravity>0 0 0
domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->createAndPlace(COLLADA_ELEMENT_GRAVITY)); domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
g->getValue().set3(0.0, 0.0, 0.0); g->getValue().set3(0.0, 0.0, 0.0);
// </gravity> // </gravity>
} }
@ -210,7 +210,7 @@ void ColladaWriter::addGeometries() {
urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale
// <geometry id="g1.link0.geom0"> // <geometry id="g1.link0.geom0">
domGeometryRef geometry = daeSafeCast<domGeometry>(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY)); domGeometryRef geometry = daeSafeCast<domGeometry>(geometriesLib_->add(COLLADA_ELEMENT_GEOMETRY));
string geometry_id = string("g1.link") + boost::lexical_cast<string>(link_num) + string(".geom0"); string geometry_id = string("g1.link") + boost::lexical_cast<string>(link_num) + string(".geom0");
geometry->setId(geometry_id.c_str()); geometry->setId(geometry_id.c_str());
{ {
@ -296,18 +296,18 @@ bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource con
void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElementRef parent, string const& geometry_id) { void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElementRef parent, string const& geometry_id) {
// <mesh> // <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH)); domMeshRef mesh = daeSafeCast<domMesh>(parent->add(COLLADA_ELEMENT_MESH));
{ {
unsigned int num_vertices = stl_mesh->vertices.size(); unsigned int num_vertices = stl_mesh->vertices.size();
unsigned int num_indices = stl_mesh->indices.size(); unsigned int num_indices = stl_mesh->indices.size();
unsigned int num_faces = num_indices / 3; unsigned int num_faces = num_indices / 3;
// <source id="g1.link0.geom0.positions"> // <source id="g1.link0.geom0.positions">
domSourceRef positions_source = daeSafeCast<domSource>(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); domSourceRef positions_source = daeSafeCast<domSource>(mesh->add(COLLADA_ELEMENT_SOURCE));
positions_source->setId((geometry_id + string(".positions")).c_str()); positions_source->setId((geometry_id + string(".positions")).c_str());
{ {
// <float_array id="g1.link0.geom0.positions-array" count="4533" digits="6"> // <float_array id="g1.link0.geom0.positions-array" count="4533" digits="6">
domFloat_arrayRef positions_array = daeSafeCast<domFloat_array>(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY)); domFloat_arrayRef positions_array = daeSafeCast<domFloat_array>(positions_source->add(COLLADA_ELEMENT_FLOAT_ARRAY));
positions_array->setId((geometry_id + string(".positions-array")).c_str()); positions_array->setId((geometry_id + string(".positions-array")).c_str());
positions_array->setCount(num_vertices * 3); positions_array->setCount(num_vertices * 3);
positions_array->setDigits(6); // 6 decimal places positions_array->setDigits(6); // 6 decimal places
@ -320,10 +320,10 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElement
// </float_array> // </float_array>
// <technique_common> // <technique_common>
domSource::domTechnique_commonRef source_tech = daeSafeCast<domSource::domTechnique_common>(positions_source->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domSource::domTechnique_commonRef source_tech = daeSafeCast<domSource::domTechnique_common>(positions_source->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{ {
// <accessor count="4533" source="#g1.link0.geom0.positions-array" stride="3"> // <accessor count="4533" source="#g1.link0.geom0.positions-array" stride="3">
domAccessorRef accessor = daeSafeCast<domAccessor>(source_tech->createAndPlace(COLLADA_ELEMENT_ACCESSOR)); domAccessorRef accessor = daeSafeCast<domAccessor>(source_tech->add(COLLADA_ELEMENT_ACCESSOR));
accessor->setCount(num_vertices / 3); accessor->setCount(num_vertices / 3);
accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array"))); accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array")));
accessor->setStride(3); accessor->setStride(3);
@ -331,9 +331,9 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElement
// <param name="X" type="float"/> // <param name="X" type="float"/>
// <param name="Y" type="float"/> // <param name="Y" type="float"/>
// <param name="Z" type="float"/> // <param name="Z" type="float"/>
domParamRef px = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float"); domParamRef px = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float"); domParamRef py = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float"); domParamRef pz = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
} }
// </accessor> // </accessor>
} }
@ -341,30 +341,30 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElement
} }
// <vertices id="vertices"> // <vertices id="vertices">
domVerticesRef vertices = daeSafeCast<domVertices>(mesh->createAndPlace(COLLADA_ELEMENT_VERTICES)); domVerticesRef vertices = daeSafeCast<domVertices>(mesh->add(COLLADA_ELEMENT_VERTICES));
string vertices_id = geometry_id + string(".vertices"); string vertices_id = geometry_id + string(".vertices");
vertices->setId(vertices_id.c_str()); vertices->setId(vertices_id.c_str());
{ {
// <input semantic="POSITION" source="#g1.link0.geom0.positions"/> // <input semantic="POSITION" source="#g1.link0.geom0.positions"/>
domInput_localRef vertices_input = daeSafeCast<domInput_local>(vertices->createAndPlace(COLLADA_ELEMENT_INPUT)); domInput_localRef vertices_input = daeSafeCast<domInput_local>(vertices->add(COLLADA_ELEMENT_INPUT));
vertices_input->setSemantic("POSITION"); vertices_input->setSemantic("POSITION");
vertices_input->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId()))); vertices_input->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId())));
} }
// </vertices> // </vertices>
// <triangles count="1511" material="mat0"> // <triangles count="1511" material="mat0">
domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->add(COLLADA_ELEMENT_TRIANGLES));
triangles->setCount(num_faces); triangles->setCount(num_faces);
triangles->setMaterial("mat0"); triangles->setMaterial("mat0");
{ {
// <input offset="0" semantic="VERTEX" source="#g1.link0.geom0/vertices" set="0"/> // <input offset="0" semantic="VERTEX" source="#g1.link0.geom0/vertices" set="0"/>
domInput_local_offsetRef vertex_offset = daeSafeCast<domInput_local_offset>(triangles->createAndPlace(COLLADA_ELEMENT_INPUT)); domInput_local_offsetRef vertex_offset = daeSafeCast<domInput_local_offset>(triangles->add(COLLADA_ELEMENT_INPUT));
vertex_offset->setSemantic("VERTEX"); vertex_offset->setSemantic("VERTEX");
vertex_offset->setOffset(0); vertex_offset->setOffset(0);
vertex_offset->setSource(domUrifragment(*positions_source, string("#") + vertices_id)); vertex_offset->setSource(domUrifragment(*positions_source, string("#") + vertices_id));
{ {
// <p>0 1 2 3 ... // <p>0 1 2 3 ...
domPRef indices = daeSafeCast<domP>(triangles->createAndPlace(COLLADA_ELEMENT_P)); domPRef indices = daeSafeCast<domP>(triangles->add(COLLADA_ELEMENT_P));
indices->getValue().setCount(num_indices); indices->getValue().setCount(num_indices);
for (unsigned int i = 0; i < num_indices; i++) for (unsigned int i = 0; i < num_indices; i++)
indices->getValue()[i] = stl_mesh->indices[i]; indices->getValue()[i] = stl_mesh->indices[i];
@ -382,7 +382,7 @@ void ColladaWriter::addJoints(daeElementRef parent) {
shared_ptr<urdf::Joint> urdf_joint = i->second; shared_ptr<urdf::Joint> urdf_joint = i->second;
// <joint name="base_laser_joint" sid="joint0"> // <joint name="base_laser_joint" sid="joint0">
domJointRef joint = daeSafeCast<domJoint>(parent->createAndPlace(COLLADA_ELEMENT_JOINT)); domJointRef joint = daeSafeCast<domJoint>(parent->add(COLLADA_ELEMENT_JOINT));
string joint_sid = string("joint") + boost::lexical_cast<string>(joint_num); string joint_sid = string("joint") + boost::lexical_cast<string>(joint_num);
joint_num++; joint_num++;
joint->setName(urdf_joint->name.c_str()); joint->setName(urdf_joint->name.c_str());
@ -398,20 +398,15 @@ void ColladaWriter::addJoints(daeElementRef parent) {
axis_z = 0.0; axis_z = 0.0;
} }
// @hack: OpenRAVE appears to flip joint axes
//axis_x *= -1.0;
//axis_y *= -1.0;
//axis_z *= -1.0;
switch (urdf_joint->type) switch (urdf_joint->type)
{ {
case urdf::Joint::REVOLUTE: { case urdf::Joint::REVOLUTE: {
// <revolute sid="axis0"> // <revolute sid="axis0">
domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_REVOLUTE));
revolute->setSid("axis0"); revolute->setSid("axis0");
{ {
// <axis> // <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); domAxisRef axis = daeSafeCast<domAxis>(revolute->add(COLLADA_ELEMENT_AXIS));
{ {
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = axis_x; axis->getValue()[0] = axis_x;
@ -421,10 +416,10 @@ void ColladaWriter::addJoints(daeElementRef parent) {
// </axis> // </axis>
// <limits> // <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->createAndPlace(COLLADA_TYPE_LIMITS)); domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->add(COLLADA_ELEMENT_LIMITS));
{ {
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower); daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower);
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper); daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper);
} }
// </limits> // </limits>
} }
@ -435,11 +430,11 @@ void ColladaWriter::addJoints(daeElementRef parent) {
// Model as a REVOLUTE joint without limits // Model as a REVOLUTE joint without limits
// <revolute sid="axis0"> // <revolute sid="axis0">
domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_REVOLUTE));
revolute->setSid("axis0"); revolute->setSid("axis0");
{ {
// <axis> // <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); domAxisRef axis = daeSafeCast<domAxis>(revolute->add(COLLADA_ELEMENT_AXIS));
{ {
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = axis_x; axis->getValue()[0] = axis_x;
@ -453,11 +448,11 @@ void ColladaWriter::addJoints(daeElementRef parent) {
} }
case urdf::Joint::PRISMATIC: { case urdf::Joint::PRISMATIC: {
// <prismatic sid="axis0"> // <prismatic sid="axis0">
domAxis_constraintRef prismatic = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC)); domAxis_constraintRef prismatic = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_PRISMATIC));
prismatic->setSid("axis0"); prismatic->setSid("axis0");
{ {
// <axis> // <axis>
domAxisRef axis = daeSafeCast<domAxis>(prismatic->createAndPlace(COLLADA_ELEMENT_AXIS)); domAxisRef axis = daeSafeCast<domAxis>(prismatic->add(COLLADA_ELEMENT_AXIS));
{ {
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = axis_x; axis->getValue()[0] = axis_x;
@ -467,10 +462,10 @@ void ColladaWriter::addJoints(daeElementRef parent) {
// </axis> // </axis>
// <limits> // <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(prismatic->createAndPlace(COLLADA_TYPE_LIMITS)); domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(prismatic->add(COLLADA_ELEMENT_LIMITS));
{ {
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower; daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower;
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper; daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper;
} }
// </limits> // </limits>
} }
@ -480,11 +475,11 @@ void ColladaWriter::addJoints(daeElementRef parent) {
case urdf::Joint::FIXED: { case urdf::Joint::FIXED: {
// Model as a REVOLUTE joint with no leeway // Model as a REVOLUTE joint with no leeway
domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_REVOLUTE));
revolute->setSid("axis0"); revolute->setSid("axis0");
{ {
// <axis> // <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); domAxisRef axis = daeSafeCast<domAxis>(revolute->add(COLLADA_ELEMENT_AXIS));
{ {
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = axis_x; axis->getValue()[0] = axis_x;
@ -494,10 +489,10 @@ void ColladaWriter::addJoints(daeElementRef parent) {
// </axis> // </axis>
// <limits> // <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->createAndPlace(COLLADA_TYPE_LIMITS)); domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->add(COLLADA_ELEMENT_LIMITS));
{ {
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = 0.0; daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = 0.0;
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = 0.0; daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = 0.0;
} }
// </limits> // </limits>
} }
@ -530,9 +525,9 @@ void ColladaWriter::addBindings(SCENE const& scene) {
// <bind_kinematics_scene> // <bind_kinematics_scene>
// <bind_kinematics_model node="node0"> // <bind_kinematics_model node="node0">
domBind_kinematics_modelRef kmodel_bind = daeSafeCast<domBind_kinematics_model>(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); domBind_kinematics_modelRef kmodel_bind = daeSafeCast<domBind_kinematics_model>(scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
kmodel_bind->setNode("v1.node0"); // @todo kmodel_bind->setNode("v1.node0"); // @todo
daeSafeCast<domCommon_param>(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str()); daeSafeCast<domCommon_param>(kmodel_bind->add(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str());
for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) { for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) {
shared_ptr<urdf::Joint> urdf_joint = i->second; shared_ptr<urdf::Joint> urdf_joint = i->second;
@ -544,20 +539,20 @@ void ColladaWriter::addBindings(SCENE const& scene) {
string joint_axis_value_sid = joint_axis_sid + string("_value"); string joint_axis_value_sid = joint_axis_sid + string("_value");
// <bind_joint_axis target="node0/joint_1_axis0"> // <bind_joint_axis target="node0/joint_1_axis0">
domBind_joint_axisRef joint_bind = daeSafeCast<domBind_joint_axis>(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_JOINT_AXIS)); domBind_joint_axisRef joint_bind = daeSafeCast<domBind_joint_axis>(scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
string node_name = node_ids_[urdf_joint->name]; string node_name = node_ids_[urdf_joint->name];
joint_bind->setTarget((node_name + string("/node_") + joint_sid + string("_") + axis_name).c_str()); joint_bind->setTarget((node_name + string("/node_") + joint_sid + string("_") + axis_name).c_str());
{ {
// <axis> // <axis>
domCommon_sidref_or_paramRef axis_bind = daeSafeCast<domCommon_sidref_or_param>(joint_bind->createAndPlace(COLLADA_ELEMENT_AXIS)); domCommon_sidref_or_paramRef axis_bind = daeSafeCast<domCommon_sidref_or_param>(joint_bind->add(COLLADA_ELEMENT_AXIS));
{ {
daeSafeCast<domCommon_param>(axis_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_sid.c_str()); daeSafeCast<domCommon_param>(axis_bind->add(COLLADA_ELEMENT_PARAM))->setValue(joint_axis_sid.c_str());
} }
// </axis> // </axis>
// <value> // <value>
domCommon_float_or_paramRef value_bind = daeSafeCast<domCommon_float_or_param>(joint_bind->createAndPlace(COLLADA_ELEMENT_VALUE)); domCommon_float_or_paramRef value_bind = daeSafeCast<domCommon_float_or_param>(joint_bind->add(COLLADA_ELEMENT_VALUE));
{ {
daeSafeCast<domCommon_param>(value_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_value_sid.c_str()); daeSafeCast<domCommon_param>(value_bind->add(COLLADA_ELEMENT_PARAM))->setValue(joint_axis_value_sid.c_str());
} }
} }
// </bind_joint_axis> // </bind_joint_axis>
@ -566,12 +561,12 @@ void ColladaWriter::addBindings(SCENE const& scene) {
void ColladaWriter::addKinematics(SCENE const& scene) { void ColladaWriter::addKinematics(SCENE const& scene) {
// <kinematics_model id="k1" name="pr2"> // <kinematics_model id="k1" name="pr2">
domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
kmodel->setId("k1"); kmodel->setId("k1");
kmodel->setName(robot_.getName().c_str()); kmodel->setName(robot_.getName().c_str());
{ {
// <technique_common> // <technique_common>
domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
addJoints(technique); addJoints(technique);
// </technique_common> // </technique_common>
@ -587,18 +582,18 @@ void ColladaWriter::addKinematics(SCENE const& scene) {
string inst_model_sid = string("inst_") + model_id; string inst_model_sid = string("inst_") + model_id;
// <instance_kinematics_model url="#k1" sid="inst_k1"> // <instance_kinematics_model url="#k1" sid="inst_k1">
domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL)); domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
ikm->setUrl((string("#") + model_id).c_str()); ikm->setUrl((string("#") + model_id).c_str());
ikm->setSid(inst_model_sid.c_str()); ikm->setSid(inst_model_sid.c_str());
{ {
// <newparam sid="kscene.inst_k1"> // <newparam sid="kscene.inst_k1">
domKinematics_newparamRef newparam_model = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef newparam_model = daeSafeCast<domKinematics_newparam>(ikm->add(COLLADA_ELEMENT_NEWPARAM));
string newparam_model_sid = string("kscene.inst_") + model_id; string newparam_model_sid = string("kscene.inst_") + model_id;
newparam_model->setSid(newparam_model_sid.c_str()); newparam_model->setSid(newparam_model_sid.c_str());
{ {
// <SIDREF>kscene/inst_k1</SIDREF> // <SIDREF>kscene/inst_k1</SIDREF>
string model_sidref = string("kscene/inst_") + model_id; string model_sidref = string("kscene/inst_") + model_id;
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam_model->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(model_sidref.c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(newparam_model->add(COLLADA_ELEMENT_SIDREF))->setValue(model_sidref.c_str());
} }
// </newparam> // </newparam>
@ -612,23 +607,23 @@ void ColladaWriter::addKinematics(SCENE const& scene) {
string axis_name = string("axis") + boost::lexical_cast<string>(idof); string axis_name = string("axis") + boost::lexical_cast<string>(idof);
// <newparam sid="kscene.inst_k1.joint0.axis0"> // <newparam sid="kscene.inst_k1.joint0.axis0">
domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->add(COLLADA_ELEMENT_NEWPARAM));
string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name; string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name;
newparam->setSid(newparam_sid.c_str()); newparam->setSid(newparam_sid.c_str());
{ {
// <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF> // <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF>
string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/") + axis_name; string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/") + axis_name;
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->add(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str());
} }
// </newparam> // </newparam>
// <newparam sid="kscene.inst_k1.joint0.axis0_value"> // <newparam sid="kscene.inst_k1.joint0.axis0_value">
domKinematics_newparamRef newparam_value = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef newparam_value = daeSafeCast<domKinematics_newparam>(ikm->add(COLLADA_ELEMENT_NEWPARAM));
string newparam_value_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name + string("_value"); string newparam_value_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name + string("_value");
newparam_value->setSid(newparam_value_sid.c_str()); newparam_value->setSid(newparam_value_sid.c_str());
{ {
// <float>0</float> // <float>0</float>
daeSafeCast<domKinematics_newparam::domFloat>(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f); daeSafeCast<domKinematics_newparam::domFloat>(newparam_value->add(COLLADA_ELEMENT_FLOAT))->setValue(0.0f);
} }
// </newparam> // </newparam>
} }
@ -638,19 +633,22 @@ void ColladaWriter::addKinematics(SCENE const& scene) {
void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, daeElementRef parent, int& link_num) { void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, daeElementRef parent, int& link_num) {
// <link sid="link0" name="base_link"> // <link sid="link0" name="base_link">
domLinkRef link = daeSafeCast<domLink>(parent->createAndPlace(COLLADA_ELEMENT_LINK)); domLinkRef link = daeSafeCast<domLink>(parent->add(COLLADA_ELEMENT_LINK));
string link_sid = string("link") + boost::lexical_cast<string>(link_num); string link_sid = string("link") + boost::lexical_cast<string>(link_num);
link->setName(urdf_link->name.c_str()); link->setName(urdf_link->name.c_str());
link->setSid(link_sid.c_str()); link->setSid(link_sid.c_str());
link_num++; link_num++;
foreach(shared_ptr<urdf::Joint> urdf_joint, urdf_link->child_joints) { foreach(shared_ptr<urdf::Joint> urdf_joint, urdf_link->child_joints) {
// <attachment_full joint="k1/joint0"> // <attachment_full joint="k1/joint0">
domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(link->createAndPlace(COLLADA_TYPE_ATTACHMENT_FULL)); domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(link->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name]; string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name];
attachment_full->setJoint(attachment_joint.c_str()); attachment_full->setJoint(attachment_joint.c_str());
{ {
addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation); // <rotate>x y z w</rotate>
addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position); addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation, NULL, true);
// <translate>x y z</translate>
addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position, NULL, true);
addKinematicLink(robot_.getLink(urdf_joint->child_link_name), attachment_full, link_num); addKinematicLink(robot_.getLink(urdf_joint->child_link_name), attachment_full, link_num);
} }
// </attachment_full> // </attachment_full>
@ -660,7 +658,7 @@ void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, dae
void ColladaWriter::addVisuals(SCENE const& scene) { void ColladaWriter::addVisuals(SCENE const& scene) {
// <node id="v1" name="pr2"> // <node id="v1" name="pr2">
domNodeRef root_node = daeSafeCast<domNode>(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE)); domNodeRef root_node = daeSafeCast<domNode>(scene.vscene->add(COLLADA_ELEMENT_NODE));
root_node->setId("v1"); root_node->setId("v1");
root_node->setName(robot_.getName().c_str()); root_node->setName(robot_.getName().c_str());
{ {
@ -686,12 +684,12 @@ void ColladaWriter::addMaterials() {
domEffectRef effect = addEffect(geometry_id, ambient, diffuse); domEffectRef effect = addEffect(geometry_id, ambient, diffuse);
// <material id="g1.link0.geom0.eff"> // <material id="g1.link0.geom0.eff">
domMaterialRef material = daeSafeCast<domMaterial>(materialsLib_->createAndPlace(COLLADA_ELEMENT_MATERIAL)); domMaterialRef material = daeSafeCast<domMaterial>(materialsLib_->add(COLLADA_ELEMENT_MATERIAL));
string material_id = geometry_id + string(".mat"); string material_id = geometry_id + string(".mat");
material->setId(material_id.c_str()); material->setId(material_id.c_str());
{ {
// <instance_effect url="#g1.link0.geom0.eff"/> // <instance_effect url="#g1.link0.geom0.eff"/>
domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(material->createAndPlace(COLLADA_ELEMENT_INSTANCE_EFFECT)); domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(material->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
string effect_id(effect->getId()); string effect_id(effect->getId());
instance_effect->setUrl((string("#") + effect_id).c_str()); instance_effect->setUrl((string("#") + effect_id).c_str());
} }
@ -702,24 +700,24 @@ void ColladaWriter::addMaterials() {
domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse) domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse)
{ {
// <effect id="g1.link0.geom0.eff"> // <effect id="g1.link0.geom0.eff">
domEffectRef effect = daeSafeCast<domEffect>(effectsLib_->createAndPlace(COLLADA_ELEMENT_EFFECT)); domEffectRef effect = daeSafeCast<domEffect>(effectsLib_->add(COLLADA_ELEMENT_EFFECT));
string effect_id = geometry_id + string(".eff"); string effect_id = geometry_id + string(".eff");
effect->setId(effect_id.c_str()); effect->setId(effect_id.c_str());
{ {
// <profile_COMMON> // <profile_COMMON>
domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->createAndPlace(COLLADA_ELEMENT_PROFILE_COMMON)); domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->add(COLLADA_ELEMENT_PROFILE_COMMON));
{ {
// <technique sid=""> // <technique sid="">
domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->createAndPlace(COLLADA_ELEMENT_TECHNIQUE)); domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->add(COLLADA_ELEMENT_TECHNIQUE));
{ {
// <phong> // <phong>
domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->createAndPlace(COLLADA_ELEMENT_PHONG)); domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->add(COLLADA_ELEMENT_PHONG));
{ {
// <ambient> // <ambient>
domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->createAndPlace(COLLADA_ELEMENT_AMBIENT)); domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_AMBIENT));
{ {
// <color>r g b a // <color>r g b a
domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->createAndPlace(COLLADA_ELEMENT_COLOR)); domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->add(COLLADA_ELEMENT_COLOR));
ambient_color->getValue().setCount(4); ambient_color->getValue().setCount(4);
ambient_color->getValue()[0] = color_ambient.r; ambient_color->getValue()[0] = color_ambient.r;
ambient_color->getValue()[1] = color_ambient.g; ambient_color->getValue()[1] = color_ambient.g;
@ -730,10 +728,10 @@ domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color con
// </ambient> // </ambient>
// <diffuse> // <diffuse>
domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->createAndPlace(COLLADA_ELEMENT_DIFFUSE)); domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_DIFFUSE));
{ {
// <color>r g b a // <color>r g b a
domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->createAndPlace(COLLADA_ELEMENT_COLOR)); domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->add(COLLADA_ELEMENT_COLOR));
diffuse_color->getValue().setCount(4); diffuse_color->getValue().setCount(4);
diffuse_color->getValue()[0] = color_diffuse.r; diffuse_color->getValue()[0] = color_diffuse.r;
diffuse_color->getValue()[1] = color_diffuse.g; diffuse_color->getValue()[1] = color_diffuse.g;
@ -756,7 +754,7 @@ domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color con
void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num) { void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num) {
// <node id="v1.node0" name="base_link" sid="node0"> // <node id="v1.node0" name="base_link" sid="node0">
domNodeRef node = daeSafeCast<domNode>(parent->createAndPlace(COLLADA_ELEMENT_NODE)); domNodeRef node = daeSafeCast<domNode>(parent->add(COLLADA_ELEMENT_NODE));
string node_sid = string("node") + boost::lexical_cast<string>(link_num); string node_sid = string("node") + boost::lexical_cast<string>(link_num);
string node_id = string("v1.") + node_sid; string node_id = string("v1.") + node_sid;
node->setName(urdf_link->name.c_str()); node->setName(urdf_link->name.c_str());
@ -769,13 +767,13 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
{ {
if (urdf_link->parent_joint != NULL) { if (urdf_link->parent_joint != NULL) {
// <rotate>x y z w</rotate> // <rotate>x y z w</rotate>
addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true);
// <translate>x y z</translate> // <translate>x y z</translate>
addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position); addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position, NULL, true);
if (urdf_link->visual != NULL) { if (urdf_link->visual != NULL) {
// <node id="v1.node0.visual" name="visual" sid="visual"> // <node id="v1.node0.visual" name="visual" sid="visual">
domNodeRef visual_node = daeSafeCast<domNode>(node->createAndPlace(COLLADA_ELEMENT_NODE)); domNodeRef visual_node = daeSafeCast<domNode>(node->add(COLLADA_ELEMENT_NODE));
string visual_sid("visual"); string visual_sid("visual");
string visual_id = node_id + "." + visual_sid; string visual_id = node_id + "." + visual_sid;
visual_node->setName("visual"); visual_node->setName("visual");
@ -785,9 +783,9 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
parent_node = visual_node; parent_node = visual_node;
// <rotate>x y z w</rotate> // <rotate>x y z w</rotate>
addRotate(parent_node, urdf_link->visual->origin.rotation); addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true);
// <translate>x y z</translate> // <translate>x y z</translate>
addTranslate(parent_node, urdf_link->visual->origin.position); addTranslate(parent_node, urdf_link->visual->origin.position, NULL, true);
} }
// <rotate sid="node_joint0_axis0">x y z angle</rotate> // <rotate sid="node_joint0_axis0">x y z angle</rotate>
@ -802,19 +800,19 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
// <instance_geometry url="#g1.link0.geom"> // <instance_geometry url="#g1.link0.geom">
map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name); map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name);
if (i != geometry_ids_.end()) { if (i != geometry_ids_.end()) {
domInstance_geometryRef instance_geometry = daeSafeCast<domInstance_geometry>(parent_node->createAndPlace(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); domInstance_geometryRef instance_geometry = daeSafeCast<domInstance_geometry>(parent_node->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
string geometry_id = i->second; string geometry_id = i->second;
string instance_geometry_url = string("#") + geometry_id; string instance_geometry_url = string("#") + geometry_id;
instance_geometry->setUrl(instance_geometry_url.c_str()); instance_geometry->setUrl(instance_geometry_url.c_str());
{ {
// <bind_material> // <bind_material>
domBind_materialRef bind_material = daeSafeCast<domBind_material>(instance_geometry->createAndPlace(COLLADA_ELEMENT_BIND_MATERIAL)); domBind_materialRef bind_material = daeSafeCast<domBind_material>(instance_geometry->add(COLLADA_ELEMENT_BIND_MATERIAL));
{ {
// <technique_common> // <technique_common>
domBind_material::domTechnique_commonRef technique_common = daeSafeCast<domBind_material::domTechnique_common>(bind_material->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domBind_material::domTechnique_commonRef technique_common = daeSafeCast<domBind_material::domTechnique_common>(bind_material->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{ {
// <instance_material> // <instance_material>
domInstance_materialRef instance_material = daeSafeCast<domInstance_material>(technique_common->createAndPlace(COLLADA_ELEMENT_INSTANCE_MATERIAL)); domInstance_materialRef instance_material = daeSafeCast<domInstance_material>(technique_common->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
instance_material->setTarget((instance_geometry_url + string(".mat")).c_str()); instance_material->setTarget((instance_geometry_url + string(".mat")).c_str());
instance_material->setSymbol("mat0"); instance_material->setSymbol("mat0");
// </instance_material> // </instance_material>
@ -834,9 +832,18 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
// </node> // </node>
} }
domTranslateRef ColladaWriter::addTranslate(daeElementRef parent, urdf::Vector3 const& position) { domTranslateRef ColladaWriter::addTranslate(daeElementRef parent, urdf::Vector3 const& position, daeElementRef before, bool ignore_zero_translations) {
if (ignore_zero_translations && position.x == 0.0 && position.y == 0.0 && position.z == 0.0)
return NULL;
// <translate>x y z</translate> // <translate>x y z</translate>
domTranslateRef trans = daeSafeCast<domTranslate>(parent->createAndPlace(COLLADA_ELEMENT_TRANSLATE)); domTranslateRef trans;
if (before) {
trans = daeSafeCast<domTranslate>(parent->createElement(COLLADA_ELEMENT_TRANSLATE));
parent->addBefore(trans, before);
}
else
trans = daeSafeCast<domTranslate>(parent->add(COLLADA_ELEMENT_TRANSLATE));
trans->getValue().setCount(3); trans->getValue().setCount(3);
trans->getValue()[0] = position.x; trans->getValue()[0] = position.x;
trans->getValue()[1] = position.y; trans->getValue()[1] = position.y;
@ -844,7 +851,7 @@ domTranslateRef ColladaWriter::addTranslate(daeElementRef parent, urdf::Vector3
return trans; return trans;
} }
domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const& r) { domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const& r, daeElementRef before, bool ignore_zero_rotations) {
double ax, ay, az, aa; double ax, ay, az, aa;
// Convert from quaternion to axis-angle // Convert from quaternion to axis-angle
@ -858,7 +865,10 @@ domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const
az = r.z * inv_len; az = r.z * inv_len;
} }
else { else {
// Angle is 0 (mod 2*pi), so any axis will do if (ignore_zero_rotations)
return NULL;
// Angle is 0 (mod 2*pi), so no need to add rotate node
aa = 0.0; aa = 0.0;
ax = 1.0; ax = 1.0;
ay = 0.0; ay = 0.0;
@ -866,7 +876,13 @@ domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const
} }
// <rotate>x y z w</rotate> // <rotate>x y z w</rotate>
domRotateRef rot = daeSafeCast<domRotate>(parent->createAndPlace(COLLADA_ELEMENT_ROTATE)); domRotateRef rot;
if (before) {
rot = daeSafeCast<domRotate>(parent->createElement(COLLADA_ELEMENT_ROTATE));
parent->addBefore(rot, before);
}
else
rot = daeSafeCast<domRotate>(parent->add(COLLADA_ELEMENT_ROTATE));
rot->getValue().setCount(4); rot->getValue().setCount(4);
rot->getValue()[0] = ax; rot->getValue()[0] = ax;
rot->getValue()[1] = ay; rot->getValue()[1] = ay;