diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index 0763be9..7ed58f2 100644 --- a/collada_urdf/CMakeLists.txt +++ b/collada_urdf/CMakeLists.txt @@ -3,4 +3,4 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) set(ROS_BUILD_TYPE Debug) rosbuild_init() set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp src/STLLoader.cpp) +rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp src/ColladaWriter.cpp src/STLLoader.cpp) diff --git a/collada_urdf/include/collada_urdf/ColladaWriter.h b/collada_urdf/include/collada_urdf/ColladaWriter.h new file mode 100644 index 0000000..728438c --- /dev/null +++ b/collada_urdf/include/collada_urdf/ColladaWriter.h @@ -0,0 +1,128 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redstributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef COLLADA_URDF_COLLADA_WRITER_H +#define COLLADA_URDF_COLLADA_WRITER_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collada_urdf { + +class Mesh; + +class ColladaWriter : public daeErrorHandler +{ +private: + struct SCENE + { + domVisual_sceneRef vscene; + domKinematics_sceneRef kscene; + domPhysics_sceneRef pscene; + domInstance_with_extraRef viscene; + domInstance_kinematics_sceneRef kiscene; + domInstance_with_extraRef piscene; + }; + +public: + ColladaWriter(std::string const& documentName, urdf::Model* robot); + virtual ~ColladaWriter(); + + /** + * todo + * @return + */ + bool writeScene(); + +protected: + virtual void handleError(daeString msg); + virtual void handleWarning(daeString msg); + +private: + SCENE createScene(); + void setupPhysics(SCENE scene); + void addGeometries(); + void loadMesh(std::string const& filename, domGeometryRef geometry, std::string const& geometry_id); + bool loadMeshWithSTLLoader(resource_retriever::MemoryResource const& resource, domGeometryRef geometry, std::string const& geometry_id); + void buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, std::string const& geometry_id); + void addJoints(daeElementRef parent); + void addBindings(SCENE scene); + void addKinematics(SCENE scene); + void addKinematicLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); + void addVisuals(SCENE scene); + void addMaterials(); + domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse); + void addVisualLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); + domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position); + domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r); + +private: + urdf::Model* robot_; + + boost::shared_ptr collada_; + domCOLLADA* dom_; + domCOLLADA::domSceneRef scene_; + + domLibrary_geometriesRef geometriesLib_; + domLibrary_visual_scenesRef visualScenesLib_; + domLibrary_kinematics_scenesRef kinematicsScenesLib_; + domLibrary_kinematics_modelsRef kinematicsModelsLib_; + domLibrary_jointsRef jointsLib_; + domLibrary_physics_scenesRef physicsScenesLib_; + domLibrary_materialsRef materialsLib_; + domLibrary_effectsRef effectsLib_; + + domKinematics_modelRef kmodel_; + + std::map geometry_ids_; //!< link.name -> geometry.id + std::map joint_sids_; //!< joint.name -> joint.sid + std::map node_ids_; //!< joint.name -> node.id +}; + +} + +#endif diff --git a/collada_urdf/src/STLLoader.h b/collada_urdf/include/collada_urdf/STLLoader.h similarity index 65% rename from collada_urdf/src/STLLoader.h rename to collada_urdf/include/collada_urdf/STLLoader.h index 8c7d768..6bec856 100644 --- a/collada_urdf/src/STLLoader.h +++ b/collada_urdf/include/collada_urdf/STLLoader.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2008, Willow Garage, Inc. +* Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,14 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tim Field */ - -// STLLoader.h - -#ifndef COLLADA_URDF_STLLOADER_HH -#define COLLADA_URDF_STLLOADER_HH +#ifndef COLLADA_URDF_STL_LOADER_H +#define COLLADA_URDF_STL_LOADER_H #include + #include #include @@ -48,48 +45,49 @@ #define ORDER_MAX 10 #define FACE_MAX 200000 -namespace collada_urdf +namespace collada_urdf { + +class Vector3 { - class Vector3 - { - public: - Vector3(float x, float y, float z); +public: + Vector3(float x, float y, float z); - bool operator==(const Vector3& v) const; + bool operator==(Vector3 const& v) const; - float x; - float y; - float z; - }; + float x; + float y; + float z; +}; - class Mesh - { - public: - Mesh(); +class Mesh +{ +public: + Mesh(); - int getVertexIndex(const Vector3& v) const; + int getVertexIndex(Vector3 const& v) const; - void addVertex(const Vector3& v); - void addNormal(const Vector3& n); - void addIndex(unsigned int i); + void addVertex(Vector3 const& v); + void addNormal(Vector3 const& n); + void addIndex(unsigned int i); - public: - std::vector vertices; - std::vector normals; - std::vector indices; - }; +public: + std::vector vertices; + std::vector normals; + std::vector indices; +}; - class STLLoader - { - public: - Mesh* load(const std::string& filename); +class STLLoader +{ +public: + Mesh* load(std::string const& filename); + +private: + void readBinary (FILE* filein, Mesh* mesh); + uint32_t readLongInt (FILE* filein); + uint16_t readShortInt(FILE* filein); + float readFloat (FILE* filein); +}; - private: - void readBinary(FILE* filein, Mesh* mesh); - uint32_t readLongInt(FILE* filein); - uint16_t readShortInt(FILE* filein); - float readFloat(FILE* filein); - }; } #endif diff --git a/collada_urdf/manifest.xml b/collada_urdf/manifest.xml index ff3a891..d6bedf7 100644 --- a/collada_urdf/manifest.xml +++ b/collada_urdf/manifest.xml @@ -11,7 +11,6 @@ - diff --git a/collada_urdf/src/ColladaWriter.cpp b/collada_urdf/src/ColladaWriter.cpp new file mode 100644 index 0000000..b6e626a --- /dev/null +++ b/collada_urdf/src/ColladaWriter.cpp @@ -0,0 +1,833 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redstributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include "collada_urdf/ColladaWriter.h" + +#include "collada_urdf/STLLoader.h" + +#include + +#define foreach BOOST_FOREACH + +using std::string; +using std::map; +using std::vector; +using boost::shared_ptr; + +namespace collada_urdf { + +ColladaWriter::ColladaWriter(string const& documentName, urdf::Model* robot) : robot_(robot) { + daeErrorHandler::setErrorHandler(this); + + collada_.reset(new DAE()); + collada_->setIOPlugin(NULL); + collada_->setDatabase(NULL); + + daeDocument* doc = NULL; + daeInt error = collada_->getDatabase()->insertDocument(documentName.c_str(), &doc); // also creates a collada root + if (error != DAE_OK || doc == NULL) + { + std::cerr << "Failed to create new document" << std::endl; + throw; + } + + dom_ = daeSafeCast(doc->getDomRoot()); + dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); + + // Create the required asset tag + domAssetRef asset = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_ASSET)); + { + domAsset::domCreatedRef created = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_CREATED)); + created->setValue("2009-04-06T17:01:00.891550"); // @todo: replace with current date + domAsset::domModifiedRef modified = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_MODIFIED)); + modified->setValue("2009-04-06T17:01:00.891550"); // @todo: replace with current date + + domAsset::domContributorRef contrib = daeSafeCast(asset->createAndPlace(COLLADA_TYPE_CONTRIBUTOR)); + domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast(contrib->createAndPlace(COLLADA_ELEMENT_AUTHORING_TOOL)); + authoringtool->setValue("URDF Collada Writer"); + + domAsset::domUnitRef units = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_UNIT)); + units->setMeter(1); + units->setName("meter"); + + domAsset::domUp_axisRef zup = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_UP_AXIS)); + zup->setValue(UP_AXIS_Z_UP); + } + + scene_ = dom_->getScene(); + if (!scene_) + scene_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); + + visualScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); + visualScenesLib_->setId("vscenes"); + geometriesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); + geometriesLib_->setId("geometries"); + kinematicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); + kinematicsScenesLib_->setId("kscenes"); + kinematicsModelsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); + kinematicsModelsLib_->setId("kmodels"); + jointsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); + jointsLib_->setId("joints"); + + physicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); + physicsScenesLib_->setId("physics_scenes"); + effectsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS)); + effectsLib_->setId("effects"); + materialsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS)); + materialsLib_->setId("materials"); +} + +ColladaWriter::~ColladaWriter() { + collada_.reset(); + DAE::cleanup(); +} + +bool ColladaWriter::writeScene() { + SCENE scene = createScene(); + + setupPhysics(scene); + addGeometries(); + addKinematics(scene); + addVisuals(scene); + addMaterials(); + addBindings(scene); + + collada_->writeAll(); + + return true; +} + +// Implementation + +ColladaWriter::SCENE ColladaWriter::createScene() { + SCENE s; + + // Create visual scene + s.vscene = daeSafeCast(visualScenesLib_->createAndPlace(COLLADA_ELEMENT_VISUAL_SCENE)); + s.vscene->setId("vscene"); + s.vscene->setName("URDF Visual Scene"); + + // Create instance visual scene + s.viscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE)); + s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str()); + + // Create kinematics scene + s.kscene = daeSafeCast(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE)); + s.kscene->setId("kscene"); + s.kscene->setName("URDF Kinematics Scene"); + + // Create instance kinematics scene + s.kiscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE)); + s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str()); + + // Create physics scene + s.pscene = daeSafeCast(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE)); + s.pscene->setId("pscene"); + s.pscene->setName("URDF Physics Scene"); + + // Create instance physics scene + s.piscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE)); + s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str()); + + return s; +} + +void ColladaWriter::handleError(daeString msg) { + std::cerr << "COLLADA error: " << msg << std::endl; +} + +void ColladaWriter::handleWarning(daeString msg) { + std::cerr << "COLLADA warning: " << msg << std::endl; +} + +void ColladaWriter::setupPhysics(SCENE scene) { + // + domPhysics_scene::domTechnique_commonRef common = daeSafeCast(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + { + // 0 0 0 + domTargetable_float3Ref g = daeSafeCast(common->createAndPlace(COLLADA_ELEMENT_GRAVITY)); + g->getValue().set3(0.0, 0.0, 0.0); + // + } + // +} + +void ColladaWriter::addGeometries() { + int link_num = 0; + + for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { + shared_ptr urdf_link = i->second; + + if (urdf_link->visual == NULL || urdf_link->visual->geometry == NULL) + continue; + + switch (urdf_link->visual->geometry->type) { + case urdf::Geometry::MESH: { + urdf::Mesh* urdf_mesh = (urdf::Mesh*) urdf_link->visual->geometry.get(); + + string filename = urdf_mesh->filename; + urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale + + // + domGeometryRef geometry = daeSafeCast(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY)); + string geometry_id = string("g1.link") + boost::lexical_cast(link_num) + string(".geom0"); + geometry->setId(geometry_id.c_str()); + { + loadMesh(filename, geometry, geometry_id); + } + geometry_ids_[urdf_link->name] = geometry_id; + // + + link_num++; + break; + } + case urdf::Geometry::SPHERE: { + std::cerr << "Warning: geometry type SPHERE of link " << urdf_link->name << " is unsupported" << std::endl; + break; + } + case urdf::Geometry::BOX: { + std::cerr << "Warning: geometry type BOX of link " << urdf_link->name << " is unsupported" << std::endl; + break; + } + case urdf::Geometry::CYLINDER: { + std::cerr << "Warning: geometry type CYLINDER of link " << urdf_link->name << " is unsupported" << std::endl; + break; + } + default: { + std::cerr << "Warning: geometry type " << urdf_link->visual->geometry->type << " of link " << urdf_link->name << " is supported" << std::endl; + break; + } + } + } +} + +void ColladaWriter::loadMesh(string const& filename, domGeometryRef geometry, string const& geometry_id) { + // Load the mesh + resource_retriever::MemoryResource resource; + resource_retriever::Retriever retriever; + try { + resource = retriever.get(filename.c_str()); + } + catch (resource_retriever::Exception& e) { + std::cerr << "Unable to load mesh file " << filename << ": " << e.what() << std::endl; + return; + } + + // Try assimp first, then STLLoader + if (!loadMeshWithSTLLoader(resource, geometry, geometry_id)) + std::cerr << "Can't load mesh " << filename << std::endl; +} + +bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource const& resource, domGeometryRef geometry, string const& geometry_id) { + // Write the resource to a temporary file + char tmp_filename[] = "/tmp/collada_urdf_XXXXXX"; + int fd = mkstemp(tmp_filename); + write(fd, resource.data.get(), resource.size); + close(fd); + + // Import the mesh using STLLoader + STLLoader loader; + Mesh* stl_mesh = loader.load(string(tmp_filename)); + buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id); + delete stl_mesh; + + // Delete the temporary file + unlink(tmp_filename); + + return true; +} + +void ColladaWriter::buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, string const& geometry_id) { + // + domMeshRef mesh = daeSafeCast(parent->createAndPlace(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; + + // + domSourceRef positions_source = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); + positions_source->setId((geometry_id + string(".positions")).c_str()); + { + // + domFloat_arrayRef positions_array = daeSafeCast(positions_source->createAndPlace(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 + positions_array->getValue().setCount(num_vertices * 3); + for (unsigned int j = 0; j < num_vertices; j++) { + positions_array->getValue()[j * 3 ] = stl_mesh->vertices[j].x; + positions_array->getValue()[j * 3 + 1] = stl_mesh->vertices[j].y; + positions_array->getValue()[j * 3 + 2] = stl_mesh->vertices[j].z; + } + // + + // + domSource::domTechnique_commonRef source_tech = daeSafeCast(positions_source->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + { + // + domAccessorRef accessor = daeSafeCast(source_tech->createAndPlace(COLLADA_ELEMENT_ACCESSOR)); + accessor->setCount(num_vertices / 3); + accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array"))); + accessor->setStride(3); + { + // + // + // + domParamRef px = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float"); + domParamRef py = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float"); + domParamRef pz = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float"); + } + // + } + // + } + + // + domVerticesRef vertices = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_VERTICES)); + string vertices_id = geometry_id + string(".vertices"); + vertices->setId(vertices_id.c_str()); + { + // + domInput_localRef vertices_input = daeSafeCast(vertices->createAndPlace(COLLADA_ELEMENT_INPUT)); + vertices_input->setSemantic("POSITION"); + vertices_input->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId()))); + } + // + + // + domTrianglesRef triangles = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); + triangles->setCount(num_faces); + triangles->setMaterial("mat0"); + { + // + domInput_local_offsetRef vertex_offset = daeSafeCast(triangles->createAndPlace(COLLADA_ELEMENT_INPUT)); + vertex_offset->setSemantic("VERTEX"); + vertex_offset->setOffset(0); + vertex_offset->setSource(domUrifragment(*positions_source, string("#") + vertices_id)); + { + //

0 1 2 3 ... + domPRef indices = daeSafeCast(triangles->createAndPlace(COLLADA_ELEMENT_P)); + indices->getValue().setCount(num_indices); + for (unsigned int i = 0; i < num_indices; i++) + indices->getValue()[i] = stl_mesh->indices[i]; + //

+ } + } + // + } + // +} + +void ColladaWriter::addJoints(daeElementRef parent) { + int joint_num = 0; + for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + shared_ptr urdf_joint = i->second; + + // + domJointRef joint = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_JOINT)); + string joint_sid = string("joint") + boost::lexical_cast(joint_num); + joint_num++; + joint->setName(urdf_joint->name.c_str()); + joint->setSid(joint_sid.c_str()); + joint_sids_[urdf_joint->name] = joint_sid; + + double axis_x = urdf_joint->axis.x; + double axis_y = urdf_joint->axis.y; + double axis_z = urdf_joint->axis.z; + if (axis_x == 0.0 && axis_y == 0.0 && axis_z == 0.0) { + axis_x = 1.0; + axis_y = 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) + { + case urdf::Joint::REVOLUTE: { + // + domAxis_constraintRef revolute = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); + revolute->setSid("axis0"); + { + // + domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); + { + axis->getValue().setCount(3); + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; + } + // + + // + domJoint_limitsRef limits = daeSafeCast(revolute->createAndPlace(COLLADA_TYPE_LIMITS)); + { + daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower); + daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper); + } + // + } + // + break; + } + case urdf::Joint::CONTINUOUS: { + // Model as a REVOLUTE joint without limits + + // + domAxis_constraintRef revolute = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); + revolute->setSid("axis0"); + { + // + domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); + { + axis->getValue().setCount(3); + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; + } + // + } + // + break; + } + case urdf::Joint::PRISMATIC: { + // + domAxis_constraintRef prismatic = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC)); + prismatic->setSid("axis0"); + { + // + domAxisRef axis = daeSafeCast(prismatic->createAndPlace(COLLADA_ELEMENT_AXIS)); + { + axis->getValue().setCount(3); + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; + } + // + + // + domJoint_limitsRef limits = daeSafeCast(prismatic->createAndPlace(COLLADA_TYPE_LIMITS)); + { + daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower; + daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper; + } + // + } + // + break; + } + case urdf::Joint::FIXED: { + // Model as a REVOLUTE joint with no leeway + + domAxis_constraintRef revolute = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); + revolute->setSid("axis0"); + { + // + domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); + { + axis->getValue().setCount(3); + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; + } + // + + // + domJoint_limitsRef limits = daeSafeCast(revolute->createAndPlace(COLLADA_TYPE_LIMITS)); + { + daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = 0.0; + daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = 0.0; + } + // + } + // + break; + } + case urdf::Joint::UNKNOWN: { + std::cerr << "Joint type UNKNOWN of joint " << urdf_joint->name << " is unsupported" << std::endl; + break; + } + case urdf::Joint::FLOATING: { + std::cerr << "Joint type FLOATING of joint " << urdf_joint->name << " is unsupported" << std::endl; + break; + } + case urdf::Joint::PLANAR: { + std::cerr << "Joint type PLANAR of joint " << urdf_joint->name << " is unsupported" << std::endl; + break; + } + default: { + std::cerr << "Joint type " << urdf_joint->type << " of joint " << urdf_joint->name << " is unsupported" << std::endl; + break; + } + } + } +} + +void ColladaWriter::addBindings(SCENE scene) { + string model_id = kmodel_->getID(); + string inst_model_sid = string("inst_") + model_id; + + // + // + domBind_kinematics_modelRef kmodel_bind = daeSafeCast(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); + kmodel_bind->setNode("v1.node0"); // @todo + daeSafeCast(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str()); + + for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + shared_ptr urdf_joint = i->second; + + int idof = 0; // @todo assuming 1 dof joints + string joint_sid = joint_sids_[urdf_joint->name]; + string axis_name = string("axis") + boost::lexical_cast(idof); + string joint_axis_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name; + string joint_axis_value_sid = joint_axis_sid + string("_value"); + + // + domBind_joint_axisRef joint_bind = daeSafeCast(scene.kiscene->createAndPlace(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()); + { + // + domCommon_sidref_or_paramRef axis_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_AXIS)); + { + daeSafeCast(axis_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_sid.c_str()); + } + // + // + domCommon_float_or_paramRef value_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_VALUE)); + { + daeSafeCast(value_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_value_sid.c_str()); + } + } + // + } +} + +void ColladaWriter::addKinematics(SCENE scene) { + // + domKinematics_modelRef kmodel = daeSafeCast(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); + kmodel->setId("k1"); + kmodel->setName(robot_->getName().c_str()); + { + // + domKinematics_model_techniqueRef technique = daeSafeCast(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + addJoints(technique); + // + + // + int link_num = 0; + addKinematicLink(robot_->getRoot(), technique, link_num); + // + } + kmodel_ = kmodel; + // + + string model_id = kmodel->getID(); + string inst_model_sid = string("inst_") + model_id; + + // + domInstance_kinematics_modelRef ikm = daeSafeCast(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL)); + ikm->setUrl((string("#") + model_id).c_str()); + ikm->setSid(inst_model_sid.c_str()); + { + // + domKinematics_newparamRef newparam_model = daeSafeCast(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); + string newparam_model_sid = string("kscene.inst_") + model_id; + newparam_model->setSid(newparam_model_sid.c_str()); + { + // kscene/inst_k1 + string model_sidref = string("kscene/inst_") + model_id; + daeSafeCast(newparam_model->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(model_sidref.c_str()); + } + // + + for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + shared_ptr urdf_joint = i->second; + + int idof = 0; // @todo assuming 1 dof joints + + string joint_sid = joint_sids_[urdf_joint->name]; + + string axis_name = string("axis") + boost::lexical_cast(idof); + + // + domKinematics_newparamRef newparam = daeSafeCast(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); + string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name; + newparam->setSid(newparam_sid.c_str()); + { + // kscene/inst_k1/joint0/axis0 + string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/") + axis_name; + daeSafeCast(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str()); + } + // + + // + domKinematics_newparamRef newparam_value = daeSafeCast(ikm->createAndPlace(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()); + { + // 0 + daeSafeCast(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f); + } + // + } + } + // +} + +void ColladaWriter::addKinematicLink(shared_ptr urdf_link, daeElementRef parent, int& link_num) { + // + domLinkRef link = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_LINK)); + string link_sid = string("link") + boost::lexical_cast(link_num); + link->setName(urdf_link->name.c_str()); + link->setSid(link_sid.c_str()); + link_num++; + foreach(shared_ptr urdf_joint, urdf_link->child_joints) { + // + domLink::domAttachment_fullRef attachment_full = daeSafeCast(link->createAndPlace(COLLADA_TYPE_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); + addKinematicLink(robot_->getLink(urdf_joint->child_link_name), attachment_full, link_num); + } + // + } + // +} + +void ColladaWriter::addVisuals(SCENE scene) { + // + domNodeRef root_node = daeSafeCast(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE)); + root_node->setId("v1"); + root_node->setName(robot_->getName().c_str()); + { + int link_num = 0; + addVisualLink(robot_->getRoot(), root_node, link_num); + } +} + +void ColladaWriter::addMaterials() { + urdf::Color ambient, diffuse; + ambient.init("1 1 1 0"); + diffuse.init("1 1 1 0"); + + for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { + shared_ptr urdf_link = i->second; + + map::const_iterator j = geometry_ids_.find(urdf_link->name); + if (j == geometry_ids_.end()) + continue; + + string geometry_id = j->second; + + domEffectRef effect = addEffect(geometry_id, ambient, diffuse); + + // + domMaterialRef material = daeSafeCast(materialsLib_->createAndPlace(COLLADA_ELEMENT_MATERIAL)); + string material_id = geometry_id + string(".mat"); + material->setId(material_id.c_str()); + { + // + domInstance_effectRef instance_effect = daeSafeCast(material->createAndPlace(COLLADA_ELEMENT_INSTANCE_EFFECT)); + string effect_id(effect->getId()); + instance_effect->setUrl((string("#") + effect_id).c_str()); + } + // + } +} + +domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse) +{ + // + domEffectRef effect = daeSafeCast(effectsLib_->createAndPlace(COLLADA_ELEMENT_EFFECT)); + string effect_id = geometry_id + string(".eff"); + effect->setId(effect_id.c_str()); + { + // + domProfile_commonRef profile = daeSafeCast(effect->createAndPlace(COLLADA_ELEMENT_PROFILE_COMMON)); + { + // + domProfile_common::domTechniqueRef technique = daeSafeCast(profile->createAndPlace(COLLADA_ELEMENT_TECHNIQUE)); + { + // + domProfile_common::domTechnique::domPhongRef phong = daeSafeCast(technique->createAndPlace(COLLADA_ELEMENT_PHONG)); + { + // + domFx_common_color_or_textureRef ambient = daeSafeCast(phong->createAndPlace(COLLADA_ELEMENT_AMBIENT)); + { + // r g b a + domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast(ambient->createAndPlace(COLLADA_ELEMENT_COLOR)); + ambient_color->getValue().setCount(4); + ambient_color->getValue()[0] = color_ambient.r; + ambient_color->getValue()[1] = color_ambient.g; + ambient_color->getValue()[2] = color_ambient.b; + ambient_color->getValue()[3] = color_ambient.a; + // + } + // + + // + domFx_common_color_or_textureRef diffuse = daeSafeCast(phong->createAndPlace(COLLADA_ELEMENT_DIFFUSE)); + { + // r g b a + domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast(diffuse->createAndPlace(COLLADA_ELEMENT_COLOR)); + diffuse_color->getValue().setCount(4); + diffuse_color->getValue()[0] = color_diffuse.r; + diffuse_color->getValue()[1] = color_diffuse.g; + diffuse_color->getValue()[2] = color_diffuse.b; + diffuse_color->getValue()[3] = color_diffuse.a; + // + } + // + } + // + } + // + } + // + } + // + + return effect; +} + +void ColladaWriter::addVisualLink(shared_ptr urdf_link, daeElementRef parent, int& link_num) { + // + domNodeRef node = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_NODE)); + string node_sid = string("node") + boost::lexical_cast(link_num); + string node_id = string("v1.") + node_sid; + node->setName(urdf_link->name.c_str()); + node->setSid(node_sid.c_str()); + node->setId(node_id.c_str()); + link_num++; + { + if (urdf_link->parent_joint != NULL) { + // x y z w + addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); + // x y z + addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position); + + // x y z angle + domRotateRef joint_rotate = addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); + string joint_sid = joint_sids_[urdf_link->parent_joint->name]; + string joint_rotate_sid = string("node_") + joint_sid + string("_axis0"); + joint_rotate->setSid(joint_rotate_sid.c_str()); + + node_ids_[urdf_link->parent_joint->name] = node_id; + } + + // + map::const_iterator i = geometry_ids_.find(urdf_link->name); + if (i != geometry_ids_.end()) { + domInstance_geometryRef instance_geometry = daeSafeCast(node->createAndPlace(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); + string geometry_id = i->second; + string instance_geometry_url = string("#") + geometry_id; + instance_geometry->setUrl(instance_geometry_url.c_str()); + { + // + domBind_materialRef bind_material = daeSafeCast(instance_geometry->createAndPlace(COLLADA_ELEMENT_BIND_MATERIAL)); + { + // + domBind_material::domTechnique_commonRef technique_common = daeSafeCast(bind_material->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + { + // + domInstance_materialRef instance_material = daeSafeCast(technique_common->createAndPlace(COLLADA_ELEMENT_INSTANCE_MATERIAL)); + instance_material->setTarget((instance_geometry_url + string(".mat")).c_str()); + instance_material->setSymbol("mat0"); + // + } + // + } + // + } + } + // + + // + foreach(shared_ptr link2, urdf_link->child_links) + addVisualLink(link2, node, link_num); + // + } + // +} + +domTranslateRef ColladaWriter::addTranslate(daeElementRef parent, urdf::Vector3 const& position) { + // x y z + domTranslateRef trans = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_TRANSLATE)); + trans->getValue().setCount(3); + trans->getValue()[0] = position.x; + trans->getValue()[1] = position.y; + trans->getValue()[2] = position.z; + return trans; +} + +domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const& r) { + double ax, ay, az, aa; + + // Convert from quaternion to axis-angle + double sqr_len = r.x * r.x + r.y * r.y + r.z * r.z; + if (sqr_len > 0) { + aa = 2 * acos(r.w); + + double inv_len = 1.0 / sqrt(sqr_len); + ax = r.x * inv_len; + ay = r.y * inv_len; + az = r.z * inv_len; + } + else { + // Angle is 0 (mod 2*pi), so any axis will do + aa = 0.0; + ax = 1.0; + ay = 0.0; + az = 0.0; + } + + // x y z w + domRotateRef rot = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_ROTATE)); + rot->getValue().setCount(4); + rot->getValue()[0] = ax; + rot->getValue()[1] = ay; + rot->getValue()[2] = az; + rot->getValue()[3] = angles::to_degrees(aa); + + return rot; +} + +} diff --git a/collada_urdf/src/STLLoader.cpp b/collada_urdf/src/STLLoader.cpp index b96be83..ada5ef4 100644 --- a/collada_urdf/src/STLLoader.cpp +++ b/collada_urdf/src/STLLoader.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2008, Willow Garage, Inc. +* Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,18 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tim Field */ - -// STLLoader.cpp - #include #include #include #include -#include "STLLoader.h" +#include "collada_urdf/STLLoader.h" -using namespace collada_urdf; +namespace collada_urdf { Vector3::Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { } @@ -66,8 +62,6 @@ void Mesh::addVertex(const Vector3& v) { vertices.push_back(v); } void Mesh::addNormal(const Vector3& n) { normals.push_back(n); } void Mesh::addIndex(unsigned int i) { indices.push_back(i); } -// - Mesh* STLLoader::load(const std::string& filename) { Mesh* mesh = new Mesh(); @@ -133,3 +127,5 @@ uint16_t STLLoader::readShortInt(FILE* filein) { return ival; } + +} diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index edf51c9..1693f75 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2008, Willow Garage, Inc. +* Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,955 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tim Field */ - -// urdf_to_collada.cpp - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include - -#include "STLLoader.h" - -using namespace std; - -namespace collada_urdf { - -class ColladaWriter : public daeErrorHandler -{ -public: - struct SCENE - { - domVisual_sceneRef vscene; - domKinematics_sceneRef kscene; - domPhysics_sceneRef pscene; - domInstance_with_extraRef viscene; - domInstance_kinematics_sceneRef kiscene; - domInstance_with_extraRef piscene; - }; - - urdf::Model* robot_; - - boost::shared_ptr collada_; - domCOLLADA* dom_; - domCOLLADA::domSceneRef scene_; - - domLibrary_geometriesRef geometriesLib_; - domLibrary_visual_scenesRef visualScenesLib_; - domLibrary_kinematics_scenesRef kinematicsScenesLib_; - domLibrary_kinematics_modelsRef kinematicsModelsLib_; - domLibrary_jointsRef jointsLib_; - domLibrary_physics_scenesRef physicsScenesLib_; - domLibrary_materialsRef materialsLib_; - domLibrary_effectsRef effectsLib_; - //domLibrary_articulated_systemsRef articulatedSystemsLib_; - - domKinematics_modelRef kmodel_; - - map geometry_ids_; // link.name -> geometry.id - map joint_sids_; // joint.name -> joint.sid - map node_ids_; // joint.name -> node.id - -public: - ColladaWriter(const string& documentName, urdf::Model* robot) : robot_(robot) { - daeErrorHandler::setErrorHandler(this); - - collada_.reset(new DAE()); - collada_->setIOPlugin(NULL); - collada_->setDatabase(NULL); - - daeDocument* doc = NULL; - daeInt error = collada_->getDatabase()->insertDocument(documentName.c_str(), &doc); // also creates a collada root - if (error != DAE_OK || doc == NULL) - { - cerr << "Failed to create new document\n"; - throw; - } - - dom_ = daeSafeCast(doc->getDomRoot()); - dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); - - // Create the required asset tag - domAssetRef asset = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_ASSET)); - { - domAsset::domCreatedRef created = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_CREATED)); - created->setValue("2009-04-06T17:01:00.891550"); // @todo: replace with current date - domAsset::domModifiedRef modified = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_MODIFIED)); - modified->setValue("2009-04-06T17:01:00.891550"); // @todo: replace with current date - - domAsset::domContributorRef contrib = daeSafeCast(asset->createAndPlace(COLLADA_TYPE_CONTRIBUTOR)); - domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast(contrib->createAndPlace(COLLADA_ELEMENT_AUTHORING_TOOL)); - authoringtool->setValue("URDF Collada Writer"); - - domAsset::domUnitRef units = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_UNIT)); - units->setMeter(1); - units->setName("meter"); - - domAsset::domUp_axisRef zup = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_UP_AXIS)); - zup->setValue(UP_AXIS_Z_UP); - } - - scene_ = dom_->getScene(); - if (!scene_) - scene_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); - - visualScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); - visualScenesLib_->setId("vscenes"); - geometriesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); - geometriesLib_->setId("geometries"); - kinematicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); - kinematicsScenesLib_->setId("kscenes"); - kinematicsModelsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); - kinematicsModelsLib_->setId("kmodels"); - jointsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); - jointsLib_->setId("joints"); - - physicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); - physicsScenesLib_->setId("physics_scenes"); - effectsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS)); - effectsLib_->setId("effects"); - materialsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS)); - materialsLib_->setId("materials"); - - //articulatedSystemsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS)); - //articulatedSystemsLib_->setId("articulated_systems"); - } - - virtual ~ColladaWriter() { - collada_.reset(); - DAE::cleanup(); - } - - SCENE createScene() { - SCENE s; - - // Create visual scene - s.vscene = daeSafeCast(visualScenesLib_->createAndPlace(COLLADA_ELEMENT_VISUAL_SCENE)); - s.vscene->setId("vscene"); - s.vscene->setName("URDF Visual Scene"); - - // Create instance visual scene - s.viscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE)); - s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str()); - - // Create kinematics scene - s.kscene = daeSafeCast(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE)); - s.kscene->setId("kscene"); - s.kscene->setName("URDF Kinematics Scene"); - - // Create instance kinematics scene - s.kiscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE)); - s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str()); - - // Create physics scene - s.pscene = daeSafeCast(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE)); - s.pscene->setId("pscene"); - s.pscene->setName("URDF Physics Scene"); - - // Create instance physics scene - s.piscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE)); - s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str()); - - return s; - } - - virtual void handleError(daeString msg) { - cerr << "COLLADA error: " << msg << "\n"; - } - - virtual void handleWarning(daeString msg) { - cerr << "COLLADA warning: " << msg << "\n"; - } - - bool writeScene() { - SCENE scene = createScene(); - - setupPhysics(scene); - addGeometries(); - addKinematics(scene); - addVisuals(scene); - addMaterials(); - addBindings(scene); - - collada_->writeAll(); - - return true; - } - - void setupPhysics(SCENE scene) { - // - domPhysics_scene::domTechnique_commonRef common = daeSafeCast(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - { - // 0 0 0 - domTargetable_float3Ref g = daeSafeCast(common->createAndPlace(COLLADA_ELEMENT_GRAVITY)); - g->getValue().set3(0.0, 0.0, 0.0); - // - } - // - } - - void addGeometries() { - int link_num = 0; - - for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { - boost::shared_ptr urdf_link = i->second; - - if (urdf_link->visual == NULL || urdf_link->visual->geometry == NULL) - continue; - - switch (urdf_link->visual->geometry->type) { - case urdf::Geometry::MESH: { - urdf::Mesh* urdf_mesh = (urdf::Mesh*) urdf_link->visual->geometry.get(); - - string filename = urdf_mesh->filename; - urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale - - // - domGeometryRef geometry = daeSafeCast(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY)); - string geometry_id = string("g1.link") + boost::lexical_cast(link_num) + string(".geom0"); - geometry->setId(geometry_id.c_str()); - { - loadMesh(filename, geometry, geometry_id); - } - geometry_ids_[urdf_link->name] = geometry_id; - // - - link_num++; - break; - } - case urdf::Geometry::SPHERE: { - cerr << "Geometry type SPHERE of link " << urdf_link->name << " is unsupported" << endl; - break; - } - case urdf::Geometry::BOX: { - cerr << "Geometry type BOX of link " << urdf_link->name << " is unsupported" << endl; - break; - } - case urdf::Geometry::CYLINDER: { - cerr << "Geometry type CYLINDER of link " << urdf_link->name << " is unsupported" << endl; - break; - } - default: { - cerr << "Geometry type " << urdf_link->visual->geometry->type << " of link " << urdf_link->name << " is supported" << endl; - break; - } - } - } - } - - void loadMesh(const string& filename, domGeometryRef geometry, const string& geometry_id) { - // Load the mesh - resource_retriever::MemoryResource resource; - resource_retriever::Retriever retriever; - try { - resource = retriever.get(filename.c_str()); - } - catch (resource_retriever::Exception& e) { - cerr << "Unable to load mesh file " << filename << ": " << e.what() << endl; - return; - } - - // Try assimp first, then STLLoader - if (!loadMeshWithAssimp(resource, geometry, geometry_id)) - if (!loadMeshWithSTLLoader(resource, geometry, geometry_id)) - cerr << "*** Can't load " << filename << endl; - } - - bool loadMeshWithAssimp(const resource_retriever::MemoryResource& resource, domGeometryRef geometry, const string& geometry_id) { - // Import the mesh using assimp - Assimp::Importer importer; - const aiScene* scene = importer.ReadFileFromMemory(resource.data.get(), resource.size, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */); - if (!scene) - return false; - - buildMeshFromAssimp(scene, scene->mRootNode, geometry, geometry_id); - return true; - } - - bool loadMeshWithSTLLoader(const resource_retriever::MemoryResource& resource, domGeometryRef geometry, const string& geometry_id) { - // Write the resource to a temporary file - char tmp_filename[] = "/tmp/collada_urdf_XXXXXX"; - int fd = mkstemp(tmp_filename); - write(fd, resource.data.get(), resource.size); - close(fd); - - // Import the mesh using STLLoader - STLLoader loader; - Mesh* stl_mesh = loader.load(string(tmp_filename)); - buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id); - delete stl_mesh; - - // Delete the temporary file - unlink(tmp_filename); - - return true; - } - - void buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, const string& geometry_id) { - // - domMeshRef mesh = daeSafeCast(parent->createAndPlace(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; - - // - domSourceRef positions_source = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); - positions_source->setId((geometry_id + string(".positions")).c_str()); - { - // - domFloat_arrayRef positions_array = daeSafeCast(positions_source->createAndPlace(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 - positions_array->getValue().setCount(num_vertices * 3); - for (unsigned int j = 0; j < num_vertices; j++) { - positions_array->getValue()[j * 3 ] = stl_mesh->vertices[j].x; - positions_array->getValue()[j * 3 + 1] = stl_mesh->vertices[j].y; - positions_array->getValue()[j * 3 + 2] = stl_mesh->vertices[j].z; - } - // - - // - domSource::domTechnique_commonRef source_tech = daeSafeCast(positions_source->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - { - // - domAccessorRef accessor = daeSafeCast(source_tech->createAndPlace(COLLADA_ELEMENT_ACCESSOR)); - accessor->setCount(num_vertices / 3); - accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array"))); - accessor->setStride(3); - { - // - // - // - domParamRef px = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float"); - domParamRef py = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float"); - domParamRef pz = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float"); - } - // - } - // - } - - // - domVerticesRef vertices = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_VERTICES)); - string vertices_id = geometry_id + string(".vertices"); - vertices->setId(vertices_id.c_str()); - { - // - domInput_localRef vertices_input = daeSafeCast(vertices->createAndPlace(COLLADA_ELEMENT_INPUT)); - vertices_input->setSemantic("POSITION"); - vertices_input->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId()))); - } - // - - // - domTrianglesRef triangles = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); - triangles->setCount(num_faces); - triangles->setMaterial("mat0"); - { - // - domInput_local_offsetRef vertex_offset = daeSafeCast(triangles->createAndPlace(COLLADA_ELEMENT_INPUT)); - vertex_offset->setSemantic("VERTEX"); - vertex_offset->setOffset(0); - vertex_offset->setSource(domUrifragment(*positions_source, string("#") + vertices_id)); - { - //

0 1 2 3 ... - domPRef indices = daeSafeCast(triangles->createAndPlace(COLLADA_ELEMENT_P)); - indices->getValue().setCount(num_indices); - for (unsigned int i = 0; i < num_indices; i++) - indices->getValue()[i] = stl_mesh->indices[i]; - //

- } - } - // - } - // - } - - // EXPERIMENTAL: untested - void buildMeshFromAssimp(const aiScene* scene, aiNode* node, daeElementRef parent, const string& geometry_id) { - if (node == NULL) - return; - - aiMatrix4x4 transform = node->mTransformation; - - aiNode* pnode = node->mParent; - while (pnode) { - // Don't convert to y-up orientation, which is what the root node in Assimp does - if (pnode->mParent != NULL) - transform = pnode->mTransformation * transform; - pnode = pnode->mParent; - } - - // - domMeshRef mesh = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_MESH)); - { - for (unsigned int i = 0; i < node->mNumMeshes; i++) { - aiMesh* aMesh = scene->mMeshes[i]; - - // Add in the indices for each face - for (unsigned int j = 0; j < aMesh->mNumFaces; j++) { - aiFace* aFace = &(aMesh->mFaces[j]); - for (unsigned int k = 0; k < aFace->mNumIndices; k++) { - //int index = aFace->mIndices[k]; - // @todo add index - //subMesh->AddIndex(aFace->mIndices[k]); - } - } - - domSourceRef positions_source = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); - { - positions_source->setId((geometry_id + string(".positions")).c_str()); - - domFloat_arrayRef positions_array = daeSafeCast(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY)); - positions_array->setId((geometry_id + string(".positions-array")).c_str()); - positions_array->setCount(aMesh->mNumVertices); - positions_array->setDigits(6); // 6 decimal places - positions_array->getValue().setCount(3 * aMesh->mNumVertices); - - for (unsigned int j = 0; j < aMesh->mNumVertices; j++) { - aiVector3D p; - p.x = aMesh->mVertices[j].x; - p.y = aMesh->mVertices[j].y; - p.z = aMesh->mVertices[j].z; - - p *= transform; - - positions_array->getValue()[j] = p.x; - positions_array->getValue()[j] = p.y; - positions_array->getValue()[j] = p.z; - - /* - if (aMesh->HasNormals()) { - p.x = aMesh->mNormals[j].x; - p.y = aMesh->mNormals[j].y; - p.z = aMesh->mNormals[j].z; - } - - // @todo add normal - //subMesh->AddNormal(p.x, p.y, p.z); - - // @todo add tex coord - //if (aMesh->mNumUVComponents[0]) - // subMesh->AddTexCoord(aMesh->mTextureCoords[0][j].x, 1.0 -aMesh->mTextureCoords[0][j].y); - //else - // subMesh->AddTexCoord(0,0); - */ - } - } - - domTrianglesRef triangles = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); - { - triangles->setCount(aMesh->mNumFaces / 3); - } - } - - for (unsigned int i = 0; i < node->mNumChildren; i++) - buildMeshFromAssimp(scene, node->mChildren[i], mesh, geometry_id); - } - } - - void addJoints(daeElementRef parent) { - int joint_num = 0; - for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { - boost::shared_ptr urdf_joint = i->second; - - // - domJointRef joint = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_JOINT)); - string joint_sid = string("joint") + boost::lexical_cast(joint_num); - joint_num++; - joint->setName(urdf_joint->name.c_str()); - joint->setSid(joint_sid.c_str()); - joint_sids_[urdf_joint->name] = joint_sid; - - double axis_x = urdf_joint->axis.x; - double axis_y = urdf_joint->axis.y; - double axis_z = urdf_joint->axis.z; - if (axis_x == 0.0 && axis_y == 0.0 && axis_z == 0.0) { - axis_x = 1.0; - axis_y = 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) - { - case urdf::Joint::REVOLUTE: { - // - domAxis_constraintRef revolute = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); - revolute->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); - { - axis->getValue().setCount(3); - axis->getValue()[0] = axis_x; - axis->getValue()[1] = axis_y; - axis->getValue()[2] = axis_z; - } - // - - // - domJoint_limitsRef limits = daeSafeCast(revolute->createAndPlace(COLLADA_TYPE_LIMITS)); - { - daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower); - daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper); - } - // - } - // - break; - } - case urdf::Joint::CONTINUOUS: { - // Model as a REVOLUTE joint without limits - - // - domAxis_constraintRef revolute = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); - revolute->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); - { - axis->getValue().setCount(3); - axis->getValue()[0] = axis_x; - axis->getValue()[1] = axis_y; - axis->getValue()[2] = axis_z; - } - // - } - // - break; - } - case urdf::Joint::PRISMATIC: { - // - domAxis_constraintRef prismatic = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC)); - prismatic->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(prismatic->createAndPlace(COLLADA_ELEMENT_AXIS)); - { - axis->getValue().setCount(3); - axis->getValue()[0] = axis_x; - axis->getValue()[1] = axis_y; - axis->getValue()[2] = axis_z; - } - // - - // - domJoint_limitsRef limits = daeSafeCast(prismatic->createAndPlace(COLLADA_TYPE_LIMITS)); - { - daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower; - daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper; - } - // - } - // - break; - } - case urdf::Joint::FIXED: { - // Model as a REVOLUTE joint with no leeway - - domAxis_constraintRef revolute = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); - revolute->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); - { - axis->getValue().setCount(3); - axis->getValue()[0] = axis_x; - axis->getValue()[1] = axis_y; - axis->getValue()[2] = axis_z; - } - // - - // - domJoint_limitsRef limits = daeSafeCast(revolute->createAndPlace(COLLADA_TYPE_LIMITS)); - { - daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = 0.0; - daeSafeCast(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = 0.0; - } - // - } - // - break; - } - case urdf::Joint::UNKNOWN: { - cerr << "Joint type UNKNOWN of joint " << urdf_joint->name << " is unsupported" << endl; - break; - } - case urdf::Joint::FLOATING: { - cerr << "Joint type FLOATING of joint " << urdf_joint->name << " is unsupported" << endl; - break; - } - case urdf::Joint::PLANAR: { - cerr << "Joint type PLANAR of joint " << urdf_joint->name << " is unsupported" << endl; - break; - } - default: { - cerr << "Joint type " << urdf_joint->type << " of joint " << urdf_joint->name << " is unsupported" << endl; - break; - } - } - } - } - - void addBindings(SCENE scene) { - string model_id = kmodel_->getID(); - string inst_model_sid = string("inst_") + model_id; - - // - // - domBind_kinematics_modelRef kmodel_bind = daeSafeCast(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); - kmodel_bind->setNode("v1.node0"); // @todo - daeSafeCast(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str()); - - for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { - boost::shared_ptr urdf_joint = i->second; - - int idof = 0; // @todo assuming 1 dof joints - string joint_sid = joint_sids_[urdf_joint->name]; - string axis_name = string("axis") + boost::lexical_cast(idof); - string joint_axis_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name; - string joint_axis_value_sid = joint_axis_sid + string("_value"); - - // - domBind_joint_axisRef joint_bind = daeSafeCast(scene.kiscene->createAndPlace(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()); - { - // - domCommon_sidref_or_paramRef axis_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_AXIS)); - { - daeSafeCast(axis_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_sid.c_str()); - } - // - // - domCommon_float_or_paramRef value_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_VALUE)); - { - daeSafeCast(value_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_value_sid.c_str()); - } - } - // - } - } - - void addKinematics(SCENE scene) { - // - domKinematics_modelRef kmodel = daeSafeCast(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); - kmodel->setId("k1"); - kmodel->setName(robot_->getName().c_str()); - { - // - domKinematics_model_techniqueRef technique = daeSafeCast(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - addJoints(technique); - // - - // - int link_num = 0; - addKinematicLink(robot_->getRoot(), technique, link_num); - // - } - kmodel_ = kmodel; - // - - string model_id = kmodel->getID(); - string inst_model_sid = string("inst_") + model_id; - - // - domInstance_kinematics_modelRef ikm = daeSafeCast(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL)); - ikm->setUrl((string("#") + model_id).c_str()); - ikm->setSid(inst_model_sid.c_str()); - { - // - domKinematics_newparamRef newparam_model = daeSafeCast(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); - string newparam_model_sid = string("kscene.inst_") + model_id; - newparam_model->setSid(newparam_model_sid.c_str()); - { - // kscene/inst_k1 - string model_sidref = string("kscene/inst_") + model_id; - daeSafeCast(newparam_model->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(model_sidref.c_str()); - } - // - - for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { - boost::shared_ptr urdf_joint = i->second; - - int idof = 0; // @todo assuming 1 dof joints - - string joint_sid = joint_sids_[urdf_joint->name]; - - string axis_name = string("axis") + boost::lexical_cast(idof); - - // - domKinematics_newparamRef newparam = daeSafeCast(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); - string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name; - newparam->setSid(newparam_sid.c_str()); - { - // kscene/inst_k1/joint0/axis0 - string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/") + axis_name; - daeSafeCast(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str()); - } - // - - // - domKinematics_newparamRef newparam_value = daeSafeCast(ikm->createAndPlace(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()); - { - // 0 - daeSafeCast(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f); - } - // - } - } - // - } - - void addKinematicLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num) { - // - domLinkRef link = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_LINK)); - string link_sid = string("link") + boost::lexical_cast(link_num); - link->setName(urdf_link->name.c_str()); - link->setSid(link_sid.c_str()); - link_num++; - for (vector >::const_iterator i = urdf_link->child_joints.begin(); i != urdf_link->child_joints.end(); i++) { - boost::shared_ptr urdf_joint = *i; - - // - domLink::domAttachment_fullRef attachment_full = daeSafeCast(link->createAndPlace(COLLADA_TYPE_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); - addKinematicLink(robot_->getLink(urdf_joint->child_link_name), attachment_full, link_num); - } - // - } - // - } - - void addVisuals(SCENE scene) { - // - domNodeRef root_node = daeSafeCast(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE)); - root_node->setId("v1"); - root_node->setName(robot_->getName().c_str()); - { - int link_num = 0; - addVisualLink(robot_->getRoot(), root_node, link_num); - } - } - - void addMaterials() { - urdf::Color ambient, diffuse; - ambient.init("1 1 1 0"); - diffuse.init("1 1 1 0"); - - for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { - boost::shared_ptr urdf_link = i->second; - - map::const_iterator j = geometry_ids_.find(urdf_link->name); - if (j != geometry_ids_.end()) { - string geometry_id = j->second; - - domEffectRef effect = addEffect(geometry_id, ambient, diffuse); - - // - domMaterialRef material = daeSafeCast(materialsLib_->createAndPlace(COLLADA_ELEMENT_MATERIAL)); - string material_id = geometry_id + string(".mat"); - material->setId(material_id.c_str()); - { - // - domInstance_effectRef instance_effect = daeSafeCast(material->createAndPlace(COLLADA_ELEMENT_INSTANCE_EFFECT)); - string effect_id(effect->getId()); - instance_effect->setUrl((string("#") + effect_id).c_str()); - } - // - } - } - } - - domEffectRef addEffect(const string& geometry_id, const urdf::Color& color_ambient, const urdf::Color& color_diffuse) - { - // - domEffectRef effect = daeSafeCast(effectsLib_->createAndPlace(COLLADA_ELEMENT_EFFECT)); - string effect_id = geometry_id + string(".eff"); - effect->setId(effect_id.c_str()); - { - // - domProfile_commonRef profile = daeSafeCast(effect->createAndPlace(COLLADA_ELEMENT_PROFILE_COMMON)); - { - // - domProfile_common::domTechniqueRef technique = daeSafeCast(profile->createAndPlace(COLLADA_ELEMENT_TECHNIQUE)); - { - // - domProfile_common::domTechnique::domPhongRef phong = daeSafeCast(technique->createAndPlace(COLLADA_ELEMENT_PHONG)); - { - // - domFx_common_color_or_textureRef ambient = daeSafeCast(phong->createAndPlace(COLLADA_ELEMENT_AMBIENT)); - { - // r g b a - domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast(ambient->createAndPlace(COLLADA_ELEMENT_COLOR)); - ambient_color->getValue().setCount(4); - ambient_color->getValue()[0] = color_ambient.r; - ambient_color->getValue()[1] = color_ambient.g; - ambient_color->getValue()[2] = color_ambient.b; - ambient_color->getValue()[3] = color_ambient.a; - // - } - // - - // - domFx_common_color_or_textureRef diffuse = daeSafeCast(phong->createAndPlace(COLLADA_ELEMENT_DIFFUSE)); - { - // r g b a - domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast(diffuse->createAndPlace(COLLADA_ELEMENT_COLOR)); - diffuse_color->getValue().setCount(4); - diffuse_color->getValue()[0] = color_diffuse.r; - diffuse_color->getValue()[1] = color_diffuse.g; - diffuse_color->getValue()[2] = color_diffuse.b; - diffuse_color->getValue()[3] = color_diffuse.a; - // - } - // - } - // - } - // - } - // - } - // - - return effect; - } - - void addVisualLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num) { - // - domNodeRef node = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_NODE)); - string node_sid = string("node") + boost::lexical_cast(link_num); - string node_id = string("v1.") + node_sid; - node->setName(urdf_link->name.c_str()); - node->setSid(node_sid.c_str()); - node->setId(node_id.c_str()); - link_num++; - { - if (urdf_link->parent_joint != NULL) { - // x y z w - addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); - // x y z - addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position); - - // x y z angle - domRotateRef joint_rotate = addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); - string joint_sid = joint_sids_[urdf_link->parent_joint->name]; - string joint_rotate_sid = string("node_") + joint_sid + string("_axis0"); - joint_rotate->setSid(joint_rotate_sid.c_str()); - - node_ids_[urdf_link->parent_joint->name] = node_id; - } - - // - map::const_iterator i = geometry_ids_.find(urdf_link->name); - if (i != geometry_ids_.end()) { - domInstance_geometryRef instance_geometry = daeSafeCast(node->createAndPlace(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); - string geometry_id = i->second; - string instance_geometry_url = string("#") + geometry_id; - instance_geometry->setUrl(instance_geometry_url.c_str()); - { - // - domBind_materialRef bind_material = daeSafeCast(instance_geometry->createAndPlace(COLLADA_ELEMENT_BIND_MATERIAL)); - { - // - domBind_material::domTechnique_commonRef technique_common = daeSafeCast(bind_material->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - { - // - domInstance_materialRef instance_material = daeSafeCast(technique_common->createAndPlace(COLLADA_ELEMENT_INSTANCE_MATERIAL)); - instance_material->setTarget((instance_geometry_url + string(".mat")).c_str()); - instance_material->setSymbol("mat0"); - // - } - // - } - // - } - } - // - - // - for (vector >::const_iterator i = urdf_link->child_links.begin(); i != urdf_link->child_links.end(); i++) - addVisualLink(*i, node, link_num); - // - } - // - } - - domTranslateRef addTranslate(daeElementRef parent, const urdf::Vector3& position) { - // x y z - domTranslateRef trans = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_TRANSLATE)); - trans->getValue().setCount(3); - trans->getValue()[0] = position.x; - trans->getValue()[1] = position.y; - trans->getValue()[2] = position.z; - return trans; - } - - domRotateRef addRotate(daeElementRef parent, const urdf::Rotation& r) { - double ax, ay, az, aa; - - // Convert from quaternion to axis-angle - double sqr_len = r.x * r.x + r.y * r.y + r.z * r.z; - if (sqr_len > 0) { - aa = 2 * acos(r.w); - - double inv_len = 1.0 / sqrt(sqr_len); - ax = r.x * inv_len; - ay = r.y * inv_len; - az = r.z * inv_len; - } - else { - // Angle is 0 (mod 2*pi), so any axis will do - aa = 0.0; - ax = 1.0; - ay = 0.0; - az = 0.0; - } - - // x y z w - domRotateRef rot = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_ROTATE)); - rot->getValue().setCount(4); - rot->getValue()[0] = ax; - rot->getValue()[1] = ay; - rot->getValue()[2] = az; - rot->getValue()[3] = angles::to_degrees(aa); - return rot; - } -}; - -} +#include "collada_urdf/ColladaWriter.h" int main(int argc, char** argv) { @@ -1003,7 +55,7 @@ int main(int argc, char** argv) return -1; } - string output_filename(argv[2]); + std::string output_filename(argv[2]); collada_urdf::ColladaWriter* writer = new collada_urdf::ColladaWriter(output_filename, &robot); writer->writeScene();