From 01196304b675a12a373e073b761cc2cd574740de Mon Sep 17 00:00:00 2001
From: tfield <devnull@localhost>
Date: Mon, 17 May 2010 04:52:20 +0000
Subject: [PATCH] collada_urdf: trimming collada by not writing null
 transforms; shifted away from deprecated createAndPlace API; added option to
 add transform before an existing element

---
 .../include/collada_urdf/collada_writer.h     |   4 +-
 collada_urdf/src/collada_writer.cpp           | 232 ++++++++++--------
 2 files changed, 126 insertions(+), 110 deletions(-)

diff --git a/collada_urdf/include/collada_urdf/collada_writer.h b/collada_urdf/include/collada_urdf/collada_writer.h
index 6aada98..0c137df 100644
--- a/collada_urdf/include/collada_urdf/collada_writer.h
+++ b/collada_urdf/include/collada_urdf/collada_writer.h
@@ -110,8 +110,8 @@ private:
 
     void addBindings(SCENE const& scene);
 
-    domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position);
-    domRotateRef    addRotate(daeElementRef parent, urdf::Rotation const& r);
+    domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position, daeElementRef before = NULL, bool ignore_zero_translations = false);
+    domRotateRef    addRotate(daeElementRef parent, urdf::Rotation const& r, daeElementRef before = NULL, bool ignore_zero_rotations = false);
 
     std::string getTimeStampString() const;
 
diff --git a/collada_urdf/src/collada_writer.cpp b/collada_urdf/src/collada_writer.cpp
index 72c2013..9b8aa1f 100644
--- a/collada_urdf/src/collada_writer.cpp
+++ b/collada_urdf/src/collada_writer.cpp
@@ -106,45 +106,45 @@ void ColladaWriter::initDocument(string const& documentName) {
     dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML");
 
     // Create asset elements
-    domAssetRef asset = daeSafeCast<domAsset>(dom_->createAndPlace(COLLADA_ELEMENT_ASSET));
+    domAssetRef asset = daeSafeCast<domAsset>(dom_->add(COLLADA_ELEMENT_ASSET));
     {
         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());
-        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());
 
-        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");
 
-        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->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);
     }
 
     // Create top-level elements
-    scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE));
-    visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
+    scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->add(COLLADA_ELEMENT_SCENE));
+    visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
     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");
-    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");
-    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");
-    jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS));
+    jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->add(COLLADA_ELEMENT_LIBRARY_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");
-    effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS));
+    effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->add(COLLADA_ELEMENT_LIBRARY_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");
 }
 
@@ -152,30 +152,30 @@ ColladaWriter::SCENE ColladaWriter::createScene() {
     SCENE s;
 
     // 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->setName("URDF 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());
 
     // 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->setName("URDF 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());
 
     // 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->setName("URDF 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());
 
     return s;
@@ -183,10 +183,10 @@ ColladaWriter::SCENE ColladaWriter::createScene() {
 
 void ColladaWriter::setupPhysics(SCENE const& scene) {
     // <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
-        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);
         // </gravity>
     }
@@ -210,7 +210,7 @@ void ColladaWriter::addGeometries() {
                 urdf::Vector3 scale    = urdf_mesh->scale;      // @todo use scale
 
                 // <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");
                 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) {
     // <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_indices  = stl_mesh->indices.size();
         unsigned int num_faces    = num_indices / 3;
 
         // <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());
         {
             // <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->setCount(num_vertices * 3);
             positions_array->setDigits(6); // 6 decimal places
@@ -320,10 +320,10 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElement
             // </float_array>
 
             // <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">
-                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->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array")));
                 accessor->setStride(3);
@@ -331,9 +331,9 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElement
                     // <param name="X" type="float"/>
                     // <param name="Y" type="float"/>
                     // <param name="Z" type="float"/>
-                    domParamRef px = daeSafeCast<domParam>(accessor->createAndPlace(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 pz = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
+                    domParamRef px = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
+                    domParamRef py = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
+                    domParamRef pz = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
                 }
                 // </accessor>
             }
@@ -341,30 +341,30 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElement
         }
 
         // <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");
         vertices->setId(vertices_id.c_str());
         {
             // <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->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId())));
         }
         // </vertices>
 
         // <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->setMaterial("mat0");
         {
             // <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->setOffset(0);
             vertex_offset->setSource(domUrifragment(*positions_source, string("#") + vertices_id));
             {
                 // <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);
                 for (unsigned int i = 0; i < num_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;
 
         // <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);
         joint_num++;
         joint->setName(urdf_joint->name.c_str());
@@ -398,20 +398,15 @@ void ColladaWriter::addJoints(daeElementRef parent) {
             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)
         {
             case urdf::Joint::REVOLUTE: {
                 // <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");
                 {
                     // <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()[0] = axis_x;
@@ -421,10 +416,10 @@ void ColladaWriter::addJoints(daeElementRef parent) {
                     // </axis>
 
                     // <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->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper);
+                        daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower);
+                        daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper);
                     }
                     // </limits>
                 }
@@ -435,11 +430,11 @@ void ColladaWriter::addJoints(daeElementRef parent) {
                 // Model as a REVOLUTE joint without limits
 
                 // <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");
                 {
                     // <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()[0] = axis_x;
@@ -453,11 +448,11 @@ void ColladaWriter::addJoints(daeElementRef parent) {
             }
             case urdf::Joint::PRISMATIC: {
                 // <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");
                 {
                     // <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()[0] = axis_x;
@@ -467,10 +462,10 @@ void ColladaWriter::addJoints(daeElementRef parent) {
                     // </axis>
 
                     // <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->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper;
+                        daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower;
+                        daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper;
                     }
                     // </limits>
                 }
@@ -480,11 +475,11 @@ void ColladaWriter::addJoints(daeElementRef parent) {
             case urdf::Joint::FIXED: {
                 // 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");
                 {
                     // <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()[0] = axis_x;
@@ -494,10 +489,10 @@ void ColladaWriter::addJoints(daeElementRef parent) {
                     // </axis>
 
                     // <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->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = 0.0;
+                        daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = 0.0;
+                        daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = 0.0;
                     }
                     // </limits>
                 }
@@ -530,9 +525,9 @@ void ColladaWriter::addBindings(SCENE const& scene) {
 
     // <bind_kinematics_scene>
     //   <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
-    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++) {
         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");
 
         // <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];
         joint_bind->setTarget((node_name + string("/node_") + joint_sid + string("_") + axis_name).c_str());
         {
             // <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>
             // <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>
@@ -566,12 +561,12 @@ void ColladaWriter::addBindings(SCENE const& scene) {
 
 void ColladaWriter::addKinematics(SCENE const& scene) {
     // <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->setName(robot_.getName().c_str());
     {
         // <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);
         // </technique_common>
 
@@ -587,18 +582,18 @@ void ColladaWriter::addKinematics(SCENE const& scene) {
     string inst_model_sid = string("inst_") + model_id;
 
     // <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->setSid(inst_model_sid.c_str());
     {
         // <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;
         newparam_model->setSid(newparam_model_sid.c_str());
         {
             // <SIDREF>kscene/inst_k1</SIDREF>
             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>
 
@@ -612,23 +607,23 @@ void ColladaWriter::addKinematics(SCENE const& scene) {
             string axis_name = string("axis") + boost::lexical_cast<string>(idof);
 
             // <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;
             newparam->setSid(newparam_sid.c_str());
             {
                 // <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF>
                 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 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");
             newparam_value->setSid(newparam_value_sid.c_str());
             {
                 // <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>
         }
@@ -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) {
     // <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);
     link->setName(urdf_link->name.c_str());
     link->setSid(link_sid.c_str());
     link_num++;
     foreach(shared_ptr<urdf::Joint> urdf_joint, urdf_link->child_joints) {
-        // <attachment_full joint="k1/joint0">
-        domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(link->createAndPlace(COLLADA_TYPE_ATTACHMENT_FULL));
+    	// <attachment_full joint="k1/joint0">
+        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];
         attachment_full->setJoint(attachment_joint.c_str());
         {
-            addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation);
-            addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position);
+            // <rotate>x y z w</rotate>
+            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);
         }
         // </attachment_full>
@@ -660,7 +658,7 @@ void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, dae
 
 void ColladaWriter::addVisuals(SCENE const& scene) {
     // <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->setName(robot_.getName().c_str());
     {
@@ -686,12 +684,12 @@ void ColladaWriter::addMaterials() {
         domEffectRef effect = addEffect(geometry_id, ambient, diffuse);
 
         // <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");
         material->setId(material_id.c_str());
         {
             // <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());
             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)
 {
     // <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");
     effect->setId(effect_id.c_str());
     {
         // <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="">
-            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>
-                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>
-                    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
-                        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()[0] = color_ambient.r;
                         ambient_color->getValue()[1] = color_ambient.g;
@@ -730,10 +728,10 @@ domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color con
                     // </ambient>
 
                     // <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
-                        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()[0] = color_diffuse.r;
                         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) {
     // <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_id = string("v1.") + node_sid;
     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) {
             // <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>
-            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) {
                 // <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_id = node_id + "." + visual_sid;
                 visual_node->setName("visual");
@@ -785,9 +783,9 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
                 parent_node = visual_node;
 
                 // <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>
-                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>
@@ -802,19 +800,19 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
         // <instance_geometry url="#g1.link0.geom">
         map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name);
         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 instance_geometry_url = string("#") + geometry_id;
             instance_geometry->setUrl(instance_geometry_url.c_str());
             {
                 // <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>
-                    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>
-                        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->setSymbol("mat0");
                         // </instance_material>
@@ -834,9 +832,18 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
     // </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>
-    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()[0] = position.x;
     trans->getValue()[1] = position.y;
@@ -844,7 +851,7 @@ domTranslateRef ColladaWriter::addTranslate(daeElementRef parent, urdf::Vector3
     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;
 
     // Convert from quaternion to axis-angle
@@ -858,7 +865,10 @@ domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const
         az = r.z * inv_len;
     }
     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;
         ax = 1.0;
         ay = 0.0;
@@ -866,7 +876,13 @@ domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const
     }
 
     // <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()[0] = ax;
     rot->getValue()[1] = ay;