diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index ee272a6..c7daaff 100644 --- a/collada_urdf/CMakeLists.txt +++ b/collada_urdf/CMakeLists.txt @@ -4,7 +4,7 @@ set(ROS_BUILD_TYPE Debug) rosbuild_init() set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -rosbuild_add_library(collada_urdf src/collada_urdf.cpp src/collada_writer.cpp src/stl_loader.cpp) +rosbuild_add_library(collada_urdf src/collada_urdf.cpp) rosbuild_link_boost(collada_urdf system) rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp) diff --git a/collada_urdf/include/collada_urdf/collada_urdf.h b/collada_urdf/include/collada_urdf/collada_urdf.h index 44f5837..558e51f 100644 --- a/collada_urdf/include/collada_urdf/collada_urdf.h +++ b/collada_urdf/include/collada_urdf/collada_urdf.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2010, Willow Garage, Inc. +* Copyright (c) 2010, Willow Garage, Inc., University of Tokyo * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,17 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tim Field */ +/* Authors: Tim Field, Rosen Diankov */ #ifndef COLLADA_URDF_COLLADA_URDF_H #define COLLADA_URDF_COLLADA_URDF_H #include - #include - #include - #include "urdf/model.h" namespace collada_urdf { @@ -53,33 +50,20 @@ public: ColladaUrdfException(std::string const& what); }; -/** Construct a COLLADA DOM from an URDF file - * \param file The filename from where to read the URDF - * \param dom The resulting COLLADA DOM - * \return true on success, false on failure - */ -bool colladaFromUrdfFile(std::string const& file, boost::shared_ptr& dom); +enum WriteOptions +{ + WO_IgnoreCollisionGeometry = 1, ///< if set, will use only the visual geometry +}; -/** Construct a COLLADA DOM from a string containing URDF - * \param xml A string containing the XML description of the robot - * \param dom The resulting COLLADA DOM - * \return true on success, false on failure +/** Construct a COLLADA DOM from an URDF model + + \param robot_model The initialized robot model + \param dom The resulting COLLADA DOM + \param writeoptions A combination of \ref WriteOptions + + \return true on success, false on failure */ -bool colladaFromUrdfString(std::string const& xml, boost::shared_ptr& dom); - -/** Construct a COLLADA DOM from a TiXmlDocument containing URDF - * \param xml_doc The TiXmlDocument containing URDF - * \param dom The resulting COLLADA DOM - * \return true on success, false on failure - */ -bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr& dom); - -/** Construct a COLLADA DOM from a URDF robot model - * \param robot_model The URDF robot model - * \param dom The resulting COLLADA DOM - * \return true on success, false on failure - */ -bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr& dom); +bool colladaFromUrdfModel(const urdf::Model& robot_model, boost::shared_ptr& dom, int writeoptions=0); /** Write a COLLADA DOM to a file * \param dom COLLADA DOM to write diff --git a/collada_urdf/include/collada_urdf/collada_writer.h b/collada_urdf/include/collada_urdf/collada_writer.h deleted file mode 100644 index c9c8297..0000000 --- a/collada_urdf/include/collada_urdf/collada_writer.h +++ /dev/null @@ -1,144 +0,0 @@ -/********************************************************************* -* 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. -*********************************************************************/ - -/* Author: Tim Field */ - -#ifndef COLLADA_URDF_COLLADA_WRITER_H -#define COLLADA_URDF_COLLADA_WRITER_H - -#include "collada_urdf/collada_urdf.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace collada_urdf { - -class Mesh; - -/** - * Implements writing urdf::Model objects to a COLLADA DOM. - * - * The API for this class is unstable. The public API for collada_urdf is declared in collada_urdf.h. - */ -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(urdf::Model const& robot); - virtual ~ColladaWriter(); - - boost::shared_ptr convert(); - -protected: - virtual void handleError(daeString msg); - virtual void handleWarning(daeString msg); - -private: - void initDocument(std::string const& documentName); - SCENE createScene(); - - void setupPhysics(SCENE const& 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(boost::shared_ptr stl_mesh, daeElementRef parent, std::string const& geometry_id); - - void addKinematics(SCENE const& scene); - void addJoints(daeElementRef parent); - void addKinematicLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); - - void addVisuals(SCENE const& scene); - void addVisualLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); - - void addMaterials(); - domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse); - - void addBindings(SCENE const& scene); - - 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); - void addMimicJoint(domFormulaRef formula, const std::string& joint_sid,const std::string& joint_mimic_sid, double multiplier, double offset); - std::string getTimeStampString() const; - -private: - static int s_doc_count_; - - 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/include/collada_urdf/stl_loader.h b/collada_urdf/include/collada_urdf/stl_loader.h deleted file mode 100644 index 4241c54..0000000 --- a/collada_urdf/include/collada_urdf/stl_loader.h +++ /dev/null @@ -1,95 +0,0 @@ -/********************************************************************* -* 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. -*********************************************************************/ - -/* Author: Tim Field */ - -#ifndef COLLADA_URDF_STL_LOADER_H -#define COLLADA_URDF_STL_LOADER_H - -#include - -#include -#include - -#include - -namespace collada_urdf { - -class Vector3 -{ -public: - Vector3(float x, float y, float z); - - bool operator==(Vector3 const& v) const; - - float x; - float y; - float z; -}; - -class Mesh -{ -public: - Mesh(); - - int getVertexIndex(Vector3 const& v) const; - - 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; -}; - -class STLLoader -{ -public: - boost::shared_ptr load(std::string const& filename); - -private: - FILE* file_; - boost::shared_ptr mesh_; - - void readBinary(); - uint32_t readLongInt(); - uint16_t readShortInt(); - float readFloat(); -}; - -} - -#endif diff --git a/collada_urdf/manifest.xml b/collada_urdf/manifest.xml index 7addbdb..502f9d7 100644 --- a/collada_urdf/manifest.xml +++ b/collada_urdf/manifest.xml @@ -6,7 +6,7 @@ Implements robot-specific COLLADA extensions as defined by http://openrave.programmingvision.com/index.php/Started:COLLADA - Tim Field + Tim Field and Rosen Diankov BSD http://ros.org/wiki/collada_urdf @@ -15,6 +15,7 @@ + diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index 63ea86e..394d648 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2010, Willow Garage, Inc. +* Copyright (c) 2010, Willow Garage, Inc., University of Tokyo * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,59 +32,1212 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Tim Field */ +/* Authors: Rosen Diankov, Tim Field */ #include "collada_urdf/collada_urdf.h" -#include "collada_urdf/collada_writer.h" +#include +#include +#include -using std::string; -using boost::shared_ptr; +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++) +#define FOREACHC FOREACH + +using namespace std; namespace collada_urdf { +/// ResourceIOStream is copied from rviz (BSD, Willow Garage) +class ResourceIOStream : public Assimp::IOStream +{ +public: + ResourceIOStream(const resource_retriever::MemoryResource& res) + : res_(res) + , pos_(res.data.get()) + {} + + ~ResourceIOStream() + {} + + size_t Read(void* buffer, size_t size, size_t count) + { + size_t to_read = size * count; + if (pos_ + to_read > res_.data.get() + res_.size) + { + to_read = res_.size - (pos_ - res_.data.get()); + } + + memcpy(buffer, pos_, to_read); + pos_ += to_read; + + return to_read; + } + + size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; } + + aiReturn Seek( size_t offset, aiOrigin origin) + { + uint8_t* new_pos = 0; + switch (origin) + { + case aiOrigin_SET: + new_pos = res_.data.get() + offset; + break; + case aiOrigin_CUR: + new_pos = pos_ + offset; // TODO is this right? can offset really not be negative + break; + case aiOrigin_END: + new_pos = res_.data.get() + res_.size - offset; // TODO is this right? + break; + default: + ROS_BREAK(); + } + + if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size) + { + return aiReturn_FAILURE; + } + + pos_ = new_pos; + return aiReturn_SUCCESS; + } + + size_t Tell() const + { + return pos_ - res_.data.get(); + } + + size_t FileSize() const + { + return res_.size; + } + + void Flush() {} + +private: + resource_retriever::MemoryResource res_; + uint8_t* pos_; +}; + +/// ResourceIOSystem is copied from rviz (BSD, Willow Garage) +class ResourceIOSystem : public Assimp::IOSystem +{ +public: + ResourceIOSystem() + { + } + + ~ResourceIOSystem() + { + } + + // Check whether a specific file exists + bool Exists(const char* file) const + { + // Ugly -- two retrievals where there should be one (Exists + Open) + // resource_retriever needs a way of checking for existence + // TODO: cache this + resource_retriever::MemoryResource res; + try { + res = retriever_.get(file); + } + catch (resource_retriever::Exception& e) { + return false; + } + + return true; + } + + // Get the path delimiter character we'd like to see + char getOsSeparator() const + { + return '/'; + } + + // ... and finally a method to open a custom stream + Assimp::IOStream* Open(const char* file, const char* mode) + { + ROS_ASSERT(mode == std::string("r") || mode == std::string("rb")); + + // Ugly -- two retrievals where there should be one (Exists + Open) + // resource_retriever needs a way of checking for existence + resource_retriever::MemoryResource res; + try { + res = retriever_.get(file); + } + catch (resource_retriever::Exception& e) { + return 0; + } + + return new ResourceIOStream(res); + } + + void Close(Assimp::IOStream* stream) { delete stream; } + +private: + mutable resource_retriever::Retriever retriever_; +}; + +/** \brief Implements writing urdf::Model objects to a COLLADA DOM. +*/ +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; + }; + + struct LINKOUTPUT + { + list > listusedlinks; + list > listprocesseddofs; + daeElementRef plink; + domNodeRef pnode; + }; + + struct kinematics_model_output + { + struct axis_output + { + //axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {} + axis_output() : iaxis(0) {} + string sid, nodesid; + boost::shared_ptr pjoint; + int iaxis; + string jointnodesid; + }; + domKinematics_modelRef kmodel; + std::vector vaxissids; + std::vector vlinksids; + }; + + struct axis_sids + { + axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {} + string axissid, valuesid, jointnodesid; + }; + + struct instance_kinematics_model_output + { + domInstance_kinematics_modelRef ikm; + std::vector vaxissids; + boost::shared_ptr kmout; + std::vector > vkinematicsbindings; + }; + + struct instance_articulated_system_output + { + domInstance_articulated_systemRef ias; + std::vector vaxissids; + std::vector vlinksids; + std::vector > vkinematicsbindings; + }; + +public: + ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) { + daeErrorHandler::setErrorHandler(this); + _collada.reset(new DAE); + _collada->setIOPlugin(NULL); + _collada->setDatabase(NULL); + _importer.SetIOHandler(new ResourceIOSystem()); + } + virtual ~ColladaWriter() {} + + boost::shared_ptr convert() + { + try { + const char* documentName = "urdf_snapshot"; + daeDocument *doc = NULL; + daeInt error = _collada->getDatabase()->insertDocument(documentName, &doc ); // also creates a collada root + if (error != DAE_OK || doc == NULL) { + throw ColladaUrdfException("Failed to create document"); + } + _dom = daeSafeCast(doc->getDomRoot()); + _dom->setAttribute("xmlns:math","http://www.w3.org/1998/Math/MathML"); + + //create the required asset tag + domAssetRef asset = daeSafeCast( _dom->add( COLLADA_ELEMENT_ASSET ) ); + { + // facet becomes owned by locale, so no need to explicitly delete + boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s"); + std::stringstream ss; + ss.imbue(std::locale(ss.getloc(), facet)); + ss << boost::posix_time::second_clock::local_time(); + + domAsset::domCreatedRef created = daeSafeCast( asset->add( COLLADA_ELEMENT_CREATED ) ); + created->setValue(ss.str().c_str()); + domAsset::domModifiedRef modified = daeSafeCast( asset->add( COLLADA_ELEMENT_MODIFIED ) ); + modified->setValue(ss.str().c_str()); + + domAsset::domContributorRef contrib = daeSafeCast( asset->add( COLLADA_TYPE_CONTRIBUTOR ) ); + domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) ); + authoringtool->setValue("URDF Collada Writer"); + + domAsset::domUnitRef units = daeSafeCast( asset->add( COLLADA_ELEMENT_UNIT ) ); + units->setMeter(1); + units->setName("meter"); + + domAsset::domUp_axisRef zup = daeSafeCast( asset->add( COLLADA_ELEMENT_UP_AXIS ) ); + zup->setValue(UP_AXIS_Z_UP); + } + + _globalscene = _dom->getScene(); + if( !_globalscene ) { + _globalscene = daeSafeCast( _dom->add( COLLADA_ELEMENT_SCENE ) ); + } + + _visualScenesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); + _visualScenesLib->setId("vscenes"); + _geometriesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); + _geometriesLib->setId("geometries"); + _effectsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS)); + _effectsLib->setId("effects"); + _materialsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS)); + _materialsLib->setId("materials"); + _kinematicsModelsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); + _kinematicsModelsLib->setId("kmodels"); + _articulatedSystemsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS)); + _articulatedSystemsLib->setId("asystems"); + _kinematicsScenesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); + _kinematicsScenesLib->setId("kscenes"); + _physicsScenesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); + _physicsScenesLib->setId("pscenes"); + domExtraRef pextra_library_sensors = daeSafeCast(_dom->add(COLLADA_ELEMENT_EXTRA)); + pextra_library_sensors->setId("sensors"); + pextra_library_sensors->setType("library_sensors"); + _sensorsLib = daeSafeCast(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE)); + _sensorsLib->setProfile("OpenRAVE"); ///< documented profile on robot extensions + + _CreateScene(); + _WritePhysics(); + _WriteRobot(); + _WriteBindingsInstance_kinematics_scene(); + return _collada; + } + catch (ColladaUrdfException ex) { + ROS_ERROR("Error converting: %s", ex.what()); + return boost::shared_ptr(); + } + } + +protected: + virtual void handleError(daeString msg) { throw ColladaUrdfException(msg); } + virtual void handleWarning(daeString msg) { std::cerr << "COLLADA DOM warning: " << msg << std::endl; } + + void _CreateScene() + { + // Create visual scene + _scene.vscene = daeSafeCast(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE)); + _scene.vscene->setId("vscene"); + _scene.vscene->setName("URDF Visual Scene"); + + // Create kinematics scene + _scene.kscene = daeSafeCast(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE)); + _scene.kscene->setId("kscene"); + _scene.kscene->setName("URDF Kinematics Scene"); + + // Create physic scene + _scene.pscene = daeSafeCast(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE)); + _scene.pscene->setId("pscene"); + _scene.pscene->setName("URDF Physics Scene"); + + // Create instance visual scene + _scene.viscene = daeSafeCast(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE )); + _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() ); + + // Create instance kinematics scene + _scene.kiscene = daeSafeCast(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE )); + _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() ); + + // Create instance physics scene + _scene.piscene = daeSafeCast(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE )); + _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() ); + } + + void _WritePhysics() { + domPhysics_scene::domTechnique_commonRef common = daeSafeCast(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + // Create gravity + domTargetable_float3Ref g = daeSafeCast(common->add(COLLADA_ELEMENT_GRAVITY)); + g->getValue().set3 (0,0,0); + } + + /// \brief Write kinematic body in a given scene + void _WriteRobot(int id = 0) + { + ROS_DEBUG_STREAM(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%id%_robot.getName())); + string asid = str(boost::format("robot%d")%id); + string askid = str(boost::format("%s_kinematics")%asid); + string asmid = str(boost::format("%s_motion")%asid); + string iassid = str(boost::format("%s_inst")%asmid); + + domInstance_articulated_systemRef ias = daeSafeCast(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM)); + ias->setSid(iassid.c_str()); + ias->setUrl((string("#")+asmid).c_str()); + ias->setName(_robot.getName().c_str()); + + _iasout.reset(new instance_articulated_system_output()); + _iasout->ias = ias; + + // motion info + domArticulated_systemRef articulated_system_motion = daeSafeCast(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM)); + articulated_system_motion->setId(asmid.c_str()); + domMotionRef motion = daeSafeCast(articulated_system_motion->add(COLLADA_ELEMENT_MOTION)); + domMotion_techniqueRef mt = daeSafeCast(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + domInstance_articulated_systemRef ias_motion = daeSafeCast(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM)); + ias_motion->setUrl(str(boost::format("#%s")%askid).c_str()); + + // kinematics info + domArticulated_systemRef articulated_system_kinematics = daeSafeCast(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM)); + articulated_system_kinematics->setId(askid.c_str()); + domKinematicsRef kinematics = daeSafeCast(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS)); + domKinematics_techniqueRef kt = daeSafeCast(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + + _WriteInstance_kinematics_model(kinematics,askid,id); + + for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { + string axis_infosid = str(boost::format("axis_info_inst%d")%idof); + boost::shared_ptr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint; + BOOST_ASSERT(_mapjointindices[pjoint] == idof); + //int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis; + + // Kinematics axis info + domKinematics_axis_infoRef kai = daeSafeCast(kt->add(COLLADA_ELEMENT_AXIS_INFO)); + kai->setAxis(str(boost::format("%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str()); + kai->setSid(axis_infosid.c_str()); + bool bactive = !pjoint->mimic; + double flower=0, fupper=0; + if( pjoint->type != urdf::Joint::CONTINUOUS ) { + if( !!pjoint->limits ) { + flower = pjoint->limits->lower; + fupper = pjoint->limits->upper; + } + if( flower == fupper ) { + bactive = false; + } + double fmult = 1.0; + if( pjoint->type != urdf::Joint::PRISMATIC ) { + fmult = 180.0/M_PI; + } + domKinematics_limitsRef plimits = daeSafeCast(kai->add(COLLADA_ELEMENT_LIMITS)); + daeSafeCast(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult); + daeSafeCast(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult); + } + + domCommon_bool_or_paramRef active = daeSafeCast(kai->add(COLLADA_ELEMENT_ACTIVE)); + daeSafeCast(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive); + domCommon_bool_or_paramRef locked = daeSafeCast(kai->add(COLLADA_ELEMENT_LOCKED)); + daeSafeCast(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false); + + // Motion axis info + domMotion_axis_infoRef mai = daeSafeCast(mt->add(COLLADA_ELEMENT_AXIS_INFO)); + mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str()); + if( !!pjoint->limits ) { + domCommon_float_or_paramRef speed = daeSafeCast(mai->add(COLLADA_ELEMENT_SPEED)); + daeSafeCast(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity); + domCommon_float_or_paramRef accel = daeSafeCast(mai->add(COLLADA_ELEMENT_ACCELERATION)); + daeSafeCast(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort); + } + } + + // write the bindings + string asmsym = str(boost::format("%s.%s")%asmid%_ikmout->ikm->getSid()); + string assym = str(boost::format("%s.%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()); + FOREACH(it, _ikmout->vkinematicsbindings) { + domKinematics_bindRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_BIND)); + abm->setSymbol(asmsym.c_str()); + daeSafeCast(abm->add(COLLADA_ELEMENT_PARAM))->setRef(it->first.c_str()); + domKinematics_bindRef ab = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); + ab->setSymbol(assym.c_str()); + daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(asmsym.c_str()); + _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second)); + } + for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { + const axis_sids& kas = _ikmout->vaxissids.at(idof); + domKinematics_bindRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_BIND)); + abm->setSymbol(str(boost::format("%s.%s")%asmid%kas.axissid).c_str()); + daeSafeCast(abm->add(COLLADA_ELEMENT_PARAM))->setRef(kas.axissid.c_str()); + domKinematics_bindRef ab = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); + ab->setSymbol(str(boost::format("%s.%s")%assym%kas.axissid).c_str()); + daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s.%s")%asmid%kas.axissid).c_str()); + string valuesid; + if( kas.valuesid.size() > 0 ) { + domKinematics_bindRef abmvalue = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_BIND)); + abmvalue->setSymbol(str(boost::format("%s.%s")%asmid%kas.valuesid).c_str()); + daeSafeCast(abmvalue->add(COLLADA_ELEMENT_PARAM))->setRef(kas.valuesid.c_str()); + domKinematics_bindRef abvalue = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); + valuesid = str(boost::format("%s.%s")%assym%kas.valuesid); + abvalue->setSymbol(valuesid.c_str()); + daeSafeCast(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s.%s")%asmid%kas.valuesid).c_str()); + } + _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid)); + } + } + + /// \brief Write kinematic body in a given scene + virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string& sidscope, int id) + { + ROS_DEBUG_STREAM(str(boost::format("writing instance_kinematics_model %s\n")%_robot.getName())); + boost::shared_ptr kmout = WriteKinematics_model(id); + + _ikmout.reset(new instance_kinematics_model_output()); + _ikmout->kmout = kmout; + _ikmout->ikm = daeSafeCast(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL)); + + string symscope, refscope; + if( sidscope.size() > 0 ) { + symscope = sidscope+string("."); + refscope = sidscope+string("/"); + } + string ikmsid = str(boost::format("%s_inst")%kmout->kmodel->getID()); + _ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str()); + _ikmout->ikm->setSid(ikmsid.c_str()); + + domKinematics_bindRef kbind = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_BIND)); + kbind->setSymbol((symscope+ikmsid).c_str()); + daeSafeCast(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str()); + _ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSymbol()), str(boost::format("visual%d/node0")%id))); + + _ikmout->vaxissids.reserve(kmout->vaxissids.size()); + int i = 0; + FOREACH(it,kmout->vaxissids) { + domKinematics_bindRef kbind = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_BIND)); + string ref = it->sid; + size_t index = ref.find("/"); + while(index != string::npos) { + ref[index] = '.'; + index = ref.find("/",index+1); + } + string sid = symscope+ikmsid+"."+ref; + kbind->setSymbol(sid.c_str()); + daeSafeCast(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str()); + double value=0; + double flower=0, fupper=0; + if( !!it->pjoint->limits ) { + flower = it->pjoint->limits->lower; + fupper = it->pjoint->limits->upper; + if( flower > 0 || fupper < 0 ) { + value = 0.5*(flower+fupper); + } + } + + domKinematics_newparamRef pvalueparam = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM)); + pvalueparam->setSid((sid+string("_value")).c_str()); + daeSafeCast(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value); + _ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid)); + ++i; + } + } + + virtual boost::shared_ptr WriteKinematics_model(int id) + { + domKinematics_modelRef kmodel = daeSafeCast(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL)); + string kmodelid = str(boost::format("kmodel%d")%id); + kmodel->setId(kmodelid.c_str()); + kmodel->setName(_robot.getName().c_str()); + + domKinematics_model_techniqueRef ktec = daeSafeCast(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + + // Create root node for the visual scene + domNodeRef pnoderoot = daeSafeCast(_scene.vscene->add(COLLADA_ELEMENT_NODE)); + string bodyid = str(boost::format("visual%d")%id); + pnoderoot->setId(bodyid.c_str()); + pnoderoot->setSid(bodyid.c_str()); + pnoderoot->setName(_robot.getName().c_str()); + + // Declare all the joints + _mapjointindices.clear(); + int index=0; + FOREACHC(itj, _robot.joints_) { + _mapjointindices[itj->second] = index++; + } + _maplinkindices.clear(); + index=0; + FOREACHC(itj, _robot.links_) { + _maplinkindices[itj->second] = index++; + } + _mapmaterialindices.clear(); + index=0; + FOREACHC(itj, _robot.materials_) { + _mapmaterialindices[itj->second] = index++; + } + + double lmin, lmax; + vector vdomjoints(_robot.joints_.size()); + boost::shared_ptr kmout(new kinematics_model_output()); + kmout->kmodel = kmodel; + kmout->vaxissids.resize(_robot.joints_.size()); + kmout->vlinksids.resize(_robot.links_.size()); + + FOREACHC(itjoint, _robot.joints_) { + boost::shared_ptr pjoint = itjoint->second; + int index = _mapjointindices[itjoint->second]; + domJointRef pdomjoint = daeSafeCast(ktec->add(COLLADA_ELEMENT_JOINT)); + string jointid = str(boost::format("joint%d")%index); + pdomjoint->setSid( jointid.c_str() ); + pdomjoint->setName(pjoint->name.c_str()); + domAxis_constraintRef axis; + if( !!pjoint->limits ) { + lmin=pjoint->limits->lower; + lmax=pjoint->limits->upper; + } + else { + lmin = lmax = 0; + } + + double fmult = 1.0; + switch(pjoint->type) { + case urdf::Joint::REVOLUTE: + case urdf::Joint::CONTINUOUS: + axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE)); + fmult = 180.0f/M_PI; + lmin*=fmult; + lmax*=fmult; + break; + case urdf::Joint::PRISMATIC: + axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC)); + break; + case urdf::Joint::FIXED: + axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE)); + lmin = 0; + lmax = 0; + fmult = 0; + break; + default: + ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type)); + break; + } + + if( !axis ) { + continue; + } + + int ia = 0; + string axisid = str(boost::format("axis%d")%ia); + axis->setSid(axisid.c_str()); + kmout->vaxissids.at(index).pjoint = pjoint; + kmout->vaxissids.at(index).sid = jointid+string("/")+axisid; + kmout->vaxissids.at(index).iaxis = ia; + domAxisRef paxis = daeSafeCast(axis->add(COLLADA_ELEMENT_AXIS)); + paxis->getValue().setCount(3); + paxis->getValue()[0] = pjoint->axis.x; + paxis->getValue()[1] = pjoint->axis.y; + paxis->getValue()[2] = pjoint->axis.z; + if( pjoint->type != urdf::Joint::CONTINUOUS ) { + domJoint_limitsRef plimits = daeSafeCast(axis->add(COLLADA_TYPE_LIMITS)); + daeSafeCast(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin; + daeSafeCast(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax; + } + vdomjoints.at(index) = pdomjoint; + } + + LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID()); + FOREACHC(itused, childinfo.listusedlinks) { + kmout->vlinksids.at(itused->first) = itused->second; + } + FOREACH(itprocessed,childinfo.listprocesseddofs) { + kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second; + } + + // create the formulas for all mimic joints + FOREACHC(itjoint, _robot.joints_) { + int index = _mapjointindices[itjoint->second]; + boost::shared_ptr pjoint = itjoint->second; + if( !pjoint->mimic ) { + continue; + } + + domFormulaRef pf = daeSafeCast(ktec->add(COLLADA_ELEMENT_FORMULA)); + string formulaid = str(boost::format("joint%d.formula")%index); + pf->setSid(formulaid.c_str()); + domCommon_float_or_paramRef ptarget = daeSafeCast(pf->add(COLLADA_ELEMENT_TARGET)); + string targetjointid = str(boost::format("%s/joint%d")%kmodel->getID()%index); + daeSafeCast(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str()); + + domFormula_techniqueRef pftec = daeSafeCast(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + // create a const0*joint+const1 formula + // a x b + daeElementRef pmath_math = pftec->add("math"); + daeElementRef pmath_apply = pmath_math->add("apply"); + { + daeElementRef pmath_plus = pmath_apply->add("plus"); + daeElementRef pmath_apply1 = pmath_apply->add("apply"); + { + daeElementRef pmath_times = pmath_apply1->add("times"); + daeElementRef pmath_const0 = pmath_apply1->add("cn"); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); + daeElementRef pmath_symb = pmath_apply1->add("csymbol"); + pmath_symb->setAttribute("encoding","COLLADA"); + pmath_symb->setCharData(str(boost::format("%s/joint%d")%kmodel->getID()%_mapjointindices[_robot.getJoint(pjoint->mimic->joint_name)])); + } + daeElementRef pmath_const1 = pmath_apply->add("cn"); + pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); + } + } + + return kmout; + } + + /// \brief Write link of a kinematic body + /// \param link Link to write + /// \param pkinparent Kinbody parent + /// \param pnodeparent Node parent + /// \param strModelUri + virtual LINKOUTPUT _WriteLink(boost::shared_ptr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri) + { + LINKOUTPUT out; + int linkindex = _maplinkindices[plink]; + string linksid = str(boost::format("link%d")%linkindex); + domLinkRef pdomlink = daeSafeCast(pkinparent->add(COLLADA_ELEMENT_LINK)); + pdomlink->setName(plink->name.c_str()); + pdomlink->setSid(linksid.c_str()); + + domNodeRef pnode = daeSafeCast(pnodeparent->add(COLLADA_ELEMENT_NODE)); + string nodeid = str(boost::format("v%s.node%d")%strModelUri%linkindex); + pnode->setId( nodeid.c_str() ); + string nodesid = str(boost::format("node%d")%linkindex); + pnode->setSid(nodesid.c_str()); + pnode->setName(plink->name.c_str()); + + boost::shared_ptr geometry; + boost::shared_ptr material; + urdf::Pose geometry_origin; + if( !!plink->visual ) { + geometry = plink->visual->geometry; + material = plink->visual->material; + geometry_origin = plink->visual->origin; + } + else if( !!plink->collision ) { + geometry = plink->collision->geometry; + geometry_origin = plink->collision->origin; + } + + if( !!geometry ) { + int igeom = 0; + string geomid = str(boost::format("g%s.%s.geom%d")%strModelUri%linksid%igeom); + domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); + domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); + pinstgeom->setUrl((string("#")+geomid).c_str()); + + // material + _WriteMaterial(pdomgeom->getID(), material); + domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); + domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); + pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string(".mat"))); + pinstmat->setSymbol("mat0"); + } + + _WriteTransformation(pnode, geometry_origin); + urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin); + + // process all children + FOREACHC(itjoint, plink->child_joints) { + boost::shared_ptr pjoint = *itjoint; + int index = _mapjointindices[pjoint]; + + // + domLink::domAttachment_fullRef attachment_full = daeSafeCast(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL)); + string jointid = str(boost::format("%s/joint%d")%strModelUri%index); + attachment_full->setJoint(jointid.c_str()); + + LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri); + FOREACH(itused,childinfo.listusedlinks) { + out.listusedlinks.push_back(make_pair(itused->first,linksid+string("/")+itused->second)); + } + FOREACH(itprocessed,childinfo.listprocesseddofs) { + out.listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+string("/")+itprocessed->second)); + } + + // rotate/translate elements + string jointnodesid = str(boost::format("node_joint%d_axis0")%index); + switch(pjoint->type) { + case urdf::Joint::REVOLUTE: + case urdf::Joint::CONTINUOUS: + case urdf::Joint::FIXED: { + domRotateRef protate = daeSafeCast(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0)); + protate->setSid(jointnodesid.c_str()); + protate->getValue().setCount(4); + protate->getValue()[0] = pjoint->axis.x; + protate->getValue()[1] = pjoint->axis.y; + protate->getValue()[2] = pjoint->axis.z; + protate->getValue()[3] = 0; + break; + } + case urdf::Joint::PRISMATIC: { + domTranslateRef ptrans = daeSafeCast(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0)); + ptrans->setSid(jointnodesid.c_str()); + ptrans->getValue().setCount(3); + ptrans->getValue()[0] = 0; + ptrans->getValue()[1] = 0; + ptrans->getValue()[2] = 0; + break; + } + default: + ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type)); + break; + } + + _WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform); + _WriteTransformation(childinfo.pnode, pjoint->parent_to_joint_origin_transform); + _WriteTransformation(childinfo.pnode, geometry_origin_inv); // have to do multiply by inverse since geometry transformation is not part of hierarchy + out.listprocesseddofs.push_back(make_pair(index,string(childinfo.pnode->getSid())+string("/")+jointnodesid)); + // + } + + out.listusedlinks.push_back(make_pair(linkindex,linksid)); + out.plink = pdomlink; + out.pnode = pnode; + return out; + } + + domGeometryRef _WriteGeometry(boost::shared_ptr geometry, const std::string& geometry_id) + { + domGeometryRef cgeometry = daeSafeCast(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY)); + cgeometry->setId(geometry_id.c_str()); + switch (geometry->type) { + case urdf::Geometry::MESH: { + urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get(); + _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale); + break; + } + case urdf::Geometry::SPHERE: { + ROS_WARN_STREAM(str(boost::format("cannot export sphere geometries yet! %s")%geometry_id)); + break; + } + case urdf::Geometry::BOX: { + ROS_WARN_STREAM(str(boost::format("cannot export box geometries yet! %s")%geometry_id)); + break; + } + case urdf::Geometry::CYLINDER: { + ROS_WARN_STREAM(str(boost::format("cannot export cylinder geometries yet! %s")%geometry_id)); + break; + } + default: { + throw ColladaUrdfException(str(boost::format("undefined geometry type %d, name %s")%(int)geometry->type%geometry_id)); + } + } + return cgeometry; + } + + void _WriteMaterial(const string& geometry_id, boost::shared_ptr material) + { + string effid = geometry_id+string(".eff"); + string matid = geometry_id+string(".mat"); + domMaterialRef pdommat = daeSafeCast(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); + pdommat->setId(matid.c_str()); + domInstance_effectRef pdominsteff = daeSafeCast(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT)); + pdominsteff->setUrl((string("#")+effid).c_str()); + + urdf::Color ambient, diffuse; + ambient.init("0.1 0.1 0.1 0"); + diffuse.init("1 1 1 0"); + + if( !!material ) { + ambient.r = diffuse.r = material->color.r; + ambient.g = diffuse.g = material->color.g; + ambient.b = diffuse.b = material->color.b; + ambient.a = diffuse.a = material->color.a; + } + + domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse); + + // + domMaterialRef dommaterial = daeSafeCast(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); + string material_id = geometry_id + string(".mat"); + dommaterial->setId(material_id.c_str()); + { + // + domInstance_effectRef instance_effect = daeSafeCast(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT)); + string effect_id(effect->getId()); + instance_effect->setUrl((string("#") + effect_id).c_str()); + } + // + + domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse); + } + + void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale) + { + const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate);//|aiProcess_GenNormals|aiProcess_GenUVCoords|aiProcess_FlipUVs); + if( !scene ) { + ROS_WARN("failed to load resource %s",filename.c_str()); + return; + } + if( !scene->mRootNode ) { + ROS_WARN("resource %s has no data",filename.c_str()); + return; + } + if (!scene->HasMeshes()) { + ROS_WARN_STREAM(str(boost::format("No meshes found in file %s")%filename)); + return; + } + domMeshRef pdommesh = daeSafeCast(pdomgeom->add(COLLADA_ELEMENT_MESH)); + domSourceRef pvertsource = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_SOURCE)); + domAccessorRef pacc; + domFloat_arrayRef parray; + { + pvertsource->setId(str(boost::format("%s.positions")%pdomgeom->getID()).c_str()); + + parray = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); + parray->setId(str(boost::format("%s.positions-array")%pdomgeom->getID()).c_str()); + parray->setDigits(6); // 6 decimal places + + domSource::domTechnique_commonRef psourcetec = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + pacc = daeSafeCast(psourcetec->add(COLLADA_ELEMENT_ACCESSOR)); + pacc->setSource(xsAnyURI(*parray, string("#")+string(parray->getID()))); + pacc->setStride(3); + + domParamRef px = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + px->setName("X"); px->setType("float"); + domParamRef py = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + py->setName("Y"); py->setType("float"); + domParamRef pz = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + pz->setName("Z"); pz->setType("float"); + } + domVerticesRef pverts = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_VERTICES)); + { + pverts->setId("vertices"); + domInput_localRef pvertinput = daeSafeCast(pverts->add(COLLADA_ELEMENT_INPUT)); + pvertinput->setSemantic("POSITION"); + pvertinput->setSource(domUrifragment(*pvertsource, string("#")+string(pvertsource->getID()))); + } + _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale); + pacc->setCount(parray->getCount()); + } + + void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale) + { + if( !node ) { + 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; + } + + { + uint32_t vertexOffset = parray->getCount(); + uint32_t nTotalVertices=0; + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + nTotalVertices += input_mesh->mNumVertices; + } + + parray->getValue().grow(parray->getCount()+nTotalVertices*3); + parray->setCount(parray->getCount()+nTotalVertices); + + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) { + aiVector3D p = input_mesh->mVertices[j]; + p *= transform; + parray->getValue().append(p.x*scale.x); + parray->getValue().append(p.y*scale.y); + parray->getValue().append(p.z*scale.z); + } + } + + // in order to save space, separate triangles from poly lists + + vector triangleindices, otherindices; + int nNumOtherPrimitives = 0; + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + uint32_t indexCount = 0, otherIndexCount = 0; + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { + aiFace& face = input_mesh->mFaces[j]; + if( face.mNumIndices == 3 ) { + indexCount += face.mNumIndices; + } + else { + otherIndexCount += face.mNumIndices; + } + } + triangleindices.reserve(triangleindices.size()+indexCount); + otherindices.reserve(otherindices.size()+otherIndexCount); + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { + aiFace& face = input_mesh->mFaces[j]; + if( face.mNumIndices == 3 ) { + triangleindices.push_back(vertexOffset+face.mIndices[0]); + triangleindices.push_back(vertexOffset+face.mIndices[1]); + triangleindices.push_back(vertexOffset+face.mIndices[2]); + } + else { + for (uint32_t k = 0; k < face.mNumIndices; ++k) { + otherindices.push_back(face.mIndices[k]+vertexOffset); + } + nNumOtherPrimitives++; + } + } + vertexOffset += input_mesh->mNumVertices; + } + + if( triangleindices.size() > 0 ) { + domTrianglesRef ptris = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_TRIANGLES)); + ptris->setCount(triangleindices.size()/3); + ptris->setMaterial("mat0"); + domInput_local_offsetRef pvertoffset = daeSafeCast(ptris->add(COLLADA_ELEMENT_INPUT)); + pvertoffset->setSemantic("VERTEX"); + pvertoffset->setOffset(0); + pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid))); + domPRef pindices = daeSafeCast(ptris->add(COLLADA_ELEMENT_P)); + pindices->getValue().setCount(triangleindices.size()); + for(size_t ind = 0; ind < triangleindices.size(); ++ind) { + pindices->getValue()[ind] = triangleindices[ind]; + } + } + + if( nNumOtherPrimitives > 0 ) { + domPolylistRef ptris = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_POLYLIST)); + ptris->setCount(nNumOtherPrimitives); + ptris->setMaterial("mat0"); + domInput_local_offsetRef pvertoffset = daeSafeCast(ptris->add(COLLADA_ELEMENT_INPUT)); + pvertoffset->setSemantic("VERTEX"); + pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid))); + domPRef pindices = daeSafeCast(ptris->add(COLLADA_ELEMENT_P)); + pindices->getValue().setCount(otherindices.size()); + for(size_t ind = 0; ind < otherindices.size(); ++ind) { + pindices->getValue()[ind] = otherindices[ind]; + } + + domPolylist::domVcountRef pcount = daeSafeCast(ptris->add(COLLADA_ELEMENT_VCOUNT)); + pcount->getValue().setCount(nNumOtherPrimitives); + uint32_t offset = 0; + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { + aiFace& face = input_mesh->mFaces[j]; + if( face.mNumIndices > 3 ) { + pcount->getValue()[offset++] = face.mNumIndices; + } + } + } + } + } + + for (uint32_t i=0; i < node->mNumChildren; ++i) { + _buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale); + } + } + + + domEffectRef _WriteEffect(std::string const& effect_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse) + { + // + domEffectRef effect = daeSafeCast(_effectsLib->add(COLLADA_ELEMENT_EFFECT)); + effect->setId(effect_id.c_str()); + { + // + domProfile_commonRef profile = daeSafeCast(effect->add(COLLADA_ELEMENT_PROFILE_COMMON)); + { + // + domProfile_common::domTechniqueRef technique = daeSafeCast(profile->add(COLLADA_ELEMENT_TECHNIQUE)); + { + // + domProfile_common::domTechnique::domPhongRef phong = daeSafeCast(technique->add(COLLADA_ELEMENT_PHONG)); + { + // + domFx_common_color_or_textureRef ambient = daeSafeCast(phong->add(COLLADA_ELEMENT_AMBIENT)); + { + // r g b a + domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast(ambient->add(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->add(COLLADA_ELEMENT_DIFFUSE)); + { + // r g b a + domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast(diffuse->add(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; + } + + /// \brief Write transformation + /// \param pelt Element to transform + /// \param t Transform to write + void _WriteTransformation(daeElementRef pelt, const urdf::Pose& t) + { + domRotateRef prot = daeSafeCast(pelt->add(COLLADA_ELEMENT_ROTATE,0)); + domTranslateRef ptrans = daeSafeCast(pelt->add(COLLADA_ELEMENT_TRANSLATE,0)); + ptrans->getValue().setCount(3); + ptrans->getValue()[0] = t.position.x; + ptrans->getValue()[1] = t.position.y; + ptrans->getValue()[2] = t.position.z; + + prot->getValue().setCount(4); + // extract axis from quaternion + double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z; + if( std::fabs(sinang) < 1e-10 ) { + prot->getValue()[0] = 1; + prot->getValue()[1] = 0; + prot->getValue()[2] = 0; + prot->getValue()[3] = 0; + } + else { + urdf::Rotation quat; + if( t.rotation.w < 0 ) { + quat.x = -t.rotation.x; + quat.y = -t.rotation.y; + quat.z = -t.rotation.z; + quat.w = -t.rotation.w; + } + else { + quat = t.rotation; + } + sinang = std::sqrt(sinang); + prot->getValue()[0] = quat.x/sinang; + prot->getValue()[1] = quat.y/sinang; + prot->getValue()[2] = quat.z/sinang; + prot->getValue()[3] = angles::to_degrees(2.0*std::atan2(sinang,quat.w)); + } + } + + // binding in instance_kinematics_scene + void _WriteBindingsInstance_kinematics_scene() + { + FOREACHC(it, _iasout->vkinematicsbindings) { + domBind_kinematics_modelRef pmodelbind = daeSafeCast(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); + pmodelbind->setNode(it->second.c_str()); + daeSafeCast(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str()); + } + FOREACHC(it, _iasout->vaxissids) { + domBind_joint_axisRef pjointbind = daeSafeCast(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS)); + pjointbind->setTarget(it->jointnodesid.c_str()); + daeSafeCast(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str()); + daeSafeCast(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str()); + } + } + +private: + static urdf::Vector3 _poseMult(const urdf::Pose& p, const urdf::Vector3& v) + { + double ww = 2 * p.rotation.x * p.rotation.x; + double wx = 2 * p.rotation.x * p.rotation.y; + double wy = 2 * p.rotation.x * p.rotation.z; + double wz = 2 * p.rotation.x * p.rotation.w; + double xx = 2 * p.rotation.y * p.rotation.y; + double xy = 2 * p.rotation.y * p.rotation.z; + double xz = 2 * p.rotation.y * p.rotation.w; + double yy = 2 * p.rotation.z * p.rotation.z; + double yz = 2 * p.rotation.z * p.rotation.w; + urdf::Vector3 vnew; + vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x; + vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y; + vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z; + return vnew; + } + + static urdf::Pose _poseInverse(const urdf::Pose& p) + { + urdf::Pose pinv; + pinv.rotation.x = -p.rotation.x; + pinv.rotation.y = -p.rotation.y; + pinv.rotation.z = -p.rotation.z; + pinv.rotation.w = p.rotation.w; + urdf::Vector3 t = _poseMult(pinv,p.position); + pinv.position.x = -t.x; + pinv.position.y = -t.y; + pinv.position.z = -t.z; + return pinv; + } + + int _writeoptions; + + const urdf::Model& _robot; + boost::shared_ptr _collada; + domCOLLADA* _dom; + domCOLLADA::domSceneRef _globalscene; + + domLibrary_visual_scenesRef _visualScenesLib; + domLibrary_kinematics_scenesRef _kinematicsScenesLib; + domLibrary_kinematics_modelsRef _kinematicsModelsLib; + domLibrary_articulated_systemsRef _articulatedSystemsLib; + domLibrary_physics_scenesRef _physicsScenesLib; + domLibrary_materialsRef _materialsLib; + domLibrary_effectsRef _effectsLib; + domLibrary_geometriesRef _geometriesLib; + domTechniqueRef _sensorsLib;///< custom sensors library + SCENE _scene; + + boost::shared_ptr _ikmout; + boost::shared_ptr _iasout; + std::map< boost::shared_ptr, int > _mapjointindices; + std::map< boost::shared_ptr, int > _maplinkindices; + std::map< boost::shared_ptr, int > _mapmaterialindices; + Assimp::Importer _importer; +}; + ColladaUrdfException::ColladaUrdfException(std::string const& what) : std::runtime_error(what) { } -bool colladaFromUrdfFile(string const& file, shared_ptr& dom) { - TiXmlDocument urdf_xml; - if (!urdf_xml.LoadFile(file)) { - ROS_ERROR("Could not load XML file"); - return false; - } - - return colladaFromUrdfXml(&urdf_xml, dom); -} - -bool colladaFromUrdfString(string const& xml, shared_ptr& dom) { - TiXmlDocument urdf_xml; - if (urdf_xml.Parse(xml.c_str()) == 0) { - ROS_ERROR("Could not parse XML document"); - return false; - } - - return colladaFromUrdfXml(&urdf_xml, dom); -} - -bool colladaFromUrdfXml(TiXmlDocument* xml_doc, shared_ptr& dom) { - urdf::Model robot_model; - if (!robot_model.initXml(xml_doc)) { - ROS_ERROR("Could not generate robot model"); - return false; - } - - return colladaFromUrdfModel(robot_model, dom); -} - -bool colladaFromUrdfModel(urdf::Model const& robot_model, shared_ptr& dom) { - ColladaWriter writer(robot_model); +bool colladaFromUrdfModel(const urdf::Model& robot_model, boost::shared_ptr& dom, int writeoptions) +{ + ColladaWriter writer(robot_model,writeoptions); dom = writer.convert(); - - return dom != shared_ptr(); + return dom != boost::shared_ptr(); } -bool colladaToFile(shared_ptr dom, string const& file) { +bool colladaToFile(boost::shared_ptr dom, string const& file) { daeString uri = dom->getDoc(0)->getDocumentURI()->getURI(); return dom->writeTo(uri, file); } diff --git a/collada_urdf/src/collada_writer.cpp b/collada_urdf/src/collada_writer.cpp deleted file mode 100644 index 7453fd1..0000000 --- a/collada_urdf/src/collada_writer.cpp +++ /dev/null @@ -1,975 +0,0 @@ -/********************************************************************* -* 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. -*********************************************************************/ - -/* Author: Tim Field */ - -#include "collada_urdf/collada_writer.h" - -#include "collada_urdf/stl_loader.h" - -#include -#include -#include - -#define foreach BOOST_FOREACH - -using std::string; -using std::map; -using std::vector; -using boost::shared_ptr; - -namespace collada_urdf { - -int ColladaWriter::s_doc_count_ = 0; - -ColladaWriter::ColladaWriter(urdf::Model const& robot) : robot_(robot), dom_(NULL) { - daeErrorHandler::setErrorHandler(this); - - collada_.reset(new DAE); - collada_->setIOPlugin(NULL); - collada_->setDatabase(NULL); -} - -ColladaWriter::~ColladaWriter() { } - -shared_ptr ColladaWriter::convert() { - try { - string doc_count_str = boost::lexical_cast(s_doc_count_++); - initDocument(string("collada_urdf_") + doc_count_str + string(".dae")); - - SCENE scene = createScene(); - - setupPhysics(scene); - addGeometries(); - addKinematics(scene); - addVisuals(scene); - addMaterials(); - addBindings(scene); - - return collada_; - } - catch (ColladaUrdfException ex) { - ROS_ERROR("Error converting: %s", ex.what()); - return shared_ptr(); - } -} - -// Implementation - -void ColladaWriter::handleError(daeString msg) { - throw ColladaUrdfException(msg); -} - -void ColladaWriter::handleWarning(daeString msg) { - std::cerr << "COLLADA DOM warning: " << msg << std::endl; -} - -void ColladaWriter::initDocument(string const& documentName) { - // Create the document - daeDocument* doc = NULL; - daeInt error = collada_->getDatabase()->insertDocument(documentName.c_str(), &doc); // also creates a collada root - if (error != DAE_OK || doc == NULL) - throw ColladaUrdfException("Failed to create document"); - - dom_ = daeSafeCast(doc->getDomRoot()); - dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); - - // Create asset elements - domAssetRef asset = daeSafeCast(dom_->add(COLLADA_ELEMENT_ASSET)); - { - string date = getTimeStampString(); - - domAsset::domCreatedRef created = daeSafeCast(asset->add(COLLADA_ELEMENT_CREATED)); - created->setValue(date.c_str()); - domAsset::domModifiedRef modified = daeSafeCast(asset->add(COLLADA_ELEMENT_MODIFIED)); - modified->setValue(date.c_str()); - - domAsset::domContributorRef contrib = daeSafeCast(asset->add(COLLADA_ELEMENT_CONTRIBUTOR)); - - domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast(contrib->add(COLLADA_ELEMENT_AUTHORING_TOOL)); - authoringtool->setValue("URDF Collada Writer"); - - domAsset::domUnitRef units = daeSafeCast(asset->add(COLLADA_ELEMENT_UNIT)); - units->setMeter(1); - units->setName("meter"); - - domAsset::domUp_axisRef zup = daeSafeCast(asset->add(COLLADA_ELEMENT_UP_AXIS)); - zup->setValue(UP_AXIS_Z_UP); - } - - // Create top-level elements - scene_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_SCENE)); - visualScenesLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); - visualScenesLib_->setId("vscenes"); - geometriesLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); - geometriesLib_->setId("geometries"); - kinematicsScenesLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); - kinematicsScenesLib_->setId("kscenes"); - kinematicsModelsLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); - kinematicsModelsLib_->setId("kmodels"); - jointsLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_JOINTS)); - jointsLib_->setId("joints"); - physicsScenesLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); - physicsScenesLib_->setId("physics_scenes"); - effectsLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_EFFECTS)); - effectsLib_->setId("effects"); - materialsLib_ = daeSafeCast(dom_->add(COLLADA_ELEMENT_LIBRARY_MATERIALS)); - materialsLib_->setId("materials"); -} - -ColladaWriter::SCENE ColladaWriter::createScene() { - SCENE s; - - // Create visual scene - s.vscene = daeSafeCast(visualScenesLib_->add(COLLADA_ELEMENT_VISUAL_SCENE)); - s.vscene->setId("vscene"); - s.vscene->setName("URDF Visual Scene"); - - // Create instance visual scene - s.viscene = daeSafeCast(scene_->add(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE)); - s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str()); - - // Create kinematics scene - s.kscene = daeSafeCast(kinematicsScenesLib_->add(COLLADA_ELEMENT_KINEMATICS_SCENE)); - s.kscene->setId("kscene"); - s.kscene->setName("URDF Kinematics Scene"); - - // Create instance kinematics scene - s.kiscene = daeSafeCast(scene_->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE)); - s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str()); - - // Create physics scene - s.pscene = daeSafeCast(physicsScenesLib_->add(COLLADA_ELEMENT_PHYSICS_SCENE)); - s.pscene->setId("pscene"); - s.pscene->setName("URDF Physics Scene"); - - // Create instance physics scene - s.piscene = daeSafeCast(scene_->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE)); - s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str()); - - return s; -} - -void ColladaWriter::setupPhysics(SCENE const& scene) { - // - domPhysics_scene::domTechnique_commonRef common = daeSafeCast(scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - { - // 0 0 0 - domTargetable_float3Ref g = daeSafeCast(common->add(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; - boost::shared_ptr< urdf::Geometry > geometry; - if( !!urdf_link->visual ) { - geometry = urdf_link->visual->geometry; - } - else if (!!urdf_link->collision ) { - geometry = urdf_link->collision->geometry; - } - if( !geometry ) { - continue; - } - switch (geometry->type) { - case urdf::Geometry::MESH: { - urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get(); - - string filename = urdf_mesh->filename; - urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale - - // - domGeometryRef geometry = daeSafeCast(geometriesLib_->add(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 << " not exported" << std::endl; - break; - } - case urdf::Geometry::BOX: { - std::cerr << "Warning: geometry type BOX of link " << urdf_link->name << " not exported" << std::endl; - break; - } - case urdf::Geometry::CYLINDER: { - std::cerr << "Warning: geometry type CYLINDER of link " << urdf_link->name << " not exported" << std::endl; - break; - } - default: { - std::cerr << "Warning: geometry type " << geometry->type << " of link " << urdf_link->name << " not exported" << 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 - try { - loadMeshWithSTLLoader(resource, geometry, geometry_id); - } - catch (ColladaUrdfException e) { - std::cerr << "Unable to load mesh file " << filename << ": " << e.what() << 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); - if (fd == -1) - throw ColladaUrdfException("Couldn't create temporary file"); - - if ((uint32_t) write(fd, resource.data.get(), resource.size) != resource.size) { - close(fd); - unlink(tmp_filename); - throw ColladaUrdfException("Couldn't write resource to file"); - } - close(fd); - - // Import the mesh using STLLoader - STLLoader loader; - shared_ptr stl_mesh = loader.load(string(tmp_filename)); - if (stl_mesh == shared_ptr()) { - unlink(tmp_filename); - throw ColladaUrdfException("Couldn't import STL mesh"); - } - - // Build the COLLADA mesh - buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id); - - // Delete the temporary file - unlink(tmp_filename); - - return true; -} - -void ColladaWriter::buildMeshFromSTLLoader(shared_ptr stl_mesh, daeElementRef parent, string const& geometry_id) { - // - domMeshRef mesh = daeSafeCast(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; - - // - domSourceRef positions_source = daeSafeCast(mesh->add(COLLADA_ELEMENT_SOURCE)); - positions_source->setId((geometry_id + string(".positions")).c_str()); - { - // - domFloat_arrayRef positions_array = daeSafeCast(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 - 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->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - { - // - domAccessorRef accessor = daeSafeCast(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); - { - // - // - // - domParamRef px = daeSafeCast(accessor->add(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float"); - domParamRef py = daeSafeCast(accessor->add(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float"); - domParamRef pz = daeSafeCast(accessor->add(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float"); - } - // - } - // - } - - // - domVerticesRef vertices = daeSafeCast(mesh->add(COLLADA_ELEMENT_VERTICES)); - string vertices_id = geometry_id + string(".vertices"); - vertices->setId(vertices_id.c_str()); - { - // - domInput_localRef vertices_input = daeSafeCast(vertices->add(COLLADA_ELEMENT_INPUT)); - vertices_input->setSemantic("POSITION"); - vertices_input->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId()))); - } - // - - // - domTrianglesRef triangles = daeSafeCast(mesh->add(COLLADA_ELEMENT_TRIANGLES)); - triangles->setCount(num_faces); - triangles->setMaterial("mat0"); - { - // - domInput_local_offsetRef vertex_offset = daeSafeCast(triangles->add(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->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]; - //

- } - } - // - } - // -} - -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->add(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; - } - - switch (urdf_joint->type) - { - case urdf::Joint::REVOLUTE: { - // - domAxis_constraintRef revolute = daeSafeCast(joint->add(COLLADA_ELEMENT_REVOLUTE)); - revolute->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(revolute->add(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->add(COLLADA_ELEMENT_LIMITS)); - { - daeSafeCast(limits->add(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower); - daeSafeCast(limits->add(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->add(COLLADA_ELEMENT_REVOLUTE)); - revolute->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(revolute->add(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->add(COLLADA_ELEMENT_PRISMATIC)); - prismatic->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(prismatic->add(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->add(COLLADA_ELEMENT_LIMITS)); - { - daeSafeCast(limits->add(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower; - daeSafeCast(limits->add(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->add(COLLADA_ELEMENT_REVOLUTE)); - revolute->setSid("axis0"); - { - // - domAxisRef axis = daeSafeCast(revolute->add(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->add(COLLADA_ELEMENT_LIMITS)); - { - daeSafeCast(limits->add(COLLADA_ELEMENT_MIN))->getValue() = 0.0; - daeSafeCast(limits->add(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 const& scene) { - string model_id = kmodel_->getID(); - string inst_model_sid = string("inst_") + model_id; - - // - // - domBind_kinematics_modelRef kmodel_bind = daeSafeCast(scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); - kmodel_bind->setNode("v1.node0"); // @todo - daeSafeCast(kmodel_bind->add(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->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()); - { - // - domCommon_sidref_or_paramRef axis_bind = daeSafeCast(joint_bind->add(COLLADA_ELEMENT_AXIS)); - { - daeSafeCast(axis_bind->add(COLLADA_ELEMENT_PARAM))->setValue(joint_axis_sid.c_str()); - } - // - // - domCommon_float_or_paramRef value_bind = daeSafeCast(joint_bind->add(COLLADA_ELEMENT_VALUE)); - { - daeSafeCast(value_bind->add(COLLADA_ELEMENT_PARAM))->setValue(joint_axis_value_sid.c_str()); - } - } - // - } -} - -void ColladaWriter::addKinematics(SCENE const& scene) { - // - domKinematics_modelRef kmodel = daeSafeCast(kinematicsModelsLib_->add(COLLADA_ELEMENT_KINEMATICS_MODEL)); - kmodel->setId("k1"); - kmodel->setName(robot_.getName().c_str()); - { - // - domKinematics_model_techniqueRef technique = daeSafeCast(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - addJoints(technique); - // - - // - int link_num = 0; - addKinematicLink(robot_.getRoot(), technique, link_num); - // - - // add a formula for each mimic joint - for (map >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) { - shared_ptr urdf_joint = i->second; - if( !!urdf_joint->mimic ) { - string joint_sid = string("k1/")+joint_sids_[urdf_joint->name]; - string joint_mimic_sid = string("k1/")+joint_sids_[urdf_joint->mimic->joint_name]; - // - addMimicJoint(daeSafeCast(technique->add(COLLADA_ELEMENT_FORMULA)), joint_sid,joint_mimic_sid,urdf_joint->mimic->multiplier,urdf_joint->mimic->offset); - // - } - } - } - kmodel_ = kmodel; - // - - string model_id = kmodel->getID(); - string inst_model_sid = string("inst_") + model_id; - - // - domInstance_kinematics_modelRef ikm = daeSafeCast(scene.kscene->add(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->add(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->add(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->add(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->add(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str()); - } - // - - // - domKinematics_newparamRef newparam_value = daeSafeCast(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()); - { - // 0 - daeSafeCast(newparam_value->add(COLLADA_ELEMENT_FLOAT))->setValue(0.0f); - } - // - } - } - // -} - -void ColladaWriter::addKinematicLink(shared_ptr urdf_link, daeElementRef parent, int& link_num) { - // - domLinkRef link = daeSafeCast(parent->add(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->add(COLLADA_ELEMENT_ATTACHMENT_FULL)); - string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name]; - attachment_full->setJoint(attachment_joint.c_str()); - { - // x y z - addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position, NULL, true); - // x y z w - addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation, NULL, true); - - addKinematicLink(robot_.getLink(urdf_joint->child_link_name), attachment_full, link_num); - } - // - } - // -} - -void ColladaWriter::addVisuals(SCENE const& scene) { - // - domNodeRef root_node = daeSafeCast(scene.vscene->add(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; - - if (!urdf_link->visual->material->texture_filename.empty()) { - ambient.r = ambient.g = ambient.b = 1; ambient.a = 0; - diffuse.r = diffuse.g = diffuse.b = 1; diffuse.a = 0; - } - else { - ambient.r = diffuse.r = urdf_link->visual->material->color.r; - ambient.g = diffuse.g = urdf_link->visual->material->color.g; - ambient.b = diffuse.b = urdf_link->visual->material->color.b; - ambient.a = diffuse.a = urdf_link->visual->material->color.a; - } - - string geometry_id = j->second; - - domEffectRef effect = addEffect(geometry_id, ambient, diffuse); - - // - domMaterialRef material = daeSafeCast(materialsLib_->add(COLLADA_ELEMENT_MATERIAL)); - string material_id = geometry_id + string(".mat"); - material->setId(material_id.c_str()); - { - // - domInstance_effectRef instance_effect = daeSafeCast(material->add(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_->add(COLLADA_ELEMENT_EFFECT)); - string effect_id = geometry_id + string(".eff"); - effect->setId(effect_id.c_str()); - { - // - domProfile_commonRef profile = daeSafeCast(effect->add(COLLADA_ELEMENT_PROFILE_COMMON)); - { - // - domProfile_common::domTechniqueRef technique = daeSafeCast(profile->add(COLLADA_ELEMENT_TECHNIQUE)); - { - // - domProfile_common::domTechnique::domPhongRef phong = daeSafeCast(technique->add(COLLADA_ELEMENT_PHONG)); - { - // - domFx_common_color_or_textureRef ambient = daeSafeCast(phong->add(COLLADA_ELEMENT_AMBIENT)); - { - // r g b a - domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast(ambient->add(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->add(COLLADA_ELEMENT_DIFFUSE)); - { - // r g b a - domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast(diffuse->add(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->add(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++; - - domNodeRef parent_node = node; - - { - if (!!urdf_link->parent_joint) { - // x y z - addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position, NULL, true); - // x y z w - addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true); - - domRotateRef joint_rotate; - // x y z angle - //joint_rotate = addRotate(parent_node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); - - joint_rotate = daeSafeCast(parent_node->add(COLLADA_ELEMENT_ROTATE)); - joint_rotate->getValue().setCount(4); - joint_rotate->getValue()[0] = urdf_link->parent_joint->axis.x; - joint_rotate->getValue()[1] = urdf_link->parent_joint->axis.y; - joint_rotate->getValue()[2] = urdf_link->parent_joint->axis.z; - joint_rotate->getValue()[3] = 0; - - 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; - } - - if (!!urdf_link->visual) { - // - domNodeRef visual_node = daeSafeCast(node->add(COLLADA_ELEMENT_NODE)); - string visual_sid("visual"); - string visual_id = node_id + "." + visual_sid; - visual_node->setName("visual"); - visual_node->setSid(visual_sid.c_str()); - visual_node->setId(visual_id.c_str()); - parent_node = visual_node; - - // x y z - addTranslate(parent_node, urdf_link->visual->origin.position, NULL, true); - // x y z w - addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true); - } - - // - map::const_iterator i = geometry_ids_.find(urdf_link->name); - if (i != geometry_ids_.end()) { - domInstance_geometryRef instance_geometry = daeSafeCast(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()); - { - // - domBind_materialRef bind_material = daeSafeCast(instance_geometry->add(COLLADA_ELEMENT_BIND_MATERIAL)); - { - // - domBind_material::domTechnique_commonRef technique_common = daeSafeCast(bind_material->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - { - // - domInstance_materialRef instance_material = daeSafeCast(technique_common->add(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, daeElementRef before, bool ignore_zero_translations) { - if (ignore_zero_translations && position.x == 0.0 && position.y == 0.0 && position.z == 0.0) - return NULL; - - // x y z - domTranslateRef trans; - if (before) { - trans = daeSafeCast(parent->createElement(COLLADA_ELEMENT_TRANSLATE)); - parent->addBefore(trans, before); - } - else - trans = daeSafeCast(parent->add(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, daeElementRef before, bool ignore_zero_rotations) { - 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 { - 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; - az = 0.0; - } - - // x y z w - domRotateRef rot; - if (before) { - rot = daeSafeCast(parent->createElement(COLLADA_ELEMENT_ROTATE)); - parent->addBefore(rot, before); - } - else - rot = daeSafeCast(parent->add(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; -} - - void ColladaWriter::addMimicJoint(domFormulaRef formula, const std::string& joint_sid,const std::string& joint_mimic_sid, double multiplier, double offset) -{ - string sid = joint_sid+string(".formula"); - formula->setSid(sid.c_str()); - - domCommon_float_or_paramRef ptarget = daeSafeCast(formula->createAndPlace(COLLADA_ELEMENT_TARGET)); - daeSafeCast(ptarget->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_sid.c_str()); - - domFormula_techniqueRef pftec = daeSafeCast(formula->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - - // a x b - daeElementRef pmath_math = pftec->createAndPlace("math"); - daeElementRef pmath_apply = pmath_math->createAndPlace("apply"); - { - daeElementRef pmath_plus = pmath_apply->createAndPlace("plus"); - daeElementRef pmath_apply1 = pmath_apply->createAndPlace("apply"); - { - daeElementRef pmath_times = pmath_apply1->createAndPlace("times"); - daeElementRef pmath_const0 = pmath_apply1->createAndPlace("cn"); - pmath_const0->setCharData(boost::lexical_cast(multiplier)); - daeElementRef pmath_symb = pmath_apply1->createAndPlace("csymbol"); - pmath_symb->setAttribute("encoding","COLLADA"); - pmath_symb->setCharData(joint_mimic_sid); - } - daeElementRef pmath_const1 = pmath_apply->createAndPlace("cn"); - pmath_const1->setCharData(boost::lexical_cast(offset)); - } -} - -string ColladaWriter::getTimeStampString() const { - // facet becomes owned by locale, so no need to explicitly delete - boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s"); - - std::stringstream ss(std::stringstream::in | std::stringstream::out); - ss.imbue(std::locale(ss.getloc(), facet)); - ss << boost::posix_time::second_clock::local_time(); - - string date; - ss >> date; - - return date; -} - -} diff --git a/collada_urdf/src/stl_loader.cpp b/collada_urdf/src/stl_loader.cpp deleted file mode 100644 index 4194f99..0000000 --- a/collada_urdf/src/stl_loader.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/********************************************************************* -* 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. -*********************************************************************/ - -/* Author: Tim Field */ - -#include "collada_urdf/stl_loader.h" - -#include -#include -#include - -#include - -using std::string; -using boost::shared_ptr; - -namespace collada_urdf { - -// Vector3 - -Vector3::Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { } - -bool Vector3::operator==(Vector3 const& v) const { - return x == v.x && y == v.y && z == v.z; -} - -// Mesh - -Mesh::Mesh() { -} - -int Mesh::getVertexIndex(const Vector3& v) const { - for (unsigned int i = 0; i < vertices.size(); i++) - if (vertices[i] == v) - return i; - - return -1; -} - -void Mesh::addVertex(Vector3 const& v) { vertices.push_back(v); } -void Mesh::addNormal(Vector3 const& n) { normals.push_back(n); } -void Mesh::addIndex(unsigned int i) { indices.push_back(i); } - -// STLLoader - -shared_ptr STLLoader::load(std::string const& filename) { - file_ = fopen(filename.c_str(), "r"); - if (file_ == NULL) - return shared_ptr(); - - mesh_ = shared_ptr(new Mesh); - readBinary(); - fclose(file_); - file_ = NULL; - - return mesh_; -} - -void STLLoader::readBinary() { - // 80 byte Header - for (int i = 0; i < 80; i++) - fgetc(file_); - - int face_num = readLongInt(); - - for (int iface = 0; iface < face_num; iface++) { - float nx = readFloat(); - float ny = readFloat(); - float nz = readFloat(); - Vector3 normal(nx, ny, nz); - - for (int i = 0; i < 3; i++) { - float vx = readFloat(); - float vy = readFloat(); - float vz = readFloat(); - Vector3 vertex(vx, vy, vz); - - int index = mesh_->getVertexIndex(vertex); - if (index == -1) { - mesh_->addVertex(vertex); - mesh_->addNormal(normal); - index = mesh_->vertices.size() - 1; - } - mesh_->addIndex(index); - } - - readShortInt(); // 2 byte attribute - } -} - -float STLLoader::readFloat() { - float rval; - if (fread(&rval, sizeof(float), 1, file_) == 0) - std::cerr << "Error in STLLoader::readFloat" << std::endl; - - return rval; -} - -uint32_t STLLoader::readLongInt() { - union - { - uint32_t yint; - char ychar[4]; - } y; - y.ychar[0] = fgetc(file_); - y.ychar[1] = fgetc(file_); - y.ychar[2] = fgetc(file_); - y.ychar[3] = fgetc(file_); - - return y.yint; -} - -uint16_t STLLoader::readShortInt() { - uint8_t c1 = fgetc(file_); - uint8_t c2 = fgetc(file_); - - uint16_t ival = c1 | (c2 << 8); - - return ival; -} - -} diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index 6785ebe..163ec9d 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -48,8 +48,13 @@ int main(int argc, char** argv) std::string input_filename(argv[1]); std::string output_filename(argv[2]); + urdf::Model robot_model; + if( !robot_model.initFile(input_filename) ) { + ROS_ERROR("failed to open urdf file %s",input_filename.c_str()); + } + boost::shared_ptr dom; - if (!collada_urdf::colladaFromUrdfFile(input_filename, dom)) { + if (!collada_urdf::colladaFromUrdfModel(robot_model, dom)) { std::cerr << std::endl << "Error converting document" << std::endl; return -1; } diff --git a/urdf/manifest.xml b/urdf/manifest.xml index f857168..9e5ed9a 100644 --- a/urdf/manifest.xml +++ b/urdf/manifest.xml @@ -13,6 +13,7 @@ +