pax_global_header00006660000000000000000000000064137033676020014521gustar00rootroot0000000000000052 comment=461d6016d11966e8cca27511712f5fdafa263e93 collada_urdf-1.12.13/000077500000000000000000000000001370336760200143055ustar00rootroot00000000000000collada_urdf-1.12.13/.gitignore000066400000000000000000000000071370336760200162720ustar00rootroot00000000000000.*.sw? collada_urdf-1.12.13/README.md000066400000000000000000000024611370336760200155670ustar00rootroot00000000000000# Collada URDF This contains packages for converting from collada files to URDF. See the ROS wiki for API documentation and tutorials. * [`collada_urdf`](http://wiki.ros.org/collada_urdf) * [`collada_parser`](http://wiki.ros.org/collada_parser) This was originally part of the [`ros/robot_model`](https://github.com/ros/robot_model) repository. It has been moved to this repo as described by [`ros/robot_model#195`](https://github.com/ros/robot_model/issues/195) ## Linking issue with Raspbian If you are using a Raspberry Pi with Raspbian you might encounter a linking problem with Assimp that looks like: ```bash ~/ros_catkin_ws/devel_isolated/collada_urdf/lib/libcollada_urdf.so: undefined reference to `typeinfo for Assimp::IOSystem' collect2: error: ld returned 1 exit status ``` A work-around consists of compiling and installing Assimp latest version: ```bash mkdir -p $HOME/libraries/assimp-3.3.1/build_release cd $HOME/libraries/assimp-3.3.1/ wget https://github.com/assimp/assimp/archive/v3.3.1.zip unzip v3.3.1.zip rm v3.3.1.zip mv v3.3.1 src cd build_release cmake ../src -DCMAKE_BUILD_TYPE=Release -DASSIMP_BUILD_TESTS=False make -j3 sudo make -j3 install ``` Make sure you reconfigure the catkin workspace (or just delete the build files) so that the newest Assimp version is found before compiling again. collada_urdf-1.12.13/collada_parser/000077500000000000000000000000001370336760200172605ustar00rootroot00000000000000collada_urdf-1.12.13/collada_parser/CHANGELOG.rst000066400000000000000000000062031370336760200213020ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package collada_parser ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.12.13 (2020-07-14) -------------------- * Update to newer CMake to get rid of CMP0048 warning (`#38 `_) * Contributors: Chris Lalancette 1.12.12 (2018-05-08) -------------------- * add exec_depend to package.xml of collada_parser for loading by pluginlib (`#27 `_) * Contributors: Yohei Kakiuchi 1.12.11 (2018-04-17) -------------------- * Collada cleanup dependencies (`#26 `_) * update links now that this is in its own repo * Make CMakeLists.txt depend on collada-dom version 2.4. (`#11 `_) * Contributors: Chris Lalancette, Mikael Arguedas 1.12.10 (2017-05-04) -------------------- * Moved collada_parser and collada_urdf to new repository 1.12.9 (2017-04-26) ------------------- 1.12.8 (2017-03-27) ------------------- * add Chris and Shane as maintainers (`#184 `_) * fix missed mandatory -std=c++11 flag (`#181 `_) collada_parser,kdl_parser,urdf: add c++11 flag, collada_parser: replace typeof with ansi __typeof\_\_ builded/tested on gentoo Thanks den4ix for the contribution! * Contributors: Denis Romanchuk, William Woodall 1.12.7 (2017-01-26) ------------------- 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) * Contributors: Jochen Sprickerhof 1.12.5 (2016-10-27) ------------------- 1.12.4 (2016-08-23) ------------------- 1.12.3 (2016-06-10) ------------------- 1.12.2 (2016-04-12) ------------------- 1.12.1 (2016-04-10) ------------------- 1.11.8 (2015-09-11) ------------------- 1.11.7 (2015-04-22) ------------------- 1.11.6 (2014-11-30) ------------------- * fix rotation of joint axis when oriantation between parent link and child link is differ * Contributors: YoheiKakiuchi 1.11.5 (2014-07-24) ------------------- 1.11.4 (2014-07-07) ------------------- * collada_parser: add extract actuators, fix for using nominal torque * moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633 * Contributors: Tully Foote, YoheiKakiuchi 1.11.3 (2014-06-24) ------------------- * update usage of urdfdom_headers for indigo/trusty * Contributors: William Woodall 1.11.2 (2014-03-22) ------------------- 1.11.1 (2014-03-20) ------------------- * remove visual and collision if there is no vertices * do not use visual and collision group * fix debug message * fix problem of root coordinates * Contributors: YoheiKakiuchi 1.11.0 (2014-02-21) ------------------- * fix, joint axis should be rotated depend on local coords * fix overwriting velocity limit * Contributors: YoheiKakiuchi 1.10.18 (2013-12-04) -------------------- * add DEPENDS for kdl_parser * Contributors: Ioan Sucan 1.10.16 (2013-11-18) -------------------- * fix for using collada_parser_plugin 1.10.15 (2013-08-17) -------------------- collada_urdf-1.12.13/collada_parser/CMakeLists.txt000066400000000000000000000040541370336760200220230ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(collada_parser) find_package(Boost REQUIRED system) find_package(catkin REQUIRED COMPONENTS class_loader rosconsole urdf urdf_parser_plugin) find_package(urdfdom_headers REQUIRED) add_compile_options(-std=c++11) include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS}) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake-extensions/) find_package(PkgConfig) find_package(COLLADA_DOM 2.4 REQUIRED COMPONENTS 1.5) if(COLLADA_DOM_FOUND) include_directories(${COLLADA_DOM_INCLUDE_DIRS}) link_directories(${COLLADA_DOM_LIBRARY_DIRS}) endif() catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include CATKIN_DEPENDS class_loader rosconsole urdf urdf_parser_plugin DEPENDS COLLADA_DOM ) # necessary for collada reader to create the temporary dae files due # to limitations in the URDF geometry include (CheckFunctionExists) check_function_exists(mkstemps HAVE_MKSTEMPS) if(HAVE_MKSTEMPS) add_definitions("-DHAVE_MKSTEMPS") endif() # build the parser lib add_library(${PROJECT_NAME} src/collada_parser.cpp) target_link_libraries(${PROJECT_NAME} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) # build the plugin for the parser add_library(${PROJECT_NAME}_plugin src/collada_parser_plugin.cpp) target_link_libraries(${PROJECT_NAME}_plugin ${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) set_target_properties(${PROJECT_NAME} PROPERTIES COMPILER_FLAGS "${COLLADA_DOM_CFLAGS_OTHER}" ) if(APPLE) set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "${COLLADA_DOM_LDFLAGS_OTHER} -undefined dynamic_lookup" ) else() set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "${COLLADA_DOM_LDFLAGS_OTHER}" ) endif() install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(FILES collada_parser_plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) collada_urdf-1.12.13/collada_parser/collada_parser_plugin_description.xml000066400000000000000000000004021370336760200267320ustar00rootroot00000000000000 Parse models as URDF from Collada files. collada_urdf-1.12.13/collada_parser/include/000077500000000000000000000000001370336760200207035ustar00rootroot00000000000000collada_urdf-1.12.13/collada_parser/include/collada_parser/000077500000000000000000000000001370336760200236565ustar00rootroot00000000000000collada_urdf-1.12.13/collada_parser/include/collada_parser/collada_parser.h000066400000000000000000000040361370336760200270050ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, 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: * * * Redistributions 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: Wim Meeussen */ #ifndef COLLADA_PARSER_COLLADA_PARSER_H #define COLLADA_PARSER_COLLADA_PARSER_H #include #include namespace urdf { /// \brief Load Model from string urdf::ModelInterfaceSharedPtr parseCollada(const std::string& xml_string); } #endif collada_urdf-1.12.13/collada_parser/include/collada_parser/collada_parser_plugin.h000066400000000000000000000041041370336760200303570ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2013, 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: * * * Redistributions 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: Ioan Sucan */ #ifndef COLLADA_PARSER_COLLADA_PARSER_PLUGIN_H #define COLLADA_PARSER_COLLADA_PARSER_PLUGIN_H #include #include namespace urdf { class ColladaURDFParser : public URDFParser { public: virtual urdf::ModelInterfaceSharedPtr parse(const std::string& xml_string); }; } #endif collada_urdf-1.12.13/collada_parser/mainpage.dox000066400000000000000000000011201370336760200215470ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b collada_parser is ... \section codeapi Code API */ collada_urdf-1.12.13/collada_parser/package.xml000066400000000000000000000033051370336760200213760ustar00rootroot00000000000000 collada_parser 1.12.13 This package contains a C++ parser for the Collada robot description format. The parser reads a Collada XML robot description, and creates a C++ URDF model. Although it is possible to directly use this parser when working with Collada robot descriptions, the preferred user API is found in the urdf package. Rosen Diankov Kei Okada Ioan Sucan Jackie Kay Chris Lalancette Shane Loretz BSD http://ros.org/wiki/collada_parser https://github.com/ros/collada_urdf https://github.com/ros/collada_urdf/issues catkin urdf urdf_parser_plugin class_loader collada-dom liburdfdom-headers-dev rosconsole urdf urdf_parser_plugin class_loader collada-dom rosconsole urdf_parser_plugin collada_urdf-1.12.13/collada_parser/src/000077500000000000000000000000001370336760200200475ustar00rootroot00000000000000collada_urdf-1.12.13/collada_parser/src/collada_parser.cpp000066400000000000000000003740631370336760200235430ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2010, University of Tokyo * 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: Rosen Diankov, used OpenRAVE files for reference */ #include #include #include #include #include #include #include #include /* disable deprecated auto_ptr warnings */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include #include #pragma GCC diagnostic pop #include #include #include #include #include #include #include #include #include #include #include #include #include #ifndef HAVE_MKSTEMPS #include #include #endif #define typeof __typeof__ #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++) #define FOREACHC FOREACH namespace ColladaDOM150 { } namespace urdf { using namespace ColladaDOM150; class UnlinkFilename { public: UnlinkFilename(const std::string& filename) : _filename(filename) { } virtual ~UnlinkFilename() { unlink(_filename.c_str()); } std::string _filename; }; static std::list > _listTempFilenames; class ColladaModelReader : public daeErrorHandler { class JointAxisBinding { public: JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) { BOOST_ASSERT( !!pkinematicaxis ); daeElement* pae = pvisualtrans->getParentElement(); while (!!pae) { visualnode = daeSafeCast (pae); if (!!visualnode) { break; } pae = pae->getParentElement(); } if (!visualnode) { ROS_WARN_STREAM(str(boost::format("couldn't find parent node of element id %s, sid %s\n")%pkinematicaxis->getID()%pkinematicaxis->getSid())); } } daeElementRef pvisualtrans; domAxis_constraintRef pkinematicaxis; domCommon_float_or_paramRef jointvalue; domNodeRef visualnode; domKinematics_axis_infoRef kinematics_axis_info; domMotion_axis_infoRef motion_axis_info; }; /// \brief bindings for links between different spaces class LinkBinding { public: domNodeRef node; domLinkRef domlink; domInstance_rigid_bodyRef irigidbody; domRigid_bodyRef rigidbody; domNodeRef nodephysicsoffset; }; /// \brief inter-collada bindings for a kinematics scene class KinematicsSceneBindings { public: std::list< std::pair > listKinematicsVisualBindings; std::list listAxisBindings; std::list listLinkBindings; bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) { if( !kinematics_axis_info ) { return false; } for(size_t ik = 0; ik < arr.getCount(); ++ik) { daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt; if( !!pelt ) { // look for the correct placement bool bfound = false; FOREACH(itbinding,listAxisBindings) { if( itbinding->pkinematicaxis.cast() == pelt ) { itbinding->kinematics_axis_info = kinematics_axis_info; if( !!motion_axis_info ) { itbinding->motion_axis_info = motion_axis_info; } bfound = true; break; } } if( !bfound ) { ROS_WARN_STREAM(str(boost::format("could not find binding for axis: %s, %s\n")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid"))); return false; } return true; } } ROS_WARN_STREAM(str(boost::format("could not find kinematics axis target: %s\n")%kinematics_axis_info->getAxis())); return false; } }; struct USERDATA { USERDATA() { } USERDATA(double scale) : scale(scale) { } double scale; #if URDFDOM_HEADERS_MAJOR_VERSION < 1 boost::shared_ptr p; ///< custom managed data #else std::shared_ptr p; ///< custom managed data #endif }; enum GeomType { GeomNone = 0, GeomBox = 1, GeomSphere = 2, GeomCylinder = 3, GeomTrimesh = 4, }; struct GEOMPROPERTIES { Pose _t; ///< local transformation of the geom primitive with respect to the link's coordinate system Vector3 vGeomData; ///< for boxes, first 3 values are extents ///< for sphere it is radius ///< for cylinder, first 2 values are radius and height ///< for trimesh, none Color diffuseColor, ambientColor; ///< hints for how to color the meshes std::vector vertices; std::vector indices; ///< discretization value is chosen. Should be transformed by _t before rendering GeomType type; ///< the type of geometry primitive // generate a sphere triangulation starting with an icosahedron // all triangles are oriented counter clockwise static void GenerateSphereTriangulation(std::vector realvertices, std::vector realindices, int levels) { const double GTS_M_ICOSAHEDRON_X = 0.850650808352039932181540497063011072240401406; const double GTS_M_ICOSAHEDRON_Y = 0.525731112119133606025669084847876607285497935; const double GTS_M_ICOSAHEDRON_Z = 0; std::vector tempvertices[2]; std::vector tempindices[2]; tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y)); tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y)); tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X)); tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y)); const int nindices=60; int indices[nindices] = { 0, 1, 2, 1, 3, 4, 3, 5, 6, 2, 4, 7, 5, 6, 8, 2, 7, 9, 0, 5, 8, 7, 9, 10, 0, 1, 5, 7, 10, 11, 1, 3, 5, 6, 10, 11, 3, 6, 11, 9, 10, 8, 3, 4, 11, 6, 8, 10, 4, 7, 11, 1, 2, 4, 0, 8, 9, 0, 2, 9 }; Vector3 v[3]; // make sure oriented CCW for(int i = 0; i < nindices; i += 3 ) { v[0] = tempvertices[0][indices[i]]; v[1] = tempvertices[0][indices[i+1]]; v[2] = tempvertices[0][indices[i+2]]; if( _dot3(v[0], _cross3(_sub3(v[1],v[0]),_sub3(v[2],v[0]))) < 0 ) { std::swap(indices[i], indices[i+1]); } } tempindices[0].resize(nindices); std::copy(&indices[0],&indices[nindices],tempindices[0].begin()); std::vector* curvertices = &tempvertices[0], *newvertices = &tempvertices[1]; std::vector *curindices = &tempindices[0], *newindices = &tempindices[1]; while(levels-- > 0) { newvertices->resize(0); newvertices->reserve(2*curvertices->size()); newvertices->insert(newvertices->end(), curvertices->begin(), curvertices->end()); newindices->resize(0); newindices->reserve(4*curindices->size()); std::map< uint64_t, int > mapnewinds; std::map< uint64_t, int >::iterator it; for(size_t i = 0; i < curindices->size(); i += 3) { // for ever tri, create 3 new vertices and 4 new triangles. v[0] = curvertices->at(curindices->at(i)); v[1] = curvertices->at(curindices->at(i+1)); v[2] = curvertices->at(curindices->at(i+2)); int inds[3]; for(int j = 0; j < 3; ++j) { uint64_t key = ((uint64_t)curindices->at(i+j)<<32)|(uint64_t)curindices->at(i + ((j+1)%3)); it = mapnewinds.find(key); if( it == mapnewinds.end() ) { inds[j] = mapnewinds[key] = mapnewinds[(key<<32)|(key>>32)] = (int)newvertices->size(); newvertices->push_back(_normalize3(_add3(v[j],v[(j+1)%3 ]))); } else { inds[j] = it->second; } } newindices->push_back(curindices->at(i)); newindices->push_back(inds[0]); newindices->push_back(inds[2]); newindices->push_back(inds[0]); newindices->push_back(curindices->at(i+1)); newindices->push_back(inds[1]); newindices->push_back(inds[2]); newindices->push_back(inds[0]); newindices->push_back(inds[1]); newindices->push_back(inds[2]); newindices->push_back(inds[1]); newindices->push_back(curindices->at(i+2)); } std::swap(newvertices,curvertices); std::swap(newindices,curindices); } realvertices = *curvertices; realindices = *curindices; } bool InitCollisionMesh(double fTessellation=1.0) { if( type == GeomTrimesh ) { return true; } indices.clear(); vertices.clear(); if( fTessellation < 0.01f ) { fTessellation = 0.01f; } // start tesselating switch(type) { case GeomSphere: { // log_2 (1+ tess) GenerateSphereTriangulation(vertices,indices, 3 + (int)(logf(fTessellation) / logf(2.0f)) ); double fRadius = vGeomData.x; FOREACH(it, vertices) { it->x *= fRadius; it->y *= fRadius; it->z *= fRadius; } break; } case GeomBox: { // trivial Vector3 ex = vGeomData; Vector3 v[8] = { Vector3(ex.x, ex.y, ex.z), Vector3(ex.x, ex.y, -ex.z), Vector3(ex.x, -ex.y, ex.z), Vector3(ex.x, -ex.y, -ex.z), Vector3(-ex.x, ex.y, ex.z), Vector3(-ex.x, ex.y, -ex.z), Vector3(-ex.x, -ex.y, ex.z), Vector3(-ex.x, -ex.y, -ex.z) }; const int nindices = 36; int startindices[] = { 0, 1, 2, 1, 2, 3, 4, 5, 6, 5, 6, 7, 0, 1, 4, 1, 4, 5, 2, 3, 6, 3, 6, 7, 0, 2, 4, 2, 4, 6, 1, 3, 5, 3, 5, 7 }; for(int i = 0; i < nindices; i += 3 ) { Vector3 v1 = v[startindices[i]]; Vector3 v2 = v[startindices[i+1]]; Vector3 v3 = v[startindices[i+2]]; if( _dot3(v1, _sub3(v2,_cross3(v1, _sub3(v3,v1)))) < 0 ) { std::swap(indices[i], indices[i+1]); } } vertices.resize(8); std::copy(&v[0],&v[8],vertices.begin()); indices.resize(nindices); std::copy(&startindices[0],&startindices[nindices],indices.begin()); break; } case GeomCylinder: { // cylinder is on y axis double rad = vGeomData.x, len = vGeomData.y*0.5f; int numverts = (int)(fTessellation*24.0f) + 3; double dtheta = 2 * M_PI / (double)numverts; vertices.push_back(Vector3(0,0,len)); vertices.push_back(Vector3(0,0,-len)); vertices.push_back(Vector3(rad,0,len)); vertices.push_back(Vector3(rad,0,-len)); for(int i = 0; i < numverts+1; ++i) { double s = rad * std::sin(dtheta * (double)i); double c = rad * std::cos(dtheta * (double)i); int off = (int)vertices.size(); vertices.push_back(Vector3(c, s, len)); vertices.push_back(Vector3(c, s, -len)); indices.push_back(0); indices.push_back(off); indices.push_back(off-2); indices.push_back(1); indices.push_back(off-1); indices.push_back(off+1); indices.push_back(off-2); indices.push_back(off); indices.push_back(off-1); indices.push_back(off); indices.push_back(off-1); indices.push_back(off+1); } break; } default: BOOST_ASSERT(0); } return true; } }; public: ColladaModelReader(urdf::ModelInterfaceSharedPtr model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) { daeErrorHandler::setErrorHandler(this); _resourcedir = "."; } virtual ~ColladaModelReader() { _vuserdata.clear(); _collada.reset(); DAE::cleanup(); } bool InitFromFile(const std::string& filename) { ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE%filename)); _collada.reset(new DAE); _dom = (domCOLLADA*)_collada->open(filename); if (!_dom) { return false; } _filename=filename; size_t maxchildren = _countChildren(_dom); _vuserdata.resize(0); _vuserdata.reserve(maxchildren); double dScale = 1.0; _processUserData(_dom, dScale); ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren)); return _Extract(); } bool InitFromData(const std::string& pdata) { ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE)); _collada.reset(new DAE); _dom = (domCOLLADA*)_collada->openFromMemory(".",pdata.c_str()); if (!_dom) { return false; } size_t maxchildren = _countChildren(_dom); _vuserdata.resize(0); _vuserdata.reserve(maxchildren); double dScale = 1.0; _processUserData(_dom, dScale); ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren)); return _Extract(); } protected: /// \extract the first possible robot in the scene bool _Extract() { _model->clear(); std::list< std::pair > > listPossibleBodies; domCOLLADA::domSceneRef allscene = _dom->getScene(); if( !allscene ) { return false; } // parse each instance kinematics scene, prioritize real robots for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) { domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene]; domKinematics_sceneRef kscene = daeSafeCast (kiscene->getUrl().getElement().cast()); if (!kscene) { continue; } boost::shared_ptr bindings(new KinematicsSceneBindings()); _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings); _ExtractPhysicsBindings(allscene,*bindings); for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) { if( _ExtractArticulatedSystem(kscene->getInstance_articulated_system_array()[ias], *bindings) ) { _PostProcess(); return true; } } for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) { listPossibleBodies.push_back(std::make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings)); } } FOREACH(it, listPossibleBodies) { if( _ExtractKinematicsModel(it->first, *it->second) ) { _PostProcess(); return true; } } return false; } void _PostProcess() { std::map parent_link_tree; // building tree: name mapping try { _model->initTree(parent_link_tree); } catch(ParseError &e) { ROS_ERROR("Failed to build tree: %s", e.what()); } // find the root link try { _model->initRoot(parent_link_tree); } catch(ParseError &e) { ROS_ERROR("Failed to find root link: %s", e.what()); } } /// \brief extracts an articulated system. Note that an articulated system can include other articulated systems bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings) { if( !ias ) { return false; } ROS_DEBUG_STREAM(str(boost::format("instance articulated system sid %s\n")%ias->getSid())); domArticulated_systemRef articulated_system = daeSafeCast (ias->getUrl().getElement().cast()); if( !articulated_system ) { return false; } boost::shared_ptr pinterface_type = _ExtractInterfaceType(ias->getExtra_array()); if( !pinterface_type ) { pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array()); } if( !!pinterface_type ) { ROS_DEBUG_STREAM(str(boost::format("robot type: %s")%(*pinterface_type))); } // set the name if( _model->name_.size() == 0 && !!ias->getName() ) { _model->name_ = ias->getName(); } if( _model->name_.size() == 0 && !!ias->getSid()) { _model->name_ = ias->getSid(); } if( _model->name_.size() == 0 && !!articulated_system->getName() ) { _model->name_ = articulated_system->getName(); } if( _model->name_.size() == 0 && !!articulated_system->getId()) { _model->name_ = articulated_system->getId(); } if( !!articulated_system->getMotion() ) { domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system(); if( !!articulated_system->getMotion()->getTechnique_common() ) { for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) { domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i]; // this should point to a kinematics axis_info domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt); if( !!kinematics_axis_info ) { // find the parent kinematics and go through all its instance kinematics models daeElement* pparent = kinematics_axis_info->getParent(); while(!!pparent && pparent->typeID() != domKinematics::ID()) { pparent = pparent->getParent(); } BOOST_ASSERT(!!pparent); bindings.AddAxisInfo(daeSafeCast(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info); } else { ROS_WARN_STREAM(str(boost::format("failed to find kinematics axis %s\n")%motion_axis_info->getAxis())); } } } if( !_ExtractArticulatedSystem(ias_new,bindings) ) { return false; } } else { if( !articulated_system->getKinematics() ) { ROS_WARN_STREAM(str(boost::format("collada tag empty? instance_articulated_system=%s\n")%ias->getID())); return true; } if( !!articulated_system->getKinematics()->getTechnique_common() ) { for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) { bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL); } } for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) { _ExtractKinematicsModel(articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings); } } _ExtractRobotAttachedActuators(articulated_system); _ExtractRobotManipulators(articulated_system); _ExtractRobotAttachedSensors(articulated_system); return true; } bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings) { if( !ikm ) { return false; } ROS_DEBUG_STREAM(str(boost::format("instance kinematics model sid %s\n")%ikm->getSid())); domKinematics_modelRef kmodel = daeSafeCast (ikm->getUrl().getElement().cast()); if (!kmodel) { ROS_WARN_STREAM(str(boost::format("%s does not reference valid kinematics\n")%ikm->getSid())); return false; } domPhysics_modelRef pmodel; boost::shared_ptr pinterface_type = _ExtractInterfaceType(ikm->getExtra_array()); if( !pinterface_type ) { pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array()); } if( !!pinterface_type ) { ROS_DEBUG_STREAM(str(boost::format("kinbody interface type: %s")%(*pinterface_type))); } // find matching visual node domNodeRef pvisualnode; FOREACH(it, bindings.listKinematicsVisualBindings) { if( it->second == ikm ) { pvisualnode = it->first; break; } } if( !pvisualnode ) { ROS_WARN_STREAM(str(boost::format("failed to find visual node for instance kinematics model %s\n")%ikm->getSid())); return false; } if( _model->name_.size() == 0 && !!ikm->getName() ) { _model->name_ = ikm->getName(); } if( _model->name_.size() == 0 && !!ikm->getID() ) { _model->name_ = ikm->getID(); } if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings)) { ROS_WARN_STREAM(str(boost::format("failed to load kinbody from kinematics model %s\n")%kmodel->getID())); return false; } return true; } /// \brief append the kinematics model to the openrave kinbody bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings& bindings) { std::vector vdomjoints; ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model->name_)); if( !!pnode ) { ROS_DEBUG_STREAM(str(boost::format("node name: %s\n")%pnode->getId())); } // Process joint of the kinbody domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common(); // Store joints for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) { vdomjoints.push_back(ktec->getJoint_array()[ijoint]); } // Store instances of joints for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) { domJointRef pelt = daeSafeCast (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement()); if (!pelt) { ROS_WARN_STREAM("failed to get joint from instance\n"); } else { vdomjoints.push_back(pelt); } } ROS_DEBUG_STREAM(str(boost::format("Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount())); for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) { domLinkRef pdomlink = ktec->getLink_array()[ilink]; _RootOrigin = _poseFromMatrix(_ExtractFullTransform(pdomlink)); ROS_DEBUG("RootOrigin: %s %lf %lf %lf %lf %lf %lf %lf", pdomlink->getName(), _RootOrigin.position.x, _RootOrigin.position.y, _RootOrigin.position.z, _RootOrigin.rotation.x, _RootOrigin.rotation.y, _RootOrigin.rotation.z, _RootOrigin.rotation.w); domNodeRef pvisualnode; FOREACH(it, bindings.listKinematicsVisualBindings) { if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) { pvisualnode = it->first; break; } } if (!!pvisualnode) { _VisualRootOrigin = _poseFromMatrix(_getNodeParentTransform(pvisualnode)); ROS_DEBUG("VisualRootOrigin: %s %lf %lf %lf %lf %lf %lf %lf", pdomlink->getName(), _VisualRootOrigin.position.x, _VisualRootOrigin.position.y, _VisualRootOrigin.position.z, _VisualRootOrigin.rotation.x, _VisualRootOrigin.rotation.y, _VisualRootOrigin.rotation.z, _VisualRootOrigin.rotation.w); } _ExtractLink(pdomlink, ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings); } // TODO: implement mathml for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) { domFormulaRef pf = ktec->getFormula_array()[iform]; if (!pf->getTarget()) { ROS_WARN_STREAM("formula target not valid\n"); continue; } // find the target joint urdf::JointSharedPtr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf); if (!pjoint) { continue; } if (!!pf->getTechnique_common()) { daeElementRef peltmath; daeTArray children; pf->getTechnique_common()->getChildren(children); for (size_t ichild = 0; ichild < children.getCount(); ++ichild) { daeElementRef pelt = children[ichild]; if (_checkMathML(pelt,std::string("math")) ) { peltmath = pelt; } else { ROS_WARN_STREAM(str(boost::format("unsupported formula element: %s\n")%pelt->getElementName())); } } if (!!peltmath) { // full math xml spec not supported, only looking for ax+b pattern: // a x b double a = 1, b = 0; daeElementRef psymboljoint; BOOST_ASSERT(peltmath->getChildren().getCount()>0); daeElementRef papplyelt = peltmath->getChildren()[0]; BOOST_ASSERT(_checkMathML(papplyelt,"apply")); BOOST_ASSERT(papplyelt->getChildren().getCount()>0); if( _checkMathML(papplyelt->getChildren()[0],"plus") ) { BOOST_ASSERT(papplyelt->getChildren().getCount()==3); daeElementRef pa = papplyelt->getChildren()[1]; daeElementRef pb = papplyelt->getChildren()[2]; if( !_checkMathML(papplyelt->getChildren()[1],"apply") ) { std::swap(pa,pb); } if( !_checkMathML(pa,"csymbol") ) { BOOST_ASSERT(_checkMathML(pa,"apply")); BOOST_ASSERT(_checkMathML(pa->getChildren()[0],"times")); if( _checkMathML(pa->getChildren()[1],"csymbol") ) { psymboljoint = pa->getChildren()[1]; BOOST_ASSERT(_checkMathML(pa->getChildren()[2],"cn")); std::stringstream ss(pa->getChildren()[2]->getCharData()); ss >> a; } else { psymboljoint = pa->getChildren()[2]; BOOST_ASSERT(_checkMathML(pa->getChildren()[1],"cn")); std::stringstream ss(pa->getChildren()[1]->getCharData()); ss >> a; } } else { psymboljoint = pa; } BOOST_ASSERT(_checkMathML(pb,"cn")); { std::stringstream ss(pb->getCharData()); ss >> b; } } else if( _checkMathML(papplyelt->getChildren()[0],"minus") ) { BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[1],"csymbol")); a = -1; psymboljoint = papplyelt->getChildren()[1]; } else { BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[0],"csymbol")); psymboljoint = papplyelt->getChildren()[0]; } BOOST_ASSERT(psymboljoint->hasAttribute("encoding")); BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA")); urdf::JointSharedPtr pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf); if( !!pbasejoint ) { // set the mimic properties pjoint->mimic.reset(new JointMimic()); pjoint->mimic->joint_name = pbasejoint->name; pjoint->mimic->multiplier = a; pjoint->mimic->offset = b; ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b)); } } } } return true; } /// \brief Extract Link info and add it to an existing body urdf::LinkSharedPtr _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector& vdomjoints, const KinematicsSceneBindings& bindings) { const std::list& listAxisBindings = bindings.listAxisBindings; // Set link name with the name of the COLLADA's Link std::string linkname = _ExtractLinkName(pdomlink); if( linkname.size() == 0 ) { ROS_WARN_STREAM(" has no name or id, falling back to !\n"); if( !!pdomnode ) { if (!!pdomnode->getName()) { linkname = pdomnode->getName(); } if( linkname.size() == 0 && !!pdomnode->getID()) { linkname = pdomnode->getID(); } } } urdf::LinkSharedPtr plink; _model->getLink(linkname,plink); if( !plink ) { plink.reset(new Link()); plink->name = linkname; plink->visual.reset(new Visual()); plink->visual->material_name = ""; plink->visual->material.reset(new Material()); plink->visual->material->name = "Red"; plink->visual->material->color.r = 0.0; plink->visual->material->color.g = 1.0; plink->visual->material->color.b = 0.0; plink->visual->material->color.a = 1.0; plink->inertial.reset(); _model->links_.insert(std::make_pair(linkname,plink)); } _getUserData(pdomlink)->p = plink; if( !!pdomnode ) { ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName())); } // physics domInstance_rigid_bodyRef irigidbody; domRigid_bodyRef rigidbody; bool bFoundBinding = false; FOREACH(itlinkbinding, bindings.listLinkBindings) { if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) { bFoundBinding = true; irigidbody = itlinkbinding->irigidbody; rigidbody = itlinkbinding->rigidbody; } } if( !!rigidbody && !!rigidbody->getTechnique_common() ) { domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common(); if( !!rigiddata->getMass() ) { if ( !plink->inertial ) { plink->inertial.reset(new Inertial()); } plink->inertial->mass = rigiddata->getMass()->getValue(); } if( !!rigiddata->getInertia() ) { if ( !plink->inertial ) { plink->inertial.reset(new Inertial()); } plink->inertial->ixx = rigiddata->getInertia()->getValue()[0]; plink->inertial->iyy = rigiddata->getInertia()->getValue()[1]; plink->inertial->izz = rigiddata->getInertia()->getValue()[2]; } if( !!rigiddata->getMass_frame() ) { if ( !plink->inertial ) { plink->inertial.reset(new Inertial()); } //plink->inertial->origin = _poseMult(_poseInverse(tParentWorldLink), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame()))); Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink)); plink->inertial->origin = _poseMult(_poseInverse(_poseMult(_poseInverse(_RootOrigin), _poseMult(tParentWorldLink, tlink))), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame()))); } } std::list listGeomProperties; if (!pdomlink) { ROS_WARN_STREAM("Extract object NOT kinematics !!!\n"); _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,Pose()); } else { ROS_DEBUG_STREAM(str(boost::format("Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount())); Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink)); ROS_DEBUG("tlink: %s: %lf %lf %lf %lf %lf %lf %lf", linkname.c_str(), tlink.position.x, tlink.position.y, tlink.position.z, tlink.rotation.x, tlink.rotation.y, tlink.rotation.z, tlink.rotation.w); plink->visual->origin = _poseMult(tParentLink, tlink); // use the kinematics coordinate system for each link // ROS_INFO("link %s rot: %f %f %f %f",linkname.c_str(),plink->visual->origin.rotation.w, plink->visual->origin.rotation.x,plink->visual->origin.rotation.y,plink->visual->origin.rotation.z); // ROS_INFO("link %s trans: %f %f %f",linkname.c_str(),plink->visual->origin.position.x,plink->visual->origin.position.y,plink->visual->origin.position.z); // Get the geometry _ExtractGeometry(pdomnode, listGeomProperties, listAxisBindings, _poseMult(_poseMult(tParentWorldLink,tlink), plink->visual->origin)); ROS_DEBUG_STREAM(str(boost::format("After ExtractGeometry Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount())); // Process all atached links for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) { domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt]; // get link kinematics transformation Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull)); // process attached links daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt; domJointRef pdomjoint = daeSafeCast (peltjoint); if (!pdomjoint) { domInstance_jointRef pdomijoint = daeSafeCast (peltjoint); if (!!pdomijoint) { pdomjoint = daeSafeCast (pdomijoint->getUrl().getElement().cast()); } } if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) { ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint())); return urdf::LinkSharedPtr(); } // get direct child link if (!pattfull->getLink()) { ROS_WARN_STREAM(str(boost::format("joint %s needs to be attached to a valid link\n")%pdomjoint->getID())); continue; } // find the correct joint in the bindings daeTArray vdomaxes = pdomjoint->getChildrenByType(); domNodeRef pchildnode; // see if joint has a binding to a visual node FOREACHC(itaxisbinding,listAxisBindings) { for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { // If the binding for the joint axis is found, retrieve the info if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) { pchildnode = itaxisbinding->visualnode; break; } } if( !!pchildnode ) { break; } } if (!pchildnode) { ROS_DEBUG_STREAM(str(boost::format("joint %s has no visual binding\n")%pdomjoint->getID())); } // create the joints before creating the child links std::vector vjoints(vdomaxes.getCount()); for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { bool joint_active = true; // if not active, put into the passive list FOREACHC(itaxisbinding,listAxisBindings) { if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) { if( !!itaxisbinding->kinematics_axis_info ) { if( !!itaxisbinding->kinematics_axis_info->getActive() ) { joint_active = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info); } } break; } } urdf::JointSharedPtr pjoint(new Joint()); pjoint->limits.reset(new JointLimits()); pjoint->limits->velocity = 0.0; pjoint->limits->effort = 0.0; pjoint->parent_link_name = plink->name; if( !!pdomjoint->getName() ) { pjoint->name = pdomjoint->getName(); } else { pjoint->name = str(boost::format("dummy%d")%_model->joints_.size()); } if( !joint_active ) { ROS_INFO_STREAM(str(boost::format("joint %s is passive, but adding to hierarchy\n")%pjoint->name)); } domAxis_constraintRef pdomaxis = vdomaxes[ic]; if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) { pjoint->type = Joint::REVOLUTE; } else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) { pjoint->type = Joint::PRISMATIC; } else { ROS_WARN_STREAM(str(boost::format("unsupported joint type: %s\n")%pdomaxis->getElementName())); } _getUserData(pdomjoint)->p = pjoint; #if URDFDOM_HEADERS_MAJOR_VERSION < 1 _getUserData(pdomaxis)->p = boost::shared_ptr(new int(_model->joints_.size())); #else _getUserData(pdomaxis)->p = std::shared_ptr(new int(_model->joints_.size())); #endif _model->joints_[pjoint->name] = pjoint; vjoints[ic] = pjoint; } urdf::LinkSharedPtr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings); if (!pchildlink) { ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name)); continue; } int numjoints = 0; for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { domKinematics_axis_infoRef kinematics_axis_info; domMotion_axis_infoRef motion_axis_info; FOREACHC(itaxisbinding,listAxisBindings) { bool bfound = false; if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) { kinematics_axis_info = itaxisbinding->kinematics_axis_info; motion_axis_info = itaxisbinding->motion_axis_info; bfound = true; break; } } domAxis_constraintRef pdomaxis = vdomaxes[ic]; if (!pchildlink) { // create dummy child link // multiple axes can be easily done with "empty links" ROS_WARN_STREAM(str(boost::format("creating dummy link %s, num joints %d\n")%plink->name%numjoints)); std::stringstream ss; ss << plink->name; ss <<"_dummy" << numjoints; pchildlink.reset(new Link()); pchildlink->name = ss.str(); _model->links_.insert(std::make_pair(pchildlink->name,pchildlink)); } ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic)); urdf::JointSharedPtr pjoint = vjoints[ic]; pjoint->child_link_name = pchildlink->name; #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \ apose.position.x, apose.position.y, apose.position.z, \ apose.rotation.x, apose.rotation.y, apose.rotation.z, apose.rotation.w); { PRINT_POSE("tatt", tatt); PRINT_POSE("trans_joint_to_child", tatt); Pose trans_joint_to_child = _poseFromMatrix(_ExtractFullTransform(pattfull->getLink())); pjoint->parent_to_joint_origin_transform = _poseMult(tatt, trans_joint_to_child); // Axes and Anchor assignment. Vector3 ax(pdomaxis->getAxis()->getValue()[0], pdomaxis->getAxis()->getValue()[1], pdomaxis->getAxis()->getValue()[2]); Pose pinv = _poseInverse(trans_joint_to_child); // rotate axis ax = pinv.rotation * ax; pjoint->axis.x = ax.x; pjoint->axis.y = ax.y; pjoint->axis.z = ax.z; ROS_DEBUG("joint %s axis: %f %f %f -> %f %f %f\n", pjoint->name.c_str(), pdomaxis->getAxis()->getValue()[0], pdomaxis->getAxis()->getValue()[1], pdomaxis->getAxis()->getValue()[2], pjoint->axis.x, pjoint->axis.y, pjoint->axis.z); } if (!motion_axis_info) { ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name)); } // Sets the Speed and the Acceleration of the joint if (!!motion_axis_info) { if (!!motion_axis_info->getSpeed()) { pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info); ROS_DEBUG("... Joint Speed: %f...\n",pjoint->limits->velocity); } if (!!motion_axis_info->getAcceleration()) { pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info); ROS_DEBUG("... Joint effort: %f...\n",pjoint->limits->effort); } } bool joint_locked = false; // if locked, joint angle is static bool kinematics_limits = false; if (!!kinematics_axis_info) { if (!!kinematics_axis_info->getLocked()) { joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info); } if (joint_locked) { // If joint is locked set limits to the static value. if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) { ROS_WARN_STREAM("lock joint!!\n"); pjoint->limits->lower = 0; pjoint->limits->upper = 0; } } else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits kinematics_limits = true; double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(kinematics_axis_info); if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) { pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info)); pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info)); if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) { pjoint->type = Joint::FIXED; } } } } // Search limits in the joints section if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) { // If there are NO LIMITS if( !pdomaxis->getLimits() ) { ROS_DEBUG_STREAM(str(boost::format("There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits)); if( pjoint->type == Joint::REVOLUTE ) { pjoint->type = Joint::CONTINUOUS; // continuous means revolute? pjoint->limits->lower = -M_PI; pjoint->limits->upper = M_PI; } else { pjoint->limits->lower = -100000; pjoint->limits->upper = 100000; } } else { ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name)); double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(pdomaxis); pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale; pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale; if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) { pjoint->type = Joint::FIXED; } } } if (pjoint->limits->velocity == 0.0) { pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f; } pchildlink.reset(); ++numjoints; } } } if( pdomlink->getAttachment_start_array().getCount() > 0 ) { ROS_WARN("urdf collada reader does not support attachment_start\n"); } if( pdomlink->getAttachment_end_array().getCount() > 0 ) { ROS_WARN("urdf collada reader does not support attachment_end\n"); } plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties); // visual_groups deprecated //boost::shared_ptr > viss; //viss.reset(new std::vector); //viss->push_back(plink->visual); //plink->visual_groups.insert(std::make_pair("default", viss)); if( !plink->visual->geometry ) { plink->visual.reset(); plink->collision.reset(); } else { // collision plink->collision.reset(new Collision()); plink->collision->geometry = plink->visual->geometry; plink->collision->origin = plink->visual->origin; } // collision_groups deprecated //boost::shared_ptr > cols; //cols.reset(new std::vector); //cols->push_back(plink->collision); //plink->collision_groups.insert(std::make_pair("default", cols)); return plink; } urdf::GeometrySharedPtr _CreateGeometry(const std::string& name, const std::list& listGeomProperties) { std::vector > vertices; std::vector > indices; std::vector ambients; std::vector diffuses; unsigned int index, vert_counter; vertices.resize(listGeomProperties.size()); indices.resize(listGeomProperties.size()); ambients.resize(listGeomProperties.size()); diffuses.resize(listGeomProperties.size()); index = 0; vert_counter = 0; FOREACHC(it, listGeomProperties) { vertices[index].resize(it->vertices.size()); for(size_t i = 0; i < it->vertices.size(); ++i) { vertices[index][i] = _poseMult(it->_t, it->vertices[i]); vert_counter++; } indices[index].resize(it->indices.size()); for(size_t i = 0; i < it->indices.size(); ++i) { indices[index][i] = it->indices[i]; } ambients[index] = it->ambientColor; diffuses[index] = it->diffuseColor; // workaround for mesh_loader.cpp:421 // Most of are DAE files don't have ambient color defined ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5; if ( ambients[index].r == 0.0 ) { ambients[index].r = 0.0001; } if ( ambients[index].g == 0.0 ) { ambients[index].g = 0.0001; } if ( ambients[index].b == 0.0 ) { ambients[index].b = 0.0001; } index++; } if (vert_counter == 0) { urdf::MeshSharedPtr ret; ret.reset(); return ret; } urdf::MeshSharedPtr geometry(new Mesh()); geometry->type = Geometry::MESH; geometry->scale.x = 1; geometry->scale.y = 1; geometry->scale.z = 1; // have to save the geometry into individual collada 1.4 files since URDF does not allow triangle meshes to be specified std::stringstream daedata; daedata << str(boost::format("\n\ \n\ \n\ \n\ Rosen Diankov\n\ \n\ robot_model/urdf temporary collada geometry\n\ \n\ \n\ \n\ Y_UP\n\ \n\ \n")); for(unsigned int i=0; i < index; i++) { daedata << str(boost::format("\ \n\ \n\ \n")%i%i%i); } daedata << "\ \n\ \n"; for(unsigned int i=0; i < index; i++) { daedata << str(boost::format("\ \n\ \n\ \n\ \n\ \n\ %f %f %f %f\n\ \n\ \n\ %f %f %f %f\n\ \n\ \n\ 0 0 0 1\n\ \n\ \n\ \n\ \n\ \n")%i%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a); } daedata << str(boost::format("\ \n\ \n")); // fill with vertices for(unsigned int i=0; i < index; i++) { daedata << str(boost::format("\ \n\ \n\ \n\ ")%i%i%(vertices[i].size()*3)); FOREACH(it,vertices[i]) { daedata << it->x << " " << it->y << " " << it->z << " "; } daedata << str(boost::format("\n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\ \n\

")%vertices[i].size()%(indices[i].size()/3)); // fill with indices FOREACH(it,indices[i]) { daedata << *it << " "; } daedata << str(boost::format("

\n\ \n\ \n\ \n")); } daedata << str(boost::format("\ \n\ \n\ \n\ \n")%name%name); for(unsigned int i=0; i < index; i++) { daedata << str(boost::format("\ \n\ \n\ \n\ \n\ \n\ \n\ \n")%i%i); } daedata << str(boost::format("\ \n\ \n\ \n\ \n\ \n\ \n\ ")); #ifdef HAVE_MKSTEMPS geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name); int fd = mkstemps(&geometry->filename[0],4); #else int fd = -1; for(int iter = 0; iter < 1000; ++iter) { geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_%d.dae")%name%rand()); if( !!std::ifstream(geometry->filename.c_str())) { fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL); if( fd != -1 ) { break; } } } if( fd == -1 ) { ROS_ERROR("failed to open geometry dae file %s",geometry->filename.c_str()); return geometry; } #endif //ROS_INFO("temp file: %s",geometry->filename.c_str()); std::string daedatastr = daedata.str(); if( (size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) { ROS_ERROR("failed to write to geometry dae file %s",geometry->filename.c_str()); } close(fd); _listTempFilenames.push_back(boost::shared_ptr(new UnlinkFilename(geometry->filename))); geometry->filename = std::string("file://") + geometry->filename; return geometry; } /// Extract Geometry and apply the transformations of the node /// \param pdomnode Node to extract the goemetry /// \param plink Link of the kinematics model void _ExtractGeometry(const domNodeRef pdomnode,std::list& listGeomProperties, const std::list& listAxisBindings, const Pose& tlink) { if( !pdomnode ) { return; } ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName())); // For all child nodes of pdomnode for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) { // check if contains a joint bool contains=false; FOREACHC(it,listAxisBindings) { // don't check ID's check if the reference is the same! if ( (pdomnode->getNode_array()[i]) == (it->visualnode)) { contains=true; break; } } if (contains) { continue; } _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink); // Plink stayes the same for all children // replace pdomnode by child = pdomnode->getNode_array()[i] // hope for the best! // put everything in a subroutine in order to process pdomnode too! } unsigned int nGeomBefore = listGeomProperties.size(); // #of Meshes already associated to this link // get the geometry for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) { domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom]; domGeometryRef domgeom = daeSafeCast (domigeom->getUrl().getElement()); if (!domgeom) { continue; } // Gets materials std::map mapmaterials; if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) { const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array(); for (size_t imat = 0; imat < matarray.getCount(); ++imat) { domMaterialRef pmat = daeSafeCast(matarray[imat]->getTarget().getElement()); if (!!pmat) { mapmaterials[matarray[imat]->getSymbol()] = pmat; } } } // Gets the geometry _ExtractGeometry(domgeom, mapmaterials, listGeomProperties); } std::list::iterator itgeom= listGeomProperties.begin(); for (unsigned int i=0; i< nGeomBefore; i++) { itgeom++; // change only the transformations of the newly found geometries. } boost::array tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)), _poseMult(_poseMult(_matrixFromPose(_poseInverse(_VisualRootOrigin)), _getNodeParentTransform(pdomnode)), _ExtractFullTransform(pdomnode))); Pose tnodegeom; Vector3 vscale(1,1,1); _decompose(tmnodegeom, tnodegeom, vscale); ROS_DEBUG_STREAM("tnodegeom: " << pdomnode->getName() << tnodegeom.position.x << " " << tnodegeom.position.y << " " << tnodegeom.position.z << " / " << tnodegeom.rotation.x << " " << tnodegeom.rotation.y << " " << tnodegeom.rotation.z << " " << tnodegeom.rotation.w); // std::stringstream ss; ss << "geom: "; // for(int i = 0; i < 4; ++i) { // ss << tmnodegeom[4*0+i] << " " << tmnodegeom[4*1+i] << " " << tmnodegeom[4*2+i] << " "; // } // ROS_INFO(ss.str().c_str()); // Switch between different type of geometry PRIMITIVES for (; itgeom != listGeomProperties.end(); itgeom++) { itgeom->_t = tnodegeom; switch (itgeom->type) { case GeomBox: itgeom->vGeomData.x *= vscale.x; itgeom->vGeomData.y *= vscale.y; itgeom->vGeomData.z *= vscale.z; break; case GeomSphere: { itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y)); break; } case GeomCylinder: itgeom->vGeomData.x *= std::max(vscale.x, vscale.y); itgeom->vGeomData.y *= vscale.z; break; case GeomTrimesh: for(size_t i = 0; i < itgeom->vertices.size(); ++i ) { itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]); } itgeom->_t = Pose(); // reset back to identity break; default: ROS_WARN_STREAM(str(boost::format("unknown geometry type: %d\n")%itgeom->type)); } } } /// Paint the Geometry with the color material /// \param pmat Material info of the COLLADA's model /// \param geom Geometry properties in OpenRAVE void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES& geom) { if( !!pmat && !!pmat->getInstance_effect() ) { domEffectRef peffect = daeSafeCast(pmat->getInstance_effect()->getUrl().getElement().cast()); if( !!peffect ) { domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID()))); if( !!pphong ) { if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) { domFx_color c = pphong->getAmbient()->getColor()->getValue(); geom.ambientColor.r = c[0]; geom.ambientColor.g = c[1]; geom.ambientColor.b = c[2]; geom.ambientColor.a = c[3]; } if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) { domFx_color c = pphong->getDiffuse()->getColor()->getValue(); geom.diffuseColor.r = c[0]; geom.diffuseColor.g = c[1]; geom.diffuseColor.b = c[2]; geom.diffuseColor.a = c[3]; } } } } } /// Extract the Geometry in TRIANGLES and adds it to OpenRave /// \param triRef Array of triangles of the COLLADA's model /// \param vertsRef Array of vertices of the COLLADA's model /// \param mapmaterials Materials applied to the geometry /// \param plink Link of the kinematics model bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) { if( !triRef ) { return false; } listGeomProperties.push_back(GEOMPROPERTIES()); GEOMPROPERTIES& geom = listGeomProperties.back(); geom.type = GeomTrimesh; // resolve the material and assign correct colors to the geometry if( !!triRef->getMaterial() ) { std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); if( itmat != mapmaterials.end() ) { _FillGeometryColor(itmat->second,geom); } } size_t triangleIndexStride = 0, vertexoffset = -1; domInput_local_offsetRef indexOffsetRef; for (unsigned int w=0; wgetInput_array().getCount(); w++) { size_t offset = triRef->getInput_array()[w]->getOffset(); daeString str = triRef->getInput_array()[w]->getSemantic(); if (!strcmp(str,"VERTEX")) { indexOffsetRef = triRef->getInput_array()[w]; vertexoffset = offset; } if (offset> triangleIndexStride) { triangleIndexStride = offset; } } triangleIndexStride++; const domList_of_uints& indexArray =triRef->getP()->getValue(); geom.indices.reserve(triRef->getCount()*3); geom.vertices.reserve(triRef->getCount()*3); for (size_t i=0; igetInput_array().getCount(); ++i) { domInput_localRef localRef = vertsRef->getInput_array()[i]; daeString str = localRef->getSemantic(); if ( strcmp(str,"POSITION") == 0 ) { const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); if( !node ) { continue; } double fUnitScale = _GetUnitScale(node); const domFloat_arrayRef flArray = node->getFloat_array(); if (!!flArray) { const domList_of_floats& listFloats = flArray->getValue(); int k=vertexoffset; int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor' for(size_t itri = 0; itri < triRef->getCount(); ++itri) { if(k+2*triangleIndexStride < indexArray.getCount() ) { for (int j=0; j<3; j++) { int index0 = indexArray.get(k)*vertexStride; domFloat fl0 = listFloats.get(index0); domFloat fl1 = listFloats.get(index0+1); domFloat fl2 = listFloats.get(index0+2); k+=triangleIndexStride; geom.indices.push_back(geom.vertices.size()); geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); } } } } else { ROS_WARN_STREAM("float array not defined!\n"); } break; } } if( geom.indices.size() != 3*triRef->getCount() ) { ROS_WARN_STREAM("triangles declares wrong count!\n"); } geom.InitCollisionMesh(); return true; } /// Extract the Geometry in TRIGLE FANS and adds it to OpenRave /// \param triRef Array of triangle fans of the COLLADA's model /// \param vertsRef Array of vertices of the COLLADA's model /// \param mapmaterials Materials applied to the geometry bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) { if( !triRef ) { return false; } listGeomProperties.push_back(GEOMPROPERTIES()); GEOMPROPERTIES& geom = listGeomProperties.back(); geom.type = GeomTrimesh; // resolve the material and assign correct colors to the geometry if( !!triRef->getMaterial() ) { std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); if( itmat != mapmaterials.end() ) { _FillGeometryColor(itmat->second,geom); } } size_t triangleIndexStride = 0, vertexoffset = -1; domInput_local_offsetRef indexOffsetRef; for (unsigned int w=0; wgetInput_array().getCount(); w++) { size_t offset = triRef->getInput_array()[w]->getOffset(); daeString str = triRef->getInput_array()[w]->getSemantic(); if (!strcmp(str,"VERTEX")) { indexOffsetRef = triRef->getInput_array()[w]; vertexoffset = offset; } if (offset> triangleIndexStride) { triangleIndexStride = offset; } } triangleIndexStride++; size_t primitivecount = triRef->getCount(); if( primitivecount > triRef->getP_array().getCount() ) { ROS_WARN_STREAM("trifans has incorrect count\n"); primitivecount = triRef->getP_array().getCount(); } for(size_t ip = 0; ip < primitivecount; ++ip) { domList_of_uints indexArray =triRef->getP_array()[ip]->getValue(); for (size_t i=0; igetInput_array().getCount(); ++i) { domInput_localRef localRef = vertsRef->getInput_array()[i]; daeString str = localRef->getSemantic(); if ( strcmp(str,"POSITION") == 0 ) { const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); if( !node ) { continue; } double fUnitScale = _GetUnitScale(node); const domFloat_arrayRef flArray = node->getFloat_array(); if (!!flArray) { const domList_of_floats& listFloats = flArray->getValue(); int k=vertexoffset; int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor' size_t usedindices = 3*(indexArray.getCount()-2); if( geom.indices.capacity() < geom.indices.size()+usedindices ) { geom.indices.reserve(geom.indices.size()+usedindices); } if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) { geom.vertices.reserve(geom.vertices.size()+indexArray.getCount()); } size_t startoffset = (int)geom.vertices.size(); while(k < (int)indexArray.getCount() ) { int index0 = indexArray.get(k)*vertexStride; domFloat fl0 = listFloats.get(index0); domFloat fl1 = listFloats.get(index0+1); domFloat fl2 = listFloats.get(index0+2); k+=triangleIndexStride; geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); } for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) { geom.indices.push_back(startoffset); geom.indices.push_back(ivert-1); geom.indices.push_back(ivert); } } else { ROS_WARN_STREAM("float array not defined!\n"); } break; } } } geom.InitCollisionMesh(); return false; } /// Extract the Geometry in TRIANGLE STRIPS and adds it to OpenRave /// \param triRef Array of Triangle Strips of the COLLADA's model /// \param vertsRef Array of vertices of the COLLADA's model /// \param mapmaterials Materials applied to the geometry bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) { if( !triRef ) { return false; } listGeomProperties.push_back(GEOMPROPERTIES()); GEOMPROPERTIES& geom = listGeomProperties.back(); geom.type = GeomTrimesh; // resolve the material and assign correct colors to the geometry if( !!triRef->getMaterial() ) { std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); if( itmat != mapmaterials.end() ) { _FillGeometryColor(itmat->second,geom); } } size_t triangleIndexStride = 0, vertexoffset = -1; domInput_local_offsetRef indexOffsetRef; for (unsigned int w=0; wgetInput_array().getCount(); w++) { size_t offset = triRef->getInput_array()[w]->getOffset(); daeString str = triRef->getInput_array()[w]->getSemantic(); if (!strcmp(str,"VERTEX")) { indexOffsetRef = triRef->getInput_array()[w]; vertexoffset = offset; } if (offset> triangleIndexStride) { triangleIndexStride = offset; } } triangleIndexStride++; size_t primitivecount = triRef->getCount(); if( primitivecount > triRef->getP_array().getCount() ) { ROS_WARN_STREAM("tristrips has incorrect count\n"); primitivecount = triRef->getP_array().getCount(); } for(size_t ip = 0; ip < primitivecount; ++ip) { domList_of_uints indexArray = triRef->getP_array()[ip]->getValue(); for (size_t i=0; igetInput_array().getCount(); ++i) { domInput_localRef localRef = vertsRef->getInput_array()[i]; daeString str = localRef->getSemantic(); if ( strcmp(str,"POSITION") == 0 ) { const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); if( !node ) { continue; } double fUnitScale = _GetUnitScale(node); const domFloat_arrayRef flArray = node->getFloat_array(); if (!!flArray) { const domList_of_floats& listFloats = flArray->getValue(); int k=vertexoffset; int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor' size_t usedindices = indexArray.getCount()-2; if( geom.indices.capacity() < geom.indices.size()+usedindices ) { geom.indices.reserve(geom.indices.size()+usedindices); } if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) { geom.vertices.reserve(geom.vertices.size()+indexArray.getCount()); } size_t startoffset = (int)geom.vertices.size(); while(k < (int)indexArray.getCount() ) { int index0 = indexArray.get(k)*vertexStride; domFloat fl0 = listFloats.get(index0); domFloat fl1 = listFloats.get(index0+1); domFloat fl2 = listFloats.get(index0+2); k+=triangleIndexStride; geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); } bool bFlip = false; for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) { geom.indices.push_back(ivert-2); geom.indices.push_back(bFlip ? ivert : ivert-1); geom.indices.push_back(bFlip ? ivert-1 : ivert); bFlip = !bFlip; } } else { ROS_WARN_STREAM("float array not defined!\n"); } break; } } } geom.InitCollisionMesh(); return false; } /// Extract the Geometry in TRIANGLE STRIPS and adds it to OpenRave /// \param triRef Array of Triangle Strips of the COLLADA's model /// \param vertsRef Array of vertices of the COLLADA's model /// \param mapmaterials Materials applied to the geometry bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) { if( !triRef ) { return false; } listGeomProperties.push_back(GEOMPROPERTIES()); GEOMPROPERTIES& geom = listGeomProperties.back(); geom.type = GeomTrimesh; // resolve the material and assign correct colors to the geometry if( !!triRef->getMaterial() ) { std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); if( itmat != mapmaterials.end() ) { _FillGeometryColor(itmat->second,geom); } } size_t triangleIndexStride = 0,vertexoffset = -1; domInput_local_offsetRef indexOffsetRef; for (unsigned int w=0; wgetInput_array().getCount(); w++) { size_t offset = triRef->getInput_array()[w]->getOffset(); daeString str = triRef->getInput_array()[w]->getSemantic(); if (!strcmp(str,"VERTEX")) { indexOffsetRef = triRef->getInput_array()[w]; vertexoffset = offset; } if (offset> triangleIndexStride) { triangleIndexStride = offset; } } triangleIndexStride++; const domList_of_uints& indexArray =triRef->getP()->getValue(); for (size_t i=0; igetInput_array().getCount(); ++i) { domInput_localRef localRef = vertsRef->getInput_array()[i]; daeString str = localRef->getSemantic(); if ( strcmp(str,"POSITION") == 0 ) { const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); if( !node ) { continue; } double fUnitScale = _GetUnitScale(node); const domFloat_arrayRef flArray = node->getFloat_array(); if (!!flArray) { const domList_of_floats& listFloats = flArray->getValue(); size_t k=vertexoffset; int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor' for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) { size_t numverts = triRef->getVcount()->getValue()[ipoly]; if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) { size_t startoffset = geom.vertices.size(); for (size_t j=0; j& mapmaterials, std::list& listGeomProperties) { if( !geom ) { return false; } std::vector vconvexhull; if (geom->getMesh()) { const domMeshRef meshRef = geom->getMesh(); for (size_t tg = 0; tggetTriangles_array().getCount(); tg++) { _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); } for (size_t tg = 0; tggetTrifans_array().getCount(); tg++) { _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); } for (size_t tg = 0; tggetTristrips_array().getCount(); tg++) { _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); } for (size_t tg = 0; tggetPolylist_array().getCount(); tg++) { _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); } if( meshRef->getPolygons_array().getCount()> 0 ) { ROS_WARN_STREAM("openrave does not support collada polygons\n"); } // if( alltrimesh.vertices.size() == 0 ) { // const domVerticesRef vertsRef = meshRef->getVertices(); // for (size_t i=0;igetInput_array().getCount();i++) { // domInput_localRef localRef = vertsRef->getInput_array()[i]; // daeString str = localRef->getSemantic(); // if ( strcmp(str,"POSITION") == 0 ) { // const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); // if( !node ) // continue; // double fUnitScale = _GetUnitScale(node); // const domFloat_arrayRef flArray = node->getFloat_array(); // if (!!flArray) { // const domList_of_floats& listFloats = flArray->getValue(); // int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' // vconvexhull.reserve(vconvexhull.size()+listFloats.getCount()); // for (size_t vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride) { // //btVector3 verts[3]; // domFloat fl0 = listFloats.get(vertIndex); // domFloat fl1 = listFloats.get(vertIndex+1); // domFloat fl2 = listFloats.get(vertIndex+2); // vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); // } // } // } // } // // _computeConvexHull(vconvexhull,alltrimesh); // } return true; } else if (geom->getConvex_mesh()) { { const domConvex_meshRef convexRef = geom->getConvex_mesh(); daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement(); if ( !!otherElemRef ) { domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef; ROS_WARN_STREAM( "otherLinked\n"); } else { ROS_WARN("convexMesh polyCount = %d\n",(int)convexRef->getPolygons_array().getCount()); ROS_WARN("convexMesh triCount = %d\n",(int)convexRef->getTriangles_array().getCount()); } } const domConvex_meshRef convexRef = geom->getConvex_mesh(); //daeString urlref = convexRef->getConvex_hull_of().getURI(); daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI(); if (urlref2) { daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement(); // Load all the geometry libraries for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) { domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i]; for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) { domGeometryRef lib = libgeom->getGeometry_array()[i]; if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2 //found convex_hull geometry domMesh *meshElement = lib->getMesh(); //linkedGeom->getMesh(); if (meshElement) { const domVerticesRef vertsRef = meshElement->getVertices(); for (size_t i=0; igetInput_array().getCount(); i++) { domInput_localRef localRef = vertsRef->getInput_array()[i]; daeString str = localRef->getSemantic(); if ( strcmp(str,"POSITION") == 0) { const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); if( !node ) { continue; } double fUnitScale = _GetUnitScale(node); const domFloat_arrayRef flArray = node->getFloat_array(); if (!!flArray) { vconvexhull.reserve(vconvexhull.size()+flArray->getCount()); const domList_of_floats& listFloats = flArray->getValue(); for (size_t k=0; k+2getCount(); k+=3) { domFloat fl0 = listFloats.get(k); domFloat fl1 = listFloats.get(k+1); domFloat fl2 = listFloats.get(k+2); vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); } } } } } } } } } else { //no getConvex_hull_of but direct vertices const domVerticesRef vertsRef = convexRef->getVertices(); for (size_t i=0; igetInput_array().getCount(); i++) { domInput_localRef localRef = vertsRef->getInput_array()[i]; daeString str = localRef->getSemantic(); if ( strcmp(str,"POSITION") == 0 ) { const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); if( !node ) { continue; } double fUnitScale = _GetUnitScale(node); const domFloat_arrayRef flArray = node->getFloat_array(); if (!!flArray) { const domList_of_floats& listFloats = flArray->getValue(); vconvexhull.reserve(vconvexhull.size()+flArray->getCount()); for (size_t k=0; k+2getCount(); k+=3) { domFloat fl0 = listFloats.get(k); domFloat fl1 = listFloats.get(k+1); domFloat fl2 = listFloats.get(k+2); vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); } } } } } if( vconvexhull.size()> 0 ) { listGeomProperties.push_back(GEOMPROPERTIES()); GEOMPROPERTIES& geom = listGeomProperties.back(); geom.type = GeomTrimesh; //_computeConvexHull(vconvexhull,trimesh); geom.InitCollisionMesh(); } return true; } return false; } /// \brief extract the robot actuators void _ExtractRobotAttachedActuators(const domArticulated_systemRef as) { // over write joint parameters by elements in instance_actuator for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) { domExtraRef pextra = as->getExtra_array()[ie]; if( strcmp(pextra->getType(), "attach_actuator") == 0 ) { //std::string aname = pextra->getAttribute("name"); domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array()); if( !!tec ) { urdf::JointSharedPtr pjoint; daeElementRef domactuator; { daeElementRef bact = tec->getChild("bind_actuator"); pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as); if (!pjoint) continue; } { daeElementRef iact = tec->getChild("instance_actuator"); if(!iact) continue; std::string instance_url = iact->getAttribute("url"); domactuator = daeURI(*iact, instance_url).getElement(); if(!domactuator) continue; } daeElement *nom_torque = domactuator->getChild("nominal_torque"); if ( !! nom_torque ) { if( !! pjoint->limits ) { pjoint->limits->effort = boost::lexical_cast(nom_torque->getCharData()); ROS_DEBUG("effort limit at joint (%s) is over written by %f", pjoint->name.c_str(), pjoint->limits->effort); } } } } } } /// \brief extract the robot manipulators void _ExtractRobotManipulators(const domArticulated_systemRef as) { ROS_DEBUG("collada manipulators not supported yet"); } /// \brief Extract Sensors attached to a Robot void _ExtractRobotAttachedSensors(const domArticulated_systemRef as) { ROS_DEBUG("collada sensors not supported yet"); } inline daeElementRef _getElementFromUrl(const daeURI &uri) { return daeStandardURIResolver(*_collada).resolveElement(uri); } static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent) { if( !!paddr->getSIDREF() ) { return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt; } if (!!paddr->getParam()) { return searchBinding(paddr->getParam()->getValue(),parent); } return NULL; } /// Search a given parameter reference and stores the new reference to search. /// \param ref the reference name to search /// \param parent The array of parameter where the method searchs. static daeElement* searchBinding(daeString ref, daeElementRef parent) { if( !parent ) { return NULL; } daeElement* pelt = NULL; domKinematics_sceneRef kscene = daeSafeCast(parent.cast()); if( !!kscene ) { pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array()); if( !!pelt ) { return pelt; } return searchBindingArray(ref,kscene->getInstance_kinematics_model_array()); } domArticulated_systemRef articulated_system = daeSafeCast(parent.cast()); if( !!articulated_system ) { if( !!articulated_system->getKinematics() ) { pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array()); if( !!pelt ) { return pelt; } } if( !!articulated_system->getMotion() ) { return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system()); } return NULL; } // try to get a bind array daeElementRef pbindelt; const domKinematics_bind_Array* pbindarray = NULL; const domKinematics_newparam_Array* pnewparamarray = NULL; domInstance_articulated_systemRef ias = daeSafeCast(parent.cast()); if( !!ias ) { pbindarray = &ias->getBind_array(); pbindelt = ias->getUrl().getElement(); pnewparamarray = &ias->getNewparam_array(); } if( !pbindarray || !pbindelt ) { domInstance_kinematics_modelRef ikm = daeSafeCast(parent.cast()); if( !!ikm ) { pbindarray = &ikm->getBind_array(); pbindelt = ikm->getUrl().getElement(); pnewparamarray = &ikm->getNewparam_array(); } } if( !!pbindarray && !!pbindelt ) { for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) { domKinematics_bindRef pbind = (*pbindarray)[ibind]; if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) { // found a match if( !!pbind->getParam() ) { return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt; } else if( !!pbind->getSIDREF() ) { return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt; } } } for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) { domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam]; if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) { if( !!newparam->getSIDREF() ) { // can only bind with SIDREF return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt; } ROS_WARN_STREAM(str(boost::format("newparam sid=%s does not have SIDREF\n")%newparam->getSid())); } } } ROS_WARN_STREAM(str(boost::format("failed to get binding '%s' for element: %s\n")%ref%parent->getElementName())); return NULL; } static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray) { for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) { daeElement* pelt = searchBinding(ref,paramArray[iikm].cast()); if( !!pelt ) { return pelt; } } return NULL; } static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray) { for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) { daeElement* pelt = searchBinding(ref,paramArray[iikm].cast()); if( !!pelt ) { return pelt; } } return NULL; } template static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) { if( !!paddr->getBool() ) { return paddr->getBool()->getValue(); } if( !paddr->getParam() ) { ROS_WARN_STREAM("param not specified, setting to 0\n"); return false; } for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) { domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam]; if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) { if( !!pnewparam->getBool() ) { return pnewparam->getBool()->getValue(); } else if( !!pnewparam->getSIDREF() ) { domKinematics_newparam::domBoolRef ptarget = daeSafeCast(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt); if( !ptarget ) { ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID()); continue; } return ptarget->getValue(); } } } ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue())); return false; } template static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) { if( !!paddr->getFloat() ) { return paddr->getFloat()->getValue(); } if( !paddr->getParam() ) { ROS_WARN_STREAM("param not specified, setting to 0\n"); return 0; } for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) { domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam]; if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) { if( !!pnewparam->getFloat() ) { return pnewparam->getFloat()->getValue(); } else if( !!pnewparam->getSIDREF() ) { domKinematics_newparam::domFloatRef ptarget = daeSafeCast(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt); if( !ptarget ) { ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID()); continue; } return ptarget->getValue(); } } } ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue())); return 0; } static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f) { daeElement* pfloat = pcommon->getChild("float"); if( !!pfloat ) { std::stringstream sfloat(pfloat->getCharData()); sfloat >> f; return !!sfloat; } daeElement* pparam = pcommon->getChild("param"); if( !!pparam ) { if( pparam->hasAttribute("ref") ) { ROS_WARN_STREAM("cannot process param ref\n"); } else { daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt; if( !!pelt ) { ROS_WARN_STREAM(str(boost::format("found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData())); } } } return false; } static boost::array _matrixIdentity() { boost::array m = {{1,0,0,0,0,1,0,0,0,0,1,0}}; return m; }; /// Gets all transformations applied to the node static boost::array _getTransform(daeElementRef pelt) { boost::array m = _matrixIdentity(); domRotateRef protate = daeSafeCast(pelt); if( !!protate ) { m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (double)(protate->getValue()[3]*(M_PI/180.0))); return m; } domTranslateRef ptrans = daeSafeCast(pelt); if( !!ptrans ) { double scale = _GetUnitScale(pelt); m[3] = ptrans->getValue()[0]*scale; m[7] = ptrans->getValue()[1]*scale; m[11] = ptrans->getValue()[2]*scale; return m; } domMatrixRef pmat = daeSafeCast(pelt); if( !!pmat ) { double scale = _GetUnitScale(pelt); for(int i = 0; i < 3; ++i) { m[4*i+0] = pmat->getValue()[4*i+0]; m[4*i+1] = pmat->getValue()[4*i+1]; m[4*i+2] = pmat->getValue()[4*i+2]; m[4*i+3] = pmat->getValue()[4*i+3]*scale; } return m; } domScaleRef pscale = daeSafeCast(pelt); if( !!pscale ) { m[0] = pscale->getValue()[0]; m[4*1+1] = pscale->getValue()[1]; m[4*2+2] = pscale->getValue()[2]; return m; } domLookatRef pcamera = daeSafeCast(pelt); if( pelt->typeID() == domLookat::ID() ) { ROS_ERROR_STREAM("look at transform not implemented\n"); return m; } domSkewRef pskew = daeSafeCast(pelt); if( !!pskew ) { ROS_ERROR_STREAM("skew transform not implemented\n"); } return m; } /// Travels recursively the node parents of the given one /// to extract the Transform arrays that affects the node given template static boost::array _getNodeParentTransform(const T pelt) { domNodeRef pnode = daeSafeCast(pelt->getParent()); if( !pnode ) { return _matrixIdentity(); } return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode)); } /// \brief Travel by the transformation array and calls the _getTransform method template static boost::array _ExtractFullTransform(const T pelt) { boost::array t = _matrixIdentity(); for(size_t i = 0; i < pelt->getContents().getCount(); ++i) { t = _poseMult(t, _getTransform(pelt->getContents()[i])); } return t; } /// \brief Travel by the transformation array and calls the _getTransform method template static boost::array _ExtractFullTransformFromChildren(const T pelt) { boost::array t = _matrixIdentity(); daeTArray children; pelt->getChildren(children); for(size_t i = 0; i < children.getCount(); ++i) { t = _poseMult(t, _getTransform(children[i])); } return t; } // decompose a matrix into a scale and rigid transform (necessary for model scales) void _decompose(const boost::array& tm, Pose& tout, Vector3& vscale) { vscale.x = 1; vscale.y = 1; vscale.z = 1; tout = _poseFromMatrix(tm); } virtual void handleError( daeString msg ) { ROS_ERROR("COLLADA error: %s\n", msg); } virtual void handleWarning( daeString msg ) { ROS_WARN("COLLADA warning: %s\n", msg); } inline static double _GetUnitScale(daeElement* pelt) { return ((USERDATA*)pelt->getUserData())->scale; } domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr) { for(size_t i = 0; i < arr.getCount(); ++i) { if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) { return arr[i]; } } return domTechniqueRef(); } /// \brief returns an openrave interface type from the extra array boost::shared_ptr _ExtractInterfaceType(const domExtra_Array& arr) { for(size_t i = 0; i < arr.getCount(); ++i) { if( strcmp(arr[i]->getType(),"interface_type") == 0 ) { domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array()); if( !!tec ) { daeElement* ptype = tec->getChild("interface"); if( !!ptype ) { return boost::shared_ptr(new std::string(ptype->getCharData())); } } } } return boost::shared_ptr(); } std::string _ExtractLinkName(domLinkRef pdomlink) { std::string linkname; if( !!pdomlink ) { if( !!pdomlink->getName() ) { linkname = pdomlink->getName(); } if( linkname.size() == 0 && !!pdomlink->getID() ) { linkname = pdomlink->getID(); } } return linkname; } bool _checkMathML(daeElementRef pelt,const std::string& type) { if( pelt->getElementName()==type ) { return true; } // check the substring after ':' std::string name = pelt->getElementName(); std::size_t pos = name.find_last_of(':'); if( pos == std::string::npos ) { return false; } return name.substr(pos+1)==type; } urdf::JointSharedPtr _getJointFromRef(xsToken targetref, daeElementRef peltref) { daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt; domJointRef pdomjoint = daeSafeCast (peltjoint); if (!pdomjoint) { domInstance_jointRef pdomijoint = daeSafeCast (peltjoint); if (!!pdomijoint) { pdomjoint = daeSafeCast (pdomijoint->getUrl().getElement().cast()); } } if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) { ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref)); return urdf::JointSharedPtr(); } urdf::JointSharedPtr pjoint; std::string name(pdomjoint->getName()); if (_model->joints_.find(name) == _model->joints_.end()) { pjoint.reset(); } else { pjoint = _model->joints_.find(name)->second; } if(!pjoint) { ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName())); } return pjoint; } /// \brief go through all kinematics binds to get a kinematics/visual pair /// \param kiscene instance of one kinematics scene, binds the kinematic and visual models /// \param bindings the extracted bindings static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings) { domKinematics_sceneRef kscene = daeSafeCast (kiscene->getUrl().getElement().cast()); if (!kscene) { return; } for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) { domArticulated_systemRef articulated_system; // if filled, contains robot-specific information, so create a robot domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel]; if (!kbindmodel->getNode()) { ROS_WARN_STREAM("do not support kinematics models without references to nodes\n"); continue; } // visual information domNodeRef node = daeSafeCast(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt); if (!node) { ROS_WARN_STREAM(str(boost::format("bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode())); continue; } // kinematics information daeElement* pelt = searchBinding(kbindmodel,kscene); domInstance_kinematics_modelRef kimodel = daeSafeCast(pelt); if (!kimodel) { if( !pelt ) { ROS_WARN_STREAM("bind_kinematics_model does not reference element\n"); } else { ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName())); } continue; } bindings.listKinematicsVisualBindings.push_back(std::make_pair(node,kimodel)); } // axis info for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) { domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint]; daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt; if (!pjtarget) { ROS_ERROR_STREAM(str(boost::format("Target Node %s NOT found!!!\n")%bindjoint->getTarget())); continue; } daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene); domAxis_constraintRef pjointaxis = daeSafeCast(pelt); if (!pjointaxis) { continue; } bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL)); } } static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings) { for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) { domPhysics_sceneRef pscene = daeSafeCast(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast()); for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) { domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel]; domPhysics_modelRef pmodel = daeSafeCast (ipmodel->getUrl().getElement().cast()); domNodeRef nodephysicsoffset = daeSafeCast(ipmodel->getParent().getElement().cast()); for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) { LinkBinding lb; lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody]; lb.node = daeSafeCast(lb.irigidbody->getTarget().getElement().cast()); lb.rigidbody = daeSafeCast(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt); lb.nodephysicsoffset = nodephysicsoffset; if( !!lb.rigidbody && !!lb.node ) { bindings.listLinkBindings.push_back(lb); } } } } } size_t _countChildren(daeElement* pelt) { size_t c = 1; daeTArray children; pelt->getChildren(children); for (size_t i = 0; i < children.getCount(); ++i) { c += _countChildren(children[i]); } return c; } void _processUserData(daeElement* pelt, double scale) { // getChild could be optimized since asset tag is supposed to appear as the first element domAssetRef passet = daeSafeCast (pelt->getChild("asset")); if (!!passet && !!passet->getUnit()) { scale = passet->getUnit()->getMeter(); } _vuserdata.push_back(USERDATA(scale)); pelt->setUserData(&_vuserdata.back()); daeTArray children; pelt->getChildren(children); for (size_t i = 0; i < children.getCount(); ++i) { if (children[i] != passet) { _processUserData(children[i], scale); } } } USERDATA* _getUserData(daeElement* pelt) { BOOST_ASSERT(!!pelt); void* p = pelt->getUserData(); BOOST_ASSERT(!!p); return (USERDATA*)p; } // // openrave math functions (from geometry.h) // static Vector3 _poseMult(const Pose& p, const 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; 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 Vector3 _poseMult(const boost::array& m, const Vector3& v) { Vector3 vnew; vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3]; vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3]; vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3]; return vnew; } static boost::array _poseMult(const boost::array& m0, const boost::array& m1) { boost::array mres; mres[0*4+0] = m0[0*4+0]*m1[0*4+0]+m0[0*4+1]*m1[1*4+0]+m0[0*4+2]*m1[2*4+0]; mres[0*4+1] = m0[0*4+0]*m1[0*4+1]+m0[0*4+1]*m1[1*4+1]+m0[0*4+2]*m1[2*4+1]; mres[0*4+2] = m0[0*4+0]*m1[0*4+2]+m0[0*4+1]*m1[1*4+2]+m0[0*4+2]*m1[2*4+2]; mres[1*4+0] = m0[1*4+0]*m1[0*4+0]+m0[1*4+1]*m1[1*4+0]+m0[1*4+2]*m1[2*4+0]; mres[1*4+1] = m0[1*4+0]*m1[0*4+1]+m0[1*4+1]*m1[1*4+1]+m0[1*4+2]*m1[2*4+1]; mres[1*4+2] = m0[1*4+0]*m1[0*4+2]+m0[1*4+1]*m1[1*4+2]+m0[1*4+2]*m1[2*4+2]; mres[2*4+0] = m0[2*4+0]*m1[0*4+0]+m0[2*4+1]*m1[1*4+0]+m0[2*4+2]*m1[2*4+0]; mres[2*4+1] = m0[2*4+0]*m1[0*4+1]+m0[2*4+1]*m1[1*4+1]+m0[2*4+2]*m1[2*4+1]; mres[2*4+2] = m0[2*4+0]*m1[0*4+2]+m0[2*4+1]*m1[1*4+2]+m0[2*4+2]*m1[2*4+2]; mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3]; mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7]; mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11]; return mres; } static Pose _poseMult(const Pose& p0, const Pose& p1) { Pose p; p.position = _poseMult(p0,p1.position); p.rotation = _quatMult(p0.rotation,p1.rotation); return p; } static Pose _poseInverse(const Pose& p) { 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; Vector3 t = _poseMult(pinv,p.position); pinv.position.x = -t.x; pinv.position.y = -t.y; pinv.position.z = -t.z; return pinv; } static Rotation _quatMult(const Rotation& quat0, const Rotation& quat1) { Rotation q; q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y; q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z; q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x; q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z; double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w); // don't touch the divides q.x /= fnorm; q.y /= fnorm; q.z /= fnorm; q.w /= fnorm; return q; } static boost::array _matrixFromAxisAngle(const Vector3& axis, double angle) { return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle)); } static boost::array _matrixFromQuat(const Rotation& quat) { boost::array m; double qq1 = 2*quat.x*quat.x; double qq2 = 2*quat.y*quat.y; double qq3 = 2*quat.z*quat.z; m[4*0+0] = 1 - qq2 - qq3; m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z); m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y); m[4*0+3] = 0; m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z); m[4*1+1] = 1 - qq1 - qq3; m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x); m[4*1+3] = 0; m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y); m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x); m[4*2+2] = 1 - qq1 - qq2; m[4*2+3] = 0; return m; } static Pose _poseFromMatrix(const boost::array& m) { Pose t; t.rotation = _quatFromMatrix(m); t.position.x = m[3]; t.position.y = m[7]; t.position.z = m[11]; return t; } static boost::array _matrixFromPose(const Pose& t) { boost::array m = _matrixFromQuat(t.rotation); m[3] = t.position.x; m[7] = t.position.y; m[11] = t.position.z; return m; } static Rotation _quatFromAxisAngle(double x, double y, double z, double angle) { Rotation q; double axislen = std::sqrt(x*x+y*y+z*z); if( axislen == 0 ) { return q; } angle *= 0.5; double sang = std::sin(angle)/axislen; q.w = std::cos(angle); q.x = x*sang; q.y = y*sang; q.z = z*sang; return q; } static Rotation _quatFromMatrix(const boost::array& mat) { Rotation rot; double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2]; if (tr >= 0) { rot.w = tr + 1; rot.x = (mat[4*2+1] - mat[4*1+2]); rot.y = (mat[4*0+2] - mat[4*2+0]); rot.z = (mat[4*1+0] - mat[4*0+1]); } else { // find the largest diagonal element and jump to the appropriate case if (mat[4*1+1] > mat[4*0+0]) { if (mat[4*2+2] > mat[4*1+1]) { rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1; rot.x = (mat[4*2+0] + mat[4*0+2]); rot.y = (mat[4*1+2] + mat[4*2+1]); rot.w = (mat[4*1+0] - mat[4*0+1]); } else { rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1; rot.z = (mat[4*1+2] + mat[4*2+1]); rot.x = (mat[4*0+1] + mat[4*1+0]); rot.w = (mat[4*0+2] - mat[4*2+0]); } } else if (mat[4*2+2] > mat[4*0+0]) { rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1; rot.x = (mat[4*2+0] + mat[4*0+2]); rot.y = (mat[4*1+2] + mat[4*2+1]); rot.w = (mat[4*1+0] - mat[4*0+1]); } else { rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1; rot.y = (mat[4*0+1] + mat[4*1+0]); rot.z = (mat[4*2+0] + mat[4*0+2]); rot.w = (mat[4*2+1] - mat[4*1+2]); } } double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w); // don't touch the divides rot.x /= fnorm; rot.y /= fnorm; rot.z /= fnorm; rot.w /= fnorm; return rot; } static double _dot3(const Vector3& v0, const Vector3& v1) { return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z; } static Vector3 _cross3(const Vector3& v0, const Vector3& v1) { Vector3 v; v.x = v0.y * v1.z - v0.z * v1.y; v.y = v0.z * v1.x - v0.x * v1.z; v.z = v0.x * v1.y - v0.y * v1.x; return v; } static Vector3 _sub3(const Vector3& v0, const Vector3& v1) { Vector3 v; v.x = v0.x-v1.x; v.y = v0.y-v1.y; v.z = v0.z-v1.z; return v; } static Vector3 _add3(const Vector3& v0, const Vector3& v1) { Vector3 v; v.x = v0.x+v1.x; v.y = v0.y+v1.y; v.z = v0.z+v1.z; return v; } static Vector3 _normalize3(const Vector3& v0) { Vector3 v; double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z); v.x = v0.x/norm; v.y = v0.y/norm; v.z = v0.z/norm; return v; } boost::shared_ptr _collada; domCOLLADA* _dom; std::vector _vuserdata; // all userdata int _nGlobalSensorId, _nGlobalManipulatorId; std::string _filename; std::string _resourcedir; urdf::ModelInterfaceSharedPtr _model; Pose _RootOrigin; Pose _VisualRootOrigin; }; urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_str) { urdf::ModelInterfaceSharedPtr model(new ModelInterface); ColladaModelReader reader(model); if (!reader.InitFromData(xml_str)) model.reset(); return model; } } collada_urdf-1.12.13/collada_parser/src/collada_parser_plugin.cpp000066400000000000000000000041351370336760200251070ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2013, 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: * * * Redistributions 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: Ioan Sucan */ #include "collada_parser/collada_parser_plugin.h" #include "collada_parser/collada_parser.h" #include urdf::ModelInterfaceSharedPtr urdf::ColladaURDFParser::parse(const std::string &xml_string) { return urdf::parseCollada(xml_string); } CLASS_LOADER_REGISTER_CLASS(urdf::ColladaURDFParser, urdf::URDFParser) collada_urdf-1.12.13/collada_urdf/000077500000000000000000000000001370336760200167245ustar00rootroot00000000000000collada_urdf-1.12.13/collada_urdf/CHANGELOG.rst000066400000000000000000000120551370336760200207500ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package collada_urdf ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.12.13 (2020-07-14) -------------------- * Update to newer CMake to get rid of CMP0048 warning (`#38 `_) * Enable to output transmission_interface instead of pr2_mechanism_model (`#35 `_) * Contributors: Chris Lalancette, Shun Hasegawa 1.12.12 (2018-05-08) -------------------- 1.12.11 (2018-04-17) -------------------- * Collada cleanup dependencies (`#26 `_) * update links now that this is in its own repo * Switch to using Eigen for Quaternion and Matrix. (`#21 `_) * add relicensing comment (`#19 `_) * remove unused tinyxml from cmakelists (`#15 `_) * Contributors: Chris Lalancette, Mikael Arguedas, Rosen Diankov 1.12.10 (2017-05-04) -------------------- * Moved collada_parser and collada_urdf to new repository 1.12.9 (2017-04-26) ------------------- 1.12.8 (2017-03-27) ------------------- * Remove old gazebo settings. Based on an initial patch from YoheiKakiuchi, just totally remove old Gazebo 1.0 settings, as they are never used and almost certainly will never be used. * add Chris and Shane as maintainers (`#184 `_) * Do a few cleanup tasks in collada_urdf (`#183 `_) * Style cleanup within collada_urdf. No functional change, just style. * Make sure to quit out of urdf_to_collada when invalid file is found. Otherwise, we'll just end up segfaulting later on. * Re-enable one of the collada-urdf tests. In point of fact, we delete the rest of the tests because they don't make much sense anymore. Just enable this one for now; we'll enable further ones in the future. * Add in another test for collada_urdf. * remove divide by 2 when writing boxes to collada format (`#133 `_) * Contributors: Chris Lalancette, Jackie Kay, William Woodall 1.12.7 (2017-01-26) ------------------- 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) * Contributors: Jochen Sprickerhof 1.12.5 (2016-10-27) ------------------- 1.12.4 (2016-08-23) ------------------- * Use the C++11 standard (`#145 `_) * Contributors: William Woodall 1.12.3 (2016-06-10) ------------------- 1.12.2 (2016-04-12) ------------------- 1.12.1 (2016-04-10) ------------------- 1.11.8 (2015-09-11) ------------------- * Removed pcre hack for newer released collada-dom. * Contributors: Kei Okada 1.11.7 (2015-04-22) ------------------- * Fixed `#89 `_ Accomplished by loading libpcrecpp before collada-dom. * Contributors: Kei Okada, William Woodall 1.11.6 (2014-11-30) ------------------- 1.11.5 (2014-07-24) ------------------- 1.11.4 (2014-07-07) ------------------- * moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633 * Fix clash with assimp 3.1 in CMake. * Contributors: Benjamin Chrétien, Tully Foote 1.11.3 (2014-06-24) ------------------- * Merge pull request `#69 `_ from YoheiKakiuchi/indigo-devel-store-original-mesh-name storing original mesh file name and location * storing original mesh file name and location * Contributors: Ioan A Sucan, YoheiKakiuchi 1.11.2 (2014-03-22) ------------------- * use new urdfdom_headers API * Contributors: Ioan Sucan 1.11.1 (2014-03-20) ------------------- * Use assimp-dev dep for building * Contributors: Scott K Logan 1.11.0 (2014-02-21) ------------------- * Use VERSION_LESS instead of STRLESS The version comparison routines were added in cmake 2.8.0 * Fix export API detection (for assimp < 2.0.885) It looks like this API was added in Assimp 2.0.885: https://github.com/assimp/assimp/commit/ae23c03bd9a0b5f1227dc0042fd98f7206c770a8 * Invert Assimp version detect logic for greater accuracy * Updated Assimp defines to be more flexible This commit is a follow-up to 85b20197671e142044e471df603debd0faf08baf Why was export.h removed from assimp < 3.0.0? * Better feature detection for assimp version The unified headers were introduced in Assimp 2.0.1150, so checking for Assimp 3.0.0 is not quite the best solution. See https://github.com/assimp/assimp/commit/6fa251c2f2e7a142bb861227dce0c26362927fbc * Contributors: Scott K Logan 1.10.18 (2013-12-04) -------------------- * add DEPENDS for kdl_parser * Contributors: Ioan Sucan 1.10.16 (2013-11-18) -------------------- * check for CATKIN_ENABLE_TESTING * fix for compiling collada_to_urdf, add dependency to tf * add collada_to_urdf.cpp for converting collada file to urdf file 1.10.15 (2013-08-17) -------------------- * fix `#30 `_ collada_urdf-1.12.13/collada_urdf/CMakeLists.txt000066400000000000000000000065341370336760200214740ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(collada_urdf) find_package(catkin REQUIRED COMPONENTS angles cmake_modules collada_parser geometric_shapes resource_retriever rosconsole urdf) find_package(urdfdom_headers REQUIRED) include(CheckCXXCompilerFlag) check_cxx_compiler_flag(-std=c++11 HAS_STD_CPP11_FLAG) if(HAS_STD_CPP11_FLAG) add_compile_options(-std=c++11) endif() include_directories(include) find_package(assimp QUIET) if ( NOT ASSIMP_FOUND ) find_package(Assimp QUIET) if ( NOT ASSIMP_FOUND ) find_package(PkgConfig REQUIRED) pkg_check_modules(ASSIMP assimp) endif() endif() if( ASSIMP_FOUND ) if( NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150" ) add_definitions(-DASSIMP_UNIFIED_HEADER_NAMES) endif() if( NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.885" ) add_definitions(-DASSIMP_EXPORT_API) endif() include_directories(${ASSIMP_INCLUDE_DIRS}) link_directories(${ASSIMP_LIBRARY_DIRS}) else() message(STATUS "could not find assimp (perhaps available thorugh ROS package?), so assuming assimp v2") set(ASSIMP_LIBRARIES assimp) set(ASSIMP_LIBRARY_DIRS) set(ASSIMP_CXX_FLAGS) set(ASSIMP_CFLAGS_OTHER) set(ASSIMP_LINK_FLAGS) set(ASSIMP_INCLUDE_DIRS) endif() # Note: assimp 3.1 overwrites CMake Boost variables, so we need to check for # Boost after assimp. find_package(Boost REQUIRED COMPONENTS system filesystem program_options) include_directories(${Boost_INCLUDE_DIR}) find_package(Eigen3 REQUIRED) find_package(COLLADA_DOM 2.3 COMPONENTS 1.5) if( COLLADA_DOM_FOUND ) include_directories(${COLLADA_DOM_INCLUDE_DIRS}) link_directories(${COLLADA_DOM_LIBRARY_DIRS}) endif() catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include CATKIN_DEPENDS collada_parser geometric_shapes resource_retriever rosconsole urdf DEPENDS ASSIMP COLLADA_DOM urdfdom_headers ) include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) add_library(${PROJECT_NAME} src/collada_urdf.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES} ${ASSIMP_LIBRARIES}) set_target_properties(${PROJECT_NAME} PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}") set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}") add_executable(urdf_to_collada src/urdf_to_collada.cpp) target_link_libraries(urdf_to_collada ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES} ${PROJECT_NAME}) add_executable(collada_to_urdf src/collada_to_urdf.cpp) target_link_libraries(collada_to_urdf ${ASSIMP_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(collada_to_urdf PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}") set_target_properties(collada_to_urdf PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}") if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_collada_urdf test/test_collada_urdf.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) target_link_libraries(test_collada_urdf ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES}) endif() install(TARGETS ${PROJECT_NAME} urdf_to_collada collada_to_urdf LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) collada_urdf-1.12.13/collada_urdf/include/000077500000000000000000000000001370336760200203475ustar00rootroot00000000000000collada_urdf-1.12.13/collada_urdf/include/collada_urdf/000077500000000000000000000000001370336760200227665ustar00rootroot00000000000000collada_urdf-1.12.13/collada_urdf/include/collada_urdf/collada_urdf.h000066400000000000000000000044711370336760200255640ustar00rootroot00000000000000/********************************************************************* * 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. *********************************************************************/ /* Authors: Tim Field */ #ifndef COLLADA_URDF_COLLADA_URDF_H #define COLLADA_URDF_COLLADA_URDF_H #include #include #include "urdf/model.h" namespace collada_urdf { class ColladaUrdfException : public std::runtime_error { public: ColladaUrdfException(std::string const& what); }; /** Write a COLLADA DOM to a file * \param robot_model The URDF robot model * \param file The filename to write the document to * \return true on success, false on failure */ bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, std::string const& file); } #endif collada_urdf-1.12.13/collada_urdf/package.xml000066400000000000000000000036621370336760200210500ustar00rootroot00000000000000 collada_urdf 1.12.13 This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents. Implements robot-specific COLLADA extensions as defined by http://openrave.programmingvision.com/index.php/Started:COLLADA Tim Field Rosen Diankov Ioan Sucan Jackie Kay Chris Lalancette Shane Loretz BSD http://ros.org/wiki/collada_urdf https://github.com/ros/collada_urdf https://github.com/ros/collada_urdf/issues catkin liburdfdom-headers-dev angles assimp-dev cmake_modules collada-dom collada_parser eigen geometric_shapes liburdfdom-dev liburdfdom-headers-dev resource_retriever rosconsole urdf assimp collada-dom collada_parser geometric_shapes liburdfdom-dev resource_retriever rosconsole urdf collada_urdf-1.12.13/collada_urdf/src/000077500000000000000000000000001370336760200175135ustar00rootroot00000000000000collada_urdf-1.12.13/collada_urdf/src/collada_to_urdf.cpp000066400000000000000000000561131370336760200233460ustar00rootroot00000000000000/* Author: Yohei Kakiuchi */ #include #include #include #include #include #include #include #include #if defined(ASSIMP_UNIFIED_HEADER_NAMES) #include #include #include #include #include #include #else #include #if defined(ASSIMP_EXPORT_API) #include #endif #include #include #endif #include #include #include #include #undef GAZEBO_1_3 #define GAZEBO_1_3 using namespace urdf; using namespace std; bool use_simple_visual = false; bool use_simple_collision = false; bool add_gazebo_description = false; bool use_transmission_interface = false; bool use_assimp_export = false; bool use_same_collision_as_visual = true; bool rotate_inertia_frame = true; bool export_collision_mesh = false; string mesh_dir = "/tmp"; string arobot_name = ""; string output_file = ""; string mesh_prefix = ""; #define PRINT_ORIGIN(os, origin) \ os << "xyz: " << origin.position.x << " " << origin.position.y << " " << origin.position.z << " "; \ { double r,p,y; origin.rotation.getRPY(r, p, y); \ os << "rpy: " << r << " " << p << " " << y; } #define PRINT_ORIGIN_XML(os, origin) \ os << "xyz=\"" << origin.position.x << " " << origin.position.y << " " << origin.position.z << "\""; \ { double h___r, h___p, h___y; \ origin.rotation.getRPY(h___r, h___p, h___y); \ os << " rpy=\"" << h___r << " " << h___p << " " << h___y << "\""; } #define PRINT_GEOM(os, geometry) \ if ( geometry->type == urdf::Geometry::MESH ) { os << "geom: name: " << ((urdf::Mesh *)geometry.get())->filename; } void assimp_file_export(std::string fname, std::string ofname, std::string mesh_type = "collada") { #if defined(ASSIMP_EXPORT_API) if (fname.find("file://") == 0) { fname.erase(0, strlen("file://")); } Assimp::Importer importer; /* { // ignore UP_DIRECTION tag in collada bool existing; importer.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true, &existing); if(existing) { fprintf(stderr, ";; OverWrite : Ignore UP_DIRECTION", existing); } } */ const aiScene* scene = importer.ReadFile(fname.c_str(), aiProcess_Triangulate | aiProcess_GenNormals | aiProcess_JoinIdenticalVertices | aiProcess_SplitLargeMeshes | aiProcess_OptimizeMeshes | aiProcess_SortByPType); if (!scene) { std::string str( importer.GetErrorString() ); std::cerr << ";; " << str << std::endl; return; } Assimp::Exporter aexpt; aiReturn ret = aexpt.Export(scene, mesh_type, ofname); if ( ret != AI_SUCCESS ) { std::string str( "assimp error" ); std::cerr << ";; " << str << std::endl; } #endif } // assimp bounding box calculation void assimp_calc_bbox(string fname, float &minx, float &miny, float &minz, float &maxx, float &maxy, float &maxz) { if (fname.find("file://") == 0) { fname.erase(0, strlen("file://")); } Assimp::Importer importer; const aiScene* scene = importer.ReadFile(fname.c_str(), aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType); // aiProcess_GenNormals // aiProcess_GenSmoothNormals // aiProcess_SplitLargeMeshes if (!scene) { std::string str( importer.GetErrorString() ); std::cerr << ";; " << str << std::endl; return; } aiNode *node = scene->mRootNode; bool found = false; if(node->mNumMeshes > 0 && node->mMeshes != NULL) { std::cerr << "Root node has meshes " << node->mMeshes << std::endl;; found = true; } else { for (unsigned int i=0; i < node->mNumChildren; ++i) { if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) { std::cerr << "Child " << i << " has meshes" << std::endl; node = node->mChildren[i]; found = true; break; } } } if(found == false) { std::cerr << "Can't find meshes in file" << std::endl; return; } aiMatrix4x4 transform = node->mTransformation; // copy vertices maxx = maxy = maxz = -100000000.0; minx = miny = minz = 100000000.0; std::cerr << ";; num meshes: " << node->mNumMeshes << std::endl; for (unsigned int m = 0; m < node->mNumMeshes; m++) { aiMesh *a = scene->mMeshes[node->mMeshes[m]]; std::cerr << ";; num vertices: " << a->mNumVertices << std::endl; for (unsigned int i = 0 ; i < a->mNumVertices ; ++i) { aiVector3D p; p.x = a->mVertices[i].x; p.y = a->mVertices[i].y; p.z = a->mVertices[i].z; p *= transform; if ( maxx < p.x ) { maxx = p.x; } if ( maxy < p.y ) { maxy = p.y; } if ( maxz < p.z ) { maxz = p.z; } if ( minx > p.x ) { minx = p.x; } if ( miny > p.y ) { miny = p.y; } if ( minz > p.z ) { minz = p.z; } } } } void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os) { os << " name << "\">" << endl; if ( !!link->visual ) { os << " " << endl; if (!use_simple_visual) { os << " visual->origin); os << "/>" << endl; os << " " << endl; if ( link->visual->geometry->type == urdf::Geometry::MESH ) { std::string ifname (((urdf::Mesh *)link->visual->geometry.get())->filename); if (ifname.find("file://") == 0) { ifname.erase(0, strlen("file://")); } std::string ofname (mesh_dir + "/" + link->name + "_mesh.dae"); if (use_assimp_export) { // using collada export assimp_file_export (ifname, ofname); } else { // copy to ofname std::ofstream tmp_os; tmp_os.open(ofname.c_str()); std::ifstream is; is.open(ifname.c_str()); std::string buf; while(is && getline(is, buf)) tmp_os << buf << std::endl; is.close(); tmp_os.close(); } if (mesh_prefix != "") { os << " name + "_mesh.dae" << "\" scale=\"1 1 1\" />" << endl; } else { os << " " << endl; } } os << " " << endl; } else { // simple visual float ax,ay,az,bx,by,bz; if ( link->visual->geometry->type == urdf::Geometry::MESH ) { assimp_calc_bbox(((urdf::Mesh *)link->visual->geometry.get())->filename, ax, ay, az, bx, by, bz); } os << " visual->origin; pp.position.x += ( ax + bx ) / 2 ; pp.position.y += ( ay + by ) / 2 ; pp.position.z += ( az + bz ) / 2 ; PRINT_ORIGIN_XML(os, pp); os << "/>" << endl; os << " " << endl; os << " " << endl; os << " " << endl; } os << " " << endl; } if ( !!link->collision ) { os << " " << endl; if (!use_simple_collision) { os << " collision->origin); os << "/>" << endl; os << " " << endl; if ( link->visual->geometry->type == urdf::Geometry::MESH ) { std::string ifname; if (use_same_collision_as_visual) { ifname.assign (((urdf::Mesh *)link->visual->geometry.get())->filename); } else { ifname.assign (((urdf::Mesh *)link->collision->geometry.get())->filename); } if (ifname.find("file://") == 0) { ifname.erase(0, strlen("file://")); } std::string oofname; if (export_collision_mesh) { oofname = link->name + "_mesh.stl"; } else { oofname = link->name + "_mesh.dae"; } std::string ofname = (mesh_dir + "/" + oofname); if (use_assimp_export) { // using collada export if (export_collision_mesh) { assimp_file_export (ifname, ofname, "stl"); } else { assimp_file_export (ifname, ofname); } } else { // copy to ofname std::ofstream tmp_os; tmp_os.open(ofname.c_str()); std::ifstream is; is.open(ifname.c_str()); std::string buf; while(is && getline(is, buf)) tmp_os << buf << std::endl; is.close(); tmp_os.close(); } if (mesh_prefix != "") { os << " " << endl; } os << " " << endl; } else { // simple collision float ax,ay,az,bx,by,bz; if ( link->visual->geometry->type == urdf::Geometry::MESH ) { assimp_calc_bbox(std::string ( ((urdf::Mesh *)link->visual->geometry.get())->filename ), ax, ay, az, bx, by, bz); } os << " visual->origin; pp.position.x += ( ax + bx ) / 2 ; pp.position.y += ( ay + by ) / 2 ; pp.position.z += ( az + bz ) / 2 ; PRINT_ORIGIN_XML(os, pp); os << "/>" << endl; os << " " << endl; os << " " << endl; os << " " << endl; } os << " " << endl; } if ( !!link->inertial ) { if (!rotate_inertia_frame) { os << " " << endl; os << " inertial->mass << "\" />" << endl; os << " inertial->origin); os << "/>" << endl; os << " inertial->ixx << "\" "; os << "ixy=\"" << link->inertial->ixy << "\" "; os << "ixz=\"" << link->inertial->ixz << "\" "; os << "iyy=\"" << link->inertial->iyy << "\" "; os << "iyz=\"" << link->inertial->iyz << "\" "; os << "izz=\"" << link->inertial->izz << "\"/>" << endl; os << " " << endl; } else { // rotation should be identity os << " " << endl; os << " inertial->mass << "\" />" << endl; os << " inertial->origin.rotation.w, link->inertial->origin.rotation.x, link->inertial->origin.rotation.y, link->inertial->origin.rotation.z); Eigen::Matrix3d mat(qt); Eigen::Matrix3d tmat(mat.transpose()); Eigen::Matrix3d imat; imat(0, 0) = link->inertial->ixx; imat(0, 1) = link->inertial->ixy; imat(0, 2) = link->inertial->ixz; imat(1, 0) = link->inertial->ixy; imat(1, 1) = link->inertial->iyy; imat(1, 2) = link->inertial->iyz; imat(2, 0) = link->inertial->ixz; imat(2, 1) = link->inertial->iyz; imat(2, 2) = link->inertial->izz; #define DEBUG_MAT(mat) \ cout << "#2f((" << mat(0, 0) << " " << mat(0, 1) << " " << mat(0, 2) << ")"; \ cout << "(" << mat(1, 0) << " " << mat(1, 1) << " " << mat(1, 2) << ")"; \ cout << "(" << mat(2, 0) << " " << mat(2, 1) << " " << mat(2, 2) << "))" << endl; #if DEBUG DEBUG_MAT(mat); DEBUG_MAT(tmat); DEBUG_MAT(imat); #endif imat = ( mat * imat * tmat ); #if DEBUG DEBUG_MAT(imat); #endif urdf::Pose t_pose (link->inertial->origin); t_pose.rotation.clear(); PRINT_ORIGIN_XML(os, t_pose); os << "/>" << endl; os << " " << endl; os << " " << endl; } } os << " " << endl; #ifdef GAZEBO_1_3 if ( add_gazebo_description ) { os << " name << "\">" << endl; os << " 0.9" << endl; os << " 0.9" << endl; os << " " << endl; } #endif for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) addChildLinkNamesXML(*child, os); } void addChildJointNamesXML(urdf::LinkConstSharedPtr link, ofstream& os) { double r, p, y; for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); std::string jtype; if ( (*child)->parent_joint->type == urdf::Joint::UNKNOWN ) { jtype = std::string("unknown"); } else if ( (*child)->parent_joint->type == urdf::Joint::REVOLUTE ) { jtype = std::string("revolute"); } else if ( (*child)->parent_joint->type == urdf::Joint::CONTINUOUS ) { jtype = std::string("continuous"); } else if ( (*child)->parent_joint->type == urdf::Joint::PRISMATIC ) { jtype = std::string("prismatic"); } else if ( (*child)->parent_joint->type == urdf::Joint::FLOATING ) { jtype = std::string("floating"); } else if ( (*child)->parent_joint->type == urdf::Joint::PLANAR ) { jtype = std::string("planar"); } else if ( (*child)->parent_joint->type == urdf::Joint::FIXED ) { jtype = std::string("fixed"); } else { ///error } os << " parent_joint->name << "\" type=\"" << jtype << "\">" << endl; os << " name << "\"/>" << endl; os << " name << "\"/>" << endl; os << " parent_joint->parent_to_joint_origin_transform.position.x << " "; os << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " "; os << (*child)->parent_joint->parent_to_joint_origin_transform.position.z; os << "\" rpy=\"" << r << " " << p << " " << y << " " << "\"/>" << endl; os << " parent_joint->axis.x << " "; os << (*child)->parent_joint->axis.y << " " << (*child)->parent_joint->axis.z << "\"/>" << endl; { urdf::JointSharedPtr jt((*child)->parent_joint); if ( !!jt->limits ) { os << " limits->lower << "\""; os << " upper=\"" << jt->limits->upper << "\""; if (jt->limits->effort == 0.0) { os << " effort=\"100\""; } else { os << " effort=\"" << jt->limits->effort << "\""; } os << " velocity=\"" << jt->limits->velocity << "\""; os << " />" << endl; } if ( !!jt->dynamics ) { os << " dynamics->damping << "\""; os << " friction=\"" << jt->dynamics->friction << "\""; os << " />" << endl; } else { os << " " << endl; } #ifdef GAZEBO_1_3 #if 0 os << " " << endl; #endif #endif } os << " " << endl; if ( add_gazebo_description ) { if ( !use_transmission_interface ) { os << " parent_joint->name << "_trans\" >" << endl; os << " parent_joint->name << "_motor\" />" << endl; os << " parent_joint->name << "\" />" << endl; os << " 1" << endl; //os << " 1" << endl; //os << " 90000" << endl; os << " " << endl; } else { os << " parent_joint->name << "_trans\">" << endl; os << " transmission_interface/SimpleTransmission" << endl; os << " parent_joint->name << "\">" << endl; os << " hardware_interface/EffortJointInterface" << endl; os << " " << endl; os << " parent_joint->name << "_motor\">" << endl; os << " hardware_interface/EffortJointInterface" << endl; os << " 1" << endl; os << " " << endl; os << " " << endl; } #ifdef GAZEBO_1_3 os << " parent_joint->name << "\">" << endl; os << " 0.4" << endl; os << " " << endl; #endif } addChildJointNamesXML(*child, os); } } void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file) { std::ofstream os; os.open(file.c_str()); os << "" << endl; os << "" << endl; addChildLinkNamesXML(link, os); addChildJointNamesXML(link, os); os << "" << endl; os.close(); } namespace po = boost::program_options; // using namespace std; int main(int argc, char** argv) { string inputfile; po::options_description desc("Usage: collada_to_urdf input.dae [options]\n Options for collada_to_urdf"); desc.add_options() ("help", "produce help message") ("simple_visual,V", "use bounding box for visual") ("simple_collision,C", "use bounding box for collision") ("export_collision_mesh", "export collision mesh as STL") ("add_gazebo_description,G", "add description for using on gazebo") ("use_transmission_interface,T", "use transmission_interface as transmission") ("use_assimp_export,A", "use assimp library for exporting mesh") ("use_collision,U", "use collision geometry (default collision is the same as visual)") ("original_inertia_rotation,R", "does not rotate inertia frame") ("robot_name,N", po::value< vector >(), "output robot name") ("mesh_output_dir", po::value< vector >(), "directory for outputing") ("mesh_prefix", po::value< vector >(), "prefix of mesh files") ("output_file,O", po::value< vector >(), "output file") ("input_file", po::value< vector >(), "input file") ; po::positional_options_description p; p.add("input_file", -1); po::variables_map vm; try { po::store(po::command_line_parser(argc, argv). options(desc).positional(p).run(), vm); po::notify(vm); } catch (po::error e) { cerr << ";; option parse error / " << e.what() << endl; return 1; } if (vm.count("help")) { cout << desc << "\n"; return 1; } if (vm.count("simple_visual")) { use_simple_visual = true; cerr << ";; Using simple_visual" << endl; } if (vm.count("simple_collision")) { use_simple_collision = true; cerr << ";; Using simple_collision" << endl; } if (vm.count("add_gazebo_description")) { add_gazebo_description = true; cerr << ";; Adding gazebo description" << endl; } if (vm.count("use_transmission_interface")) { use_transmission_interface = true; cerr << ";; Using transmission_interface as transmission" << endl; } if (vm.count("use_assimp_export")) { #if defined(ASSIMP_EXPORT_API) use_assimp_export = true; #endif cerr << ";; Use assimp export" << endl; } if (vm.count("original_inertia_rotation")) { rotate_inertia_frame = false; cerr << ";; Does not rotate inertia frame" << endl; } if (vm.count("export_collision_mesh")) { export_collision_mesh = true; cerr << ";; erxport collision mesh as STL" << endl; } if (vm.count("output_file")) { vector aa = vm["output_file"].as< vector >(); cerr << ";; output file is: " << aa[0] << endl; output_file = aa[0]; } if (vm.count("robot_name")) { vector aa = vm["robot_name"].as< vector >(); cerr << ";; robot_name is: " << aa[0] << endl; arobot_name = aa[0]; } if (vm.count("mesh_prefix")) { vector aa = vm["mesh_prefix"].as< vector >(); cerr << ";; mesh_prefix is: " << aa[0] << endl; mesh_prefix = aa[0]; } if (vm.count("mesh_output_dir")) { vector aa = vm["mesh_output_dir"].as< vector >(); cerr << ";; Mesh output directory is: " << aa[0] << endl; mesh_dir = aa[0]; // check directory existence boost::filesystem::path mpath( mesh_dir ); try { if ( ! boost::filesystem::is_directory(mpath) ) { boost::filesystem::create_directory ( mpath ); } } catch ( boost::filesystem::filesystem_error e ) { cerr << ";; mesh output directory error / " << e.what() << endl; return 1; } } if (vm.count("input_file")) { vector aa = vm["input_file"].as< vector >(); cerr << ";; Input file is: " << aa[0] << endl; inputfile = aa[0]; } if(inputfile == "") { cerr << desc << endl; return 1; } std::string xml_string; std::fstream xml_file(inputfile.c_str(), std::fstream::in); while ( xml_file.good() ) { std::string line; std::getline( xml_file, line); xml_string += (line + "\n"); } xml_file.close(); urdf::ModelInterfaceSharedPtr robot; if( xml_string.find("getName(); } if (output_file == "") { output_file = arobot_name + ".urdf"; } printTreeXML (robot->getRoot(), arobot_name, output_file); return 0; } collada_urdf-1.12.13/collada_urdf/src/collada_urdf.cpp000066400000000000000000002542361370336760200226520ustar00rootroot00000000000000// -*- coding: utf-8 -*- /********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc., University of Tokyo * 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. *********************************************************************/ /* Authors: Rosen Diankov, Tim Field */ #include "collada_urdf/collada_urdf.h" #include #include #include #include #ifndef _WIN32 #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include #include #include #include #include #include #include #include #include #pragma GCC diagnostic pop #endif #include #include #include #include #include #include #include #include #include #include #include #if defined(ASSIMP_UNIFIED_HEADER_NAMES) #include #include #include #include #include #include #include #else #include #include #include #include #include #include #endif #include #include #define FOREACH(it, v) for(decltype((v).begin()) it = (v).begin(); it != (v).end(); (it)++) #define FOREACHC FOREACH using namespace std; namespace ColladaDOM150 { } namespace collada_urdf { using namespace ColladaDOM150; /// 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_; }; namespace mathextra { // code from MagicSoftware by Dave Eberly const double g_fEpsilon = 1e-15; //=========================================================================== #define distinctRoots 0 // roots r0 < r1 < r2 #define singleRoot 1 // root r0 #define floatRoot01 2 // roots r0 = r1 < r2 #define floatRoot12 4 // roots r0 < r1 = r2 #define tripleRoot 6 // roots r0 = r1 = r2 template void Tridiagonal3 (S* mat, T* diag, T* subd) { T a, b, c, d, e, f, ell, q; a = mat[0*3+0]; b = mat[0*3+1]; c = mat[0*3+2]; d = mat[1*3+1]; e = mat[1*3+2]; f = mat[2*3+2]; subd[2] = 0.0; diag[0] = a; if ( fabs(c) >= g_fEpsilon ) { ell = (T)sqrt(b*b+c*c); b /= ell; c /= ell; q = 2*b*e+c*(f-d); diag[1] = d+c*q; diag[2] = f-c*q; subd[0] = ell; subd[1] = e-b*q; mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0; mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c; mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b; } else { diag[1] = d; diag[2] = f; subd[0] = b; subd[1] = e; mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0; mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0; mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1; } } int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2) { // polynomial is L^3-c2*L^2+c1*L-c0 int maxiter = 50; double discr, temp, pval, pdval, b0, b1; int i; // find local extrema (if any) of p'(L) = 3*L^2-2*c2*L+c1 discr = c2*c2-3*c1; if ( discr >= 0.0 ) { discr = (double)sqrt(discr); temp = (c2+discr)/3; pval = temp*(temp*(temp-c2)+c1)-c0; if ( pval >= 0.0 ) { // double root occurs before the positive local maximum (*r0) = (c2-discr)/3 - 1; // initial guess for Newton's methods pval = 2*g_fEpsilon; for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) { pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0; pdval = (*r0)*(3*(*r0)-2*c2)+c1; (*r0) -= pval/pdval; } // Other two roots are solutions to quadratic equation // L^2 + ((*r0)-c2)*L + [(*r0)*((*r0)-c2)+c1] = 0. b1 = (*r0)-c2; b0 = (*r0)*((*r0)-c2)+c1; discr = b1*b1-4*b0; if ( discr < -g_fEpsilon ) { // single root r0 return singleRoot; } else { int result = distinctRoots; // roots r0 <= r1 <= r2 discr = sqrt(fabs(discr)); (*r1) = 0.5f*(-b1-discr); (*r2) = 0.5f*(-b1+discr); if ( fabs((*r0)-(*r1)) <= g_fEpsilon ) { (*r0) = (*r1); result |= floatRoot01; } if ( fabs((*r1)-(*r2)) <= g_fEpsilon ) { (*r1) = (*r2); result |= floatRoot12; } return result; } } else { // double root occurs after the negative local minimum (*r2) = temp + 1; // initial guess for Newton's method pval = 2*g_fEpsilon; for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) { pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0; pdval = (*r2)*(3*(*r2)-2*c2)+c1; (*r2) -= pval/pdval; } // Other two roots are solutions to quadratic equation // L^2 + (r2-c2)*L + [r2*(r2-c2)+c1] = 0. b1 = (*r2)-c2; b0 = (*r2)*((*r2)-c2)+c1; discr = b1*b1-4*b0; if ( discr < -g_fEpsilon ) { // single root (*r0) = (*r2); return singleRoot; } else { int result = distinctRoots; // roots r0 <= r1 <= r2 discr = sqrt(fabs(discr)); (*r0) = 0.5f*(-b1-discr); (*r1) = 0.5f*(-b1+discr); if ( fabs((*r0)-(*r1)) <= g_fEpsilon ) { (*r0) = (*r1); result |= floatRoot01; } if ( fabs((*r1)-(*r2)) <= g_fEpsilon ) { (*r1) = (*r2); result |= floatRoot12; } return result; } } } else { // p(L) has one double root (*r0) = c0; pval = 2*g_fEpsilon; for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) { pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0; pdval = (*r0)*(3*(*r0)-2*c2)+c1; (*r0) -= pval/pdval; } return singleRoot; } } //---------------------------------------------------------------------------- template bool _QLAlgorithm3 (T* m_aafEntry, T* afDiag, T* afSubDiag) { // QL iteration with implicit shifting to reduce matrix from tridiagonal // to diagonal for (int i0 = 0; i0 < 3; i0++) { const int iMaxIter = 32; int iIter; for (iIter = 0; iIter < iMaxIter; iIter++) { int i1; for (i1 = i0; i1 <= 1; i1++) { T fSum = fabs(afDiag[i1]) + fabs(afDiag[i1+1]); if ( fabs(afSubDiag[i1]) + fSum == fSum ) break; } if ( i1 == i0 ) break; T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]); T fTmp1 = sqrt(fTmp0*fTmp0+1.0f); if ( fTmp0 < 0.0f ) fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1); else fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1); T fSin = 1.0f; T fCos = 1.0f; T fTmp2 = 0.0f; for (int i2 = i1-1; i2 >= i0; i2--) { T fTmp3 = fSin*afSubDiag[i2]; T fTmp4 = fCos*afSubDiag[i2]; if ( fabs(fTmp3) >= fabs(fTmp0) ) { fCos = fTmp0/fTmp3; fTmp1 = sqrt(fCos*fCos+1.0f); afSubDiag[i2+1] = fTmp3*fTmp1; fSin = 1.0f/fTmp1; fCos *= fSin; } else { fSin = fTmp3/fTmp0; fTmp1 = sqrt(fSin*fSin+1.0f); afSubDiag[i2+1] = fTmp0*fTmp1; fCos = 1.0f/fTmp1; fSin *= fCos; } fTmp0 = afDiag[i2+1]-fTmp2; fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos; fTmp2 = fSin*fTmp1; afDiag[i2+1] = fTmp0+fTmp2; fTmp0 = fCos*fTmp1-fTmp4; for (int iRow = 0; iRow < 3; iRow++) { fTmp3 = m_aafEntry[iRow*3+i2+1]; m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] + fCos*fTmp3; m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] - fSin*fTmp3; } } afDiag[i0] -= fTmp2; afSubDiag[i0] = fTmp0; afSubDiag[i1] = 0.0f; } if ( iIter == iMaxIter ) { // should not get here under normal circumstances return false; } } return true; } bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag) { return _QLAlgorithm3(m_aafEntry, afDiag, afSubDiag); } bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag) { return _QLAlgorithm3(m_aafEntry, afDiag, afSubDiag); } void EigenSymmetric3(const double* fmat, double* afEigenvalue, double* fevecs) { double afSubDiag[3]; memcpy(fevecs, fmat, sizeof(double)*9); Tridiagonal3(fevecs, afEigenvalue,afSubDiag); QLAlgorithm3(fevecs, afEigenvalue,afSubDiag); // make eigenvectors form a right--handed system double fDet = fevecs[0*3+0] * (fevecs[1*3+1] * fevecs[2*3+2] - fevecs[1*3+2] * fevecs[2*3+1]) + fevecs[0*3+1] * (fevecs[1*3+2] * fevecs[2*3+0] - fevecs[1*3+0] * fevecs[2*3+2]) + fevecs[0*3+2] * (fevecs[1*3+0] * fevecs[2*3+1] - fevecs[1*3+1] * fevecs[2*3+0]); if ( fDet < 0.0f ) { fevecs[0*3+2] = -fevecs[0*3+2]; fevecs[1*3+2] = -fevecs[1*3+2]; fevecs[2*3+2] = -fevecs[2*3+2]; } } /* end of MAGIC code */ } // end namespace geometry /// 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_; }; class Triangle { public: Triangle(const urdf::Vector3 &_p1, const urdf::Vector3 &_p2, const urdf::Vector3 &_p3) : p1(_p1), p2(_p2), p3(_p3) {} Triangle() { this->clear(); }; urdf::Vector3 p1, p2, p3; void clear() { p1.clear(); p2.clear(); p3.clear(); }; }; /// \brief Implements writing urdf::Model objects to a COLLADA DOM. // Portions of this code are taken verbatim from OpenRAVE (https://github.com/rdiankov/openrave, commits 87410293, ca3473f7, d844de2a) and relicensed as BSD by the original author (rdiankov) 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; }; typedef std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES; struct LINKOUTPUT { list > listusedlinks; list > listprocesseddofs; daeElementRef plink; domNodeRef pnode; MAPLINKPOSES _maplinkposes; }; struct physics_model_output { domPhysics_modelRef pmodel; std::vector vrigidbodysids; ///< same ordering as the physics indices }; 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; urdf::JointConstSharedPtr pjoint; int iaxis; string jointnodesid; }; domKinematics_modelRef kmodel; std::vector vaxissids; std::vector vlinksids; MAPLINKPOSES _maplinkposes; }; 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; }; struct instance_physics_model_output { domInstance_physics_modelRef ipm; boost::shared_ptr pmout; }; struct kinbody_models { std::string uri, kinematicsgeometryhash; boost::shared_ptr kmout; boost::shared_ptr pmout; }; public: ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) { daeErrorHandler::setErrorHandler(this); _importer.SetIOHandler(new ResourceIOSystem()); } virtual ~ColladaWriter() { } daeDocument* doc() { return _doc; } bool convert() { try { const char* documentName = "urdf_snapshot"; 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"); _physicsModelsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS)); _physicsModelsLib->setId("pmodels"); 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 true; } catch (ColladaUrdfException ex) { ROS_ERROR("Error converting: %s", ex.what()); return false; } } bool writeTo(string const& file) { try { daeString uri = _doc->getDocumentURI()->getURI(); _collada.writeTo(uri, file); } catch (ColladaUrdfException ex) { return false; } return true; } 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 = _ComputeId(str(boost::format("robot%d")%id)); string askid = _ComputeId(str(boost::format("%s_kinematics")%asid)); string asmid = _ComputeId(str(boost::format("%s_motion")%asid)); string iassid = _ComputeId(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 = _ComputeId(str(boost::format("axis_info_inst%d")%idof)); urdf::JointConstSharedPtr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint; BOOST_ASSERT(_mapjointindices[pjoint] == (int)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( !!pjoint->safety ) { flower = pjoint->safety->soft_lower_limit; fupper = pjoint->safety->soft_upper_limit; } 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 = _ComputeId(str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid())); string assym = _ComputeId(str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid())); FOREACH(it, _ikmout->vkinematicsbindings) { domKinematics_newparamRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); abm->setSid(asmsym.c_str()); daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%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(str(boost::format("%s/%s")%asmid%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_newparamRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); abm->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.axissid)).c_str()); daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%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_%s")%asmid%asmid%kas.axissid).c_str()); string valuesid; if( kas.valuesid.size() > 0 ) { domKinematics_newparamRef abmvalue = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); abmvalue->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.valuesid)).c_str()); daeSafeCast(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str()); domKinematics_bindRef abvalue = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); valuesid = _ComputeId(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_%s")%asmid%asmid%kas.valuesid).c_str()); } _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid)); } boost::shared_ptr ipmout = _WriteInstance_physics_model(id,_scene.pscene,_scene.pscene->getID(), _ikmout->kmout->_maplinkposes); } /// \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 = _ComputeId(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_newparamRef kbind = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM)); kbind->setSid(_ComputeId(symscope+ikmsid).c_str()); daeSafeCast(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str()); _ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node%d")%id%_maplinkindices[_robot.getRoot()]))); _ikmout->vaxissids.reserve(kmout->vaxissids.size()); int i = 0; FOREACH(it,kmout->vaxissids) { domKinematics_newparamRef kbind = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM)); string ref = it->sid; size_t index = ref.find("/"); while(index != string::npos) { ref[index] = '.'; index = ref.find("/",index+1); } string sid = _ComputeId(symscope+ikmsid+"_"+ref); kbind->setSid(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 = _ComputeKinematics_modelId(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 = _ComputeId(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_) { urdf::JointSharedPtr pjoint = itjoint->second; int index = _mapjointindices[itjoint->second]; domJointRef pdomjoint = daeSafeCast(ktec->add(COLLADA_ELEMENT_JOINT)); string jointid = _ComputeId(pjoint->name); //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 = _ComputeId(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; } kmout->_maplinkposes = childinfo._maplinkposes; // create the formulas for all mimic joints FOREACHC(itjoint, _robot.joints_) { string jointsid = _ComputeId(itjoint->second->name); urdf::JointSharedPtr pjoint = itjoint->second; if( !pjoint->mimic ) { continue; } domFormulaRef pf = daeSafeCast(ktec->add(COLLADA_ELEMENT_FORMULA)); string formulaid = _ComputeId(str(boost::format("%s_formula")%jointsid)); pf->setSid(formulaid.c_str()); domCommon_float_or_paramRef ptarget = daeSafeCast(pf->add(COLLADA_ELEMENT_TARGET)); string targetjointid = str(boost::format("%s/%s")%kmodel->getID()%jointsid); daeSafeCast(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str()); domTechniqueRef pftec = daeSafeCast(pf->add(COLLADA_ELEMENT_TECHNIQUE)); pftec->setProfile("OpenRAVE"); // save position equation { daeElementRef poselt = pftec->add("equation"); poselt->setAttribute("type","position"); // create a const0*joint+const1 formula // a x b daeElementRef pmath_math = poselt->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/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name))); } daeElementRef pmath_const1 = pmath_apply->add("cn"); pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); } } // save first partial derivative { daeElementRef derivelt = pftec->add("equation"); derivelt->setAttribute("type","first_partial"); derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str()); daeElementRef pmath_const0 = derivelt->add("cn"); pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); } // save second partial derivative { daeElementRef derivelt = pftec->add("equation"); derivelt->setAttribute("type","second_partial"); derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str()); daeElementRef pmath_const0 = derivelt->add("cn"); pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); } { domFormula_techniqueRef pfcommontec = daeSafeCast(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); // create a const0*joint+const1 formula // a x b daeElementRef pmath_math = pfcommontec->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/%s")%kmodel->getID()%_ComputeId(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(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri) { LINKOUTPUT out; int linkindex = _maplinkindices[plink]; string linksid = _ComputeId(plink->name); 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 = _ComputeId(str(boost::format("v%s_node%d")%strModelUri%linkindex)); pnode->setId( nodeid.c_str() ); string nodesid = _ComputeId(str(boost::format("node%d")%linkindex)); pnode->setSid(nodesid.c_str()); pnode->setName(plink->name.c_str()); urdf::GeometrySharedPtr geometry; urdf::MaterialSharedPtr 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; } urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin); if( !!geometry ) { bool write_visual = false; if ( !!plink->visual ) { if (plink->visual_array.size() > 1) { int igeom = 0; for (std::vector::const_iterator it = plink->visual_array.begin(); it != plink->visual_array.end(); it++) { // geom string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); igeom++; domGeometryRef pdomgeom; if ( it != plink->visual_array.begin() ) { urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin); pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans); } else { pdomgeom = _WriteGeometry((*it)->geometry, geomid); } domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); pinstgeom->setUrl((string("#") + geomid).c_str()); // material _WriteMaterial(pdomgeom->getID(), (*it)->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"); write_visual = true; } } } if (!write_visual) { // just 1 visual int igeom = 0; string geomid = _ComputeId(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); // process all children FOREACHC(itjoint, plink->child_joints) { urdf::JointSharedPtr pjoint = *itjoint; int index = _mapjointindices[pjoint]; // domLink::domAttachment_fullRef attachment_full = daeSafeCast(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL)); string jointid = str(boost::format("%s/%s")%strModelUri%_ComputeId(pjoint->name)); 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)); } FOREACH(itlinkpos,childinfo._maplinkposes) { out._maplinkposes[itlinkpos->first] = _poseMult(pjoint->parent_to_joint_origin_transform,itlinkpos->second); } // rotate/translate elements string jointnodesid = _ComputeId(str(boost::format("node_%s_axis0")%pjoint->name)); 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._maplinkposes[plink] = urdf::Pose(); out.listusedlinks.push_back(make_pair(linkindex,linksid)); out.plink = pdomlink; out.pnode = pnode; return out; } domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL) { 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(); cgeometry->setName(urdf_mesh->filename.c_str()); _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale, org_trans); break; } case urdf::Geometry::SPHERE: { shapes::Sphere sphere(static_cast(geometry.get())->radius); boost::scoped_ptr mesh(shapes::createMeshFromShape(sphere)); _loadVertices(mesh.get(), cgeometry); break; } case urdf::Geometry::BOX: { shapes::Box box(static_cast(geometry.get())->dim.x, static_cast(geometry.get())->dim.y, static_cast(geometry.get())->dim.z); boost::scoped_ptr mesh(shapes::createMeshFromShape(box)); _loadVertices(mesh.get(), cgeometry); break; } case urdf::Geometry::CYLINDER: { shapes::Cylinder cyl(static_cast(geometry.get())->radius, static_cast(geometry.get())->length); boost::scoped_ptr mesh(shapes::createMeshFromShape(cyl)); _loadVertices(mesh.get(), cgeometry); 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, urdf::MaterialSharedPtr 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); } boost::shared_ptr _WriteInstance_physics_model(int id, daeElementRef parent, const string& sidscope, const MAPLINKPOSES& maplinkposes) { boost::shared_ptr pmout = WritePhysics_model(id, maplinkposes); boost::shared_ptr ipmout(new instance_physics_model_output()); ipmout->pmout = pmout; ipmout->ipm = daeSafeCast(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL)); string bodyid = _ComputeId(str(boost::format("visual%d")%id)); ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,string("#")+bodyid)); string symscope, refscope; if( sidscope.size() > 0 ) { symscope = sidscope+string("_"); refscope = sidscope+string("/"); } string ipmsid = str(boost::format("%s_inst")%pmout->pmodel->getID()); ipmout->ipm->setUrl(str(boost::format("#%s")%pmout->pmodel->getID()).c_str()); ipmout->ipm->setSid(ipmsid.c_str()); string kmodelid = _ComputeKinematics_modelId(id); for(size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) { domInstance_rigid_bodyRef pirb = daeSafeCast(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY)); pirb->setBody(pmout->vrigidbodysids[i].c_str()); pirb->setTarget(xsAnyURI(*pirb,str(boost::format("#v%s_node%d")%kmodelid%i))); } return ipmout; } boost::shared_ptr WritePhysics_model(int id, const MAPLINKPOSES& maplinkposes) { boost::shared_ptr pmout(new physics_model_output()); pmout->pmodel = daeSafeCast(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL)); string pmodelid = str(boost::format("pmodel%d")%id); pmout->pmodel->setId(pmodelid.c_str()); pmout->pmodel->setName(_robot.getName().c_str()); FOREACHC(itlink,_robot.links_) { domRigid_bodyRef rigid_body = daeSafeCast(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY)); string rigidsid = str(boost::format("rigid%d")%_maplinkindices[itlink->second]); pmout->vrigidbodysids.push_back(rigidsid); rigid_body->setSid(rigidsid.c_str()); rigid_body->setName(itlink->second->name.c_str()); domRigid_body::domTechnique_commonRef ptec = daeSafeCast(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); urdf::InertialSharedPtr inertial = itlink->second->inertial; if( !!inertial ) { daeSafeCast(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true)); //!!inertial)); domTargetable_floatRef mass = daeSafeCast(ptec->add(COLLADA_ELEMENT_MASS)); mass->setValue(inertial->mass); double fCovariance[9] = { inertial->ixx, inertial->ixy, inertial->ixz, inertial->ixy, inertial->iyy, inertial->iyz, inertial->ixz, inertial->iyz, inertial->izz}; double eigenvalues[3], eigenvectors[9]; mathextra::EigenSymmetric3(fCovariance,eigenvalues,eigenvectors); boost::array minertiaframe; for(int j = 0; j < 3; ++j) { minertiaframe[4*0+j] = eigenvectors[3*j]; minertiaframe[4*1+j] = eigenvectors[3*j+1]; minertiaframe[4*2+j] = eigenvectors[3*j+2]; } urdf::Pose tinertiaframe; tinertiaframe.rotation = _quatFromMatrix(minertiaframe); tinertiaframe = _poseMult(inertial->origin,tinertiaframe); domTargetable_float3Ref pinertia = daeSafeCast(ptec->add(COLLADA_ELEMENT_INERTIA)); pinertia->getValue().setCount(3); pinertia->getValue()[0] = eigenvalues[0]; pinertia->getValue()[1] = eigenvalues[1]; pinertia->getValue()[2] = eigenvalues[2]; urdf::Pose posemassframe = _poseMult(maplinkposes.find(itlink->second)->second,tinertiaframe); _WriteTransformation(ptec->add(COLLADA_ELEMENT_MASS_FRAME), posemassframe); // // create a shape for every geometry // int igeom = 0; // FOREACHC(itgeom, (*itlink)->GetGeometries()) { // domRigid_body::domTechnique_common::domShapeRef pdomshape = daeSafeCast(ptec->add(COLLADA_ELEMENT_SHAPE)); // // there is a weird bug here where _WriteTranformation will fail to create rotate/translate elements in instance_geometry is created first... (is this part of the spec?) // _WriteTransformation(pdomshape,tbaseinv*(*itlink)->GetTransform()*itgeom->GetTransform()); // domInstance_geometryRef pinstgeom = daeSafeCast(pdomshape->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); // pinstgeom->setUrl(xsAnyURI(*pinstgeom,string("#")+_GetGeometryId(*itlink,igeom))); // ++igeom; // } } } return pmout; } void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom) { // convert the mesh into an STL binary (in memory) std::vector buffer; shapes::writeSTLBinary(mesh, buffer); // Create an instance of the Importer class Assimp::Importer importer; // And have it read the given file with some postprocessing const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast(&buffer[0]), buffer.size(), aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_OptimizeGraph | aiProcess_OptimizeMeshes, "stl"); // Note: we do this mesh -> STL -> assimp mesh because the aiScene::aiScene symbol is hidden by default 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, std::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, std::string("#")+std::string(pvertsource->getID()))); } _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1)); pacc->setCount(parray->getCount()); } void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale, urdf::Pose *org_trans) { 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,org_trans); pacc->setCount(parray->getCount()); } void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale, urdf::Pose *org_trans = NULL) { 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; if (org_trans) { urdf::Vector3 vv; vv.x = p.x*scale.x; vv.y = p.y*scale.y; vv.z = p.z*scale.z; urdf::Vector3 nv = _poseMult(*org_trans, vv); parray->getValue().append(nv.x); parray->getValue().append(nv.y); parray->getValue().append(nv.z); } else { 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,org_trans); } } 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; } static urdf::Rotation _quatMult(const urdf::Rotation& quat0, const urdf::Rotation& quat1) { urdf::Rotation q; q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y; q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z; q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x; q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z; double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w); // don't touch the divides q.x /= fnorm; q.y /= fnorm; q.z /= fnorm; q.w /= fnorm; return q; } static urdf::Pose _poseMult(const urdf::Pose& p0, const urdf::Pose& p1) { urdf::Pose p; p.position = _poseMult(p0,p1.position); p.rotation = _quatMult(p0.rotation,p1.rotation); return p; } static urdf::Rotation _quatFromMatrix(const boost::array& mat) { urdf::Rotation rot; double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2]; if (tr >= 0) { rot.w = tr + 1; rot.x = (mat[4*2+1] - mat[4*1+2]); rot.y = (mat[4*0+2] - mat[4*2+0]); rot.z = (mat[4*1+0] - mat[4*0+1]); } else { // find the largest diagonal element and jump to the appropriate case if (mat[4*1+1] > mat[4*0+0]) { if (mat[4*2+2] > mat[4*1+1]) { rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1; rot.x = (mat[4*2+0] + mat[4*0+2]); rot.y = (mat[4*1+2] + mat[4*2+1]); rot.w = (mat[4*1+0] - mat[4*0+1]); } else { rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1; rot.z = (mat[4*1+2] + mat[4*2+1]); rot.x = (mat[4*0+1] + mat[4*1+0]); rot.w = (mat[4*0+2] - mat[4*2+0]); } } else if (mat[4*2+2] > mat[4*0+0]) { rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1; rot.x = (mat[4*2+0] + mat[4*0+2]); rot.y = (mat[4*1+2] + mat[4*2+1]); rot.w = (mat[4*1+0] - mat[4*0+1]); } else { rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1; rot.y = (mat[4*0+1] + mat[4*1+0]); rot.z = (mat[4*2+0] + mat[4*0+2]); rot.w = (mat[4*2+1] - mat[4*1+2]); } } double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w); // don't touch the divides rot.x /= fnorm; rot.y /= fnorm; rot.z /= fnorm; rot.w /= fnorm; return rot; } static std::string _ComputeKinematics_modelId(int id) { return _ComputeId(str(boost::format("kmodel%d")%id)); } /// \brief computes a collada-compliant sid from the urdf name static std::string _ComputeId(const std::string& name) { std::string newname = name; for(size_t i = 0; i < newname.size(); ++i) { if( newname[i] == '/' || newname[i] == ' ' || newname[i] == '.' ) { newname[i] = '_'; } } return newname; } int _writeoptions; const urdf::Model& _robot; DAE _collada; domCOLLADA* _dom; daeDocument *_doc; domCOLLADA::domSceneRef _globalscene; domLibrary_visual_scenesRef _visualScenesLib; domLibrary_kinematics_scenesRef _kinematicsScenesLib; domLibrary_kinematics_modelsRef _kinematicsModelsLib; domLibrary_articulated_systemsRef _articulatedSystemsLib; domLibrary_physics_scenesRef _physicsScenesLib; domLibrary_physics_modelsRef _physicsModelsLib; 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< urdf::JointConstSharedPtr, int > _mapjointindices; std::map< urdf::LinkConstSharedPtr, int > _maplinkindices; std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices; Assimp::Importer _importer; }; ColladaUrdfException::ColladaUrdfException(std::string const& what) : std::runtime_error(what) { } bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) { ColladaWriter writer(robot_model, 0); if ( ! writer.convert() ) { std::cerr << std::endl << "Error converting document" << std::endl; return -1; } return writer.writeTo(file); } } collada_urdf-1.12.13/collada_urdf/src/urdf_to_collada.cpp000066400000000000000000000046341370336760200233470ustar00rootroot00000000000000/********************************************************************* * 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_urdf.h" int main(int argc, char** argv) { if (argc != 3) { std::cerr << "Usage: urdf_to_collada input.urdf output.dae" << std::endl; return -1; } std::string input_filename(argv[1]); std::string output_filename(argv[2]); urdf::Model robot_model; if (!robot_model.initFile(input_filename)) { std::cerr << "failed to open urdf file " << input_filename << std::endl; return -2; } collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename); std::cout << std::endl << "Document successfully written to " << output_filename << std::endl; return 0; } collada_urdf-1.12.13/collada_urdf/test/000077500000000000000000000000001370336760200177035ustar00rootroot00000000000000collada_urdf-1.12.13/collada_urdf/test/pr2.urdf000066400000000000000000004257251370336760200213070ustar00rootroot00000000000000 true 1000.0 true 1.0 5 power_state 10.0 87.78 -474 525 15.52 16.41 640 640 1 0.0 0.0 0.0 false -129.998394137 129.998394137 0.05 10.0 0.01 20 0.005 true 20 base_scan base_laser_link -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 true base_link_geom 100.0 true 100.0 base_bumper true 100.0 base_link base_pose_ground_truth 0.01 map 25.7 25.7 0 0 0 0 base_footprint torso_lift_link_geom 100.0 true 100.0 torso_lift_bumper -52143.33 true 100.0 imu_link torso_lift_imu/data 2.89e-08 map 0 0 0 0 0 0 6.0 6.0 R8G8B8 2448 2050 45 0.1 100 20.0 true 20.0 /prosilica/image_raw /prosilica/camera_info /prosilica/request_image high_def_frame 1224.5 1224.5 1025.5 2955 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 640 480 BAYER_BGGR8 90 0.1 100 25.0 true 25.0 wide_stereo/left/image_raw wide_stereo/left/camera_info wide_stereo_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 BAYER_BGGR8 90 0.1 100 25.0 true 25.0 wide_stereo/right/image_raw wide_stereo/right/camera_info wide_stereo_optical_frame 0.09 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 45 0.1 100 25.0 true 25.0 narrow_stereo/left/image_raw narrow_stereo/left/camera_info narrow_stereo_optical_frame 0 320.5 320.5 240.5 772.55 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 45 0.1 100 25.0 true 25.0 narrow_stereo/right/image_raw narrow_stereo/right/camera_info narrow_stereo_optical_frame 0.09 320.5 320.5 240.5 772.55 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 640 1 0.0 0.0 0.0 false -79.9999999086 79.9999999086 0.05 10.0 0.01 20 0.005 true 20 tilt_scan laser_tilt_link -6.05 true 32.6525111499 r_shoulder_pan_link_geom 100.0 true 100.0 r_shoulder_pan_bumper true r_shoulder_lift_link_geom 100.0 true 100.0 r_r_shoulder_lift_bumper true 63.1552452977 61.8948225713 r_upper_arm_link_geom 100.0 true 100.0 r_upper_arm_bumper true true -90.5142857143 -1.0 r_elbow_flex_link_geom 100.0 true 100.0 r_elbow_flex_bumper true -36.167452007 true r_forearm_link_geom 100.0 true 100.0 r_forearm_bumper true r_wrist_flex_link_geom 100.0 true 100.0 r_wrist_flex_bumper true r_wrist_roll_link_geom 100.0 true 100.0 r_wrist_roll_bumper true r_gripper_l_finger_link_geom 100.0 true 100.0 r_gripper_l_finger_bumper true r_gripper_r_finger_link_geom 100.0 true 100.0 r_gripper_r_finger_bumper true false r_gripper_l_finger_tip_link_geom 100.0 true 100.0 r_gripper_l_finger_tip_bumper true false r_gripper_r_finger_tip_link_geom 100.0 true 100.0 r_gripper_r_finger_tip_bumper true 100.0 r_gripper_l_finger_link r_gripper_l_finger_pose_ground_truth 0.0 map true 100.0 r_gripper_l_finger_link r_gripper_l_finger_force_ground_truth r_gripper_l_finger_link true r_gripper_palm_link_geom 100.0 true 100.0 r_gripper_palm_bumper true true 0.01 0.0001 0 0 0.0001 0 0.0001 0 0 0 0.82025 0.188 0.790675 0 -0 0 0 0 0 0 -0 0 0.0 0.0 0.0 0 0 0 0 -0 0 0.0 0.0 0.0 unit_box PR2/White true false r_gripper_float_link r_gripper_palm_link r_gripper_float_link 1 0 0 -0.05 0.001 r_gripper_l_finger_tip_link r_gripper_float_link r_gripper_l_finger_tip_link 0 1 0 0 0 0 r_gripper_r_finger_tip_link r_gripper_float_link r_gripper_r_finger_tip_link 0 1 0 0 0 0 true 100.0 r_gripper_palm_link r_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map true 100.0 r_gripper_tool_frame r_gripper_tool_frame_pose_ground_truth 0 0 0 0 0 0 0.0 /map true true 32.6525111499 l_shoulder_pan_link_geom 100.0 true 100.0 l_shoulder_pan_bumper true l_shoulder_lift_link_geom 100.0 true 100.0 l_r_shoulder_lift_bumper true 63.1552452977 61.8948225713 l_upper_arm_link_geom 100.0 true 100.0 l_upper_arm_bumper true true -90.5142857143 -1.0 l_elbow_flex_link_geom 100.0 true 100.0 l_elbow_flex_bumper true -36.167452007 true l_forearm_link_geom 100.0 true 100.0 l_forearm_bumper true l_wrist_flex_link_geom 100.0 true 100.0 l_wrist_flex_bumper true l_wrist_roll_link_geom 100.0 true 100.0 l_wrist_roll_bumper true l_gripper_l_finger_link_geom 100.0 true 100.0 l_gripper_l_finger_bumper true l_gripper_r_finger_link_geom 100.0 true 100.0 l_gripper_r_finger_bumper true false l_gripper_l_finger_tip_link_geom 100.0 true 100.0 l_gripper_l_finger_tip_bumper true false l_gripper_r_finger_tip_link_geom 100.0 true 100.0 l_gripper_r_finger_tip_bumper true 100.0 l_gripper_l_finger_link l_gripper_l_finger_pose_ground_truth 0.0 map true 100.0 l_gripper_l_finger_link l_gripper_l_finger_force_ground_truth l_gripper_l_finger_link true l_gripper_palm_link_geom 100.0 true 100.0 l_gripper_palm_bumper true true 0.01 0.0001 0 0 0.0001 0 0.0001 0 0 0 0.82025 0.188 0.790675 0 -0 0 0 0 0 0 -0 0 0.0 0.0 0.0 0 0 0 0 -0 0 0.0 0.0 0.0 unit_box PR2/White true false l_gripper_float_link l_gripper_palm_link l_gripper_float_link 1 0 0 -0.05 0.001 l_gripper_l_finger_tip_link l_gripper_float_link l_gripper_l_finger_tip_link 0 1 0 0 0 0 l_gripper_r_finger_tip_link l_gripper_float_link l_gripper_r_finger_tip_link 0 1 0 0 0 0 true 100.0 l_gripper_palm_link l_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map true 100.0 l_gripper_tool_frame l_gripper_tool_frame_pose_ground_truth 0 0 0 0 0 0 0.0 /map true 640 480 L8 90 0.1 100 25.0 true 25.0 l_forearm_cam/image_raw l_forearm_cam/camera_info l_forearm_cam_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 90 0.1 100 25.0 true 25.0 r_forearm_cam/image_raw r_forearm_cam/camera_info r_forearm_cam_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue collada_urdf-1.12.13/collada_urdf/test/test_collada_urdf.cpp000066400000000000000000000043631370336760200240730ustar00rootroot00000000000000// 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: // // * Redistributions 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 Willow Garage, Inc. 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_urdf.h" #include TEST(collada_urdf, collada_from_urdf_file_works) { urdf::Model robot_model; ASSERT_TRUE(robot_model.initFile("pr2.urdf")); ASSERT_TRUE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "pr2.dae")); } TEST(collada_urdf, collada_output_dir_does_not_exist) { urdf::Model robot_model; ASSERT_TRUE(robot_model.initFile("pr2.urdf")); ASSERT_FALSE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "a/very/long/directory/path/that/should/not/exist/pr2.dae")); } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }