diff --git a/README.md b/README.md new file mode 100644 index 0000000..ac6f946 --- /dev/null +++ b/README.md @@ -0,0 +1,14 @@ +# Robot Model + +`robot_model` contains packages for modeling various aspects of robot information, specified in the Xml Robot Description Format (URDF). +The core package of this stack is urdf, which parses URDF files, and constructs an object model (C++) of the robot. + +## Deprecation +This repository and the `robot_model` metapackage are deprecated. +The other packages will continue to be maintained, but are being moved to new repositories. +More information is available at [`ros/robot_model#195`](https://github.com/ros/robot_model/issues/195). + +**Moved Repos** + +* `collada_urdf` and `collada_parser` + * [`ros/collada_urdf`](https://github.com/ros/collada_urdf) diff --git a/collada_parser/CHANGELOG.rst b/collada_parser/CHANGELOG.rst deleted file mode 100644 index 7bd53d0..0000000 --- a/collada_parser/CHANGELOG.rst +++ /dev/null @@ -1,93 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package collada_parser -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -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) --------------------- diff --git a/collada_parser/CMakeLists.txt b/collada_parser/CMakeLists.txt deleted file mode 100644 index 961ac9c..0000000 --- a/collada_parser/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(collada_parser) - -find_package(Boost REQUIRED system) - -find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin roscpp class_loader urdf) -find_package(urdfdom_headers REQUIRED) - -add_compile_options(-std=c++11) - -catkin_package( - LIBRARIES ${PROJECT_NAME} - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp urdf_parser_plugin - DEPENDS urdfdom_headers -) - -include_directories(${Boost_INCLUDE_DIR}) -include_directories(include ${catkin_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS}) - -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake-extensions/) -find_package(PkgConfig) -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() - -# 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}) diff --git a/collada_parser/collada_parser_plugin_description.xml b/collada_parser/collada_parser_plugin_description.xml deleted file mode 100644 index 57f16b7..0000000 --- a/collada_parser/collada_parser_plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Parse models as URDF from Collada files. - - - diff --git a/collada_parser/include/collada_parser/collada_parser.h b/collada_parser/include/collada_parser/collada_parser.h deleted file mode 100644 index 8c1e052..0000000 --- a/collada_parser/include/collada_parser/collada_parser.h +++ /dev/null @@ -1,54 +0,0 @@ -/********************************************************************* -* 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 -#include - -#include - - -namespace urdf { - -/// \brief Load Model from string -urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string ); - -} - -#endif diff --git a/collada_parser/include/collada_parser/collada_parser_plugin.h b/collada_parser/include/collada_parser/collada_parser_plugin.h deleted file mode 100644 index ac699c0..0000000 --- a/collada_parser/include/collada_parser/collada_parser_plugin.h +++ /dev/null @@ -1,54 +0,0 @@ -/********************************************************************* -* 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 - -namespace urdf -{ - -class ColladaURDFParser : public URDFParser -{ -public: - - virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string); -}; - -} - -#endif diff --git a/collada_parser/mainpage.dox b/collada_parser/mainpage.dox deleted file mode 100644 index 643964d..0000000 --- a/collada_parser/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b collada_parser is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/collada_parser/package.xml b/collada_parser/package.xml deleted file mode 100644 index 88cca79..0000000 --- a/collada_parser/package.xml +++ /dev/null @@ -1,45 +0,0 @@ - - collada_parser - 1.12.9 - - 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/robot_model - https://github.com/ros/robot_model/issues - - catkin - - collada-dom - liburdfdom-headers-dev - roscpp - urdf_parser_plugin - class_loader - urdf - - collada-dom - liburdfdom-headers-dev - roscpp - urdf_parser_plugin - class_loader - - - - - - - diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp deleted file mode 100644 index ef5fde1..0000000 --- a/collada_parser/src/collada_parser.cpp +++ /dev/null @@ -1,2831 +0,0 @@ -/********************************************************************* -* 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 - -#ifndef HAVE_MKSTEMPS -#include -#include -#endif -#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; -} -} diff --git a/collada_parser/src/collada_parser_plugin.cpp b/collada_parser/src/collada_parser_plugin.cpp deleted file mode 100644 index d8f5369..0000000 --- a/collada_parser/src/collada_parser_plugin.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/********************************************************************* -* 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) diff --git a/collada_urdf/CHANGELOG.rst b/collada_urdf/CHANGELOG.rst deleted file mode 100644 index 2bfb306..0000000 --- a/collada_urdf/CHANGELOG.rst +++ /dev/null @@ -1,122 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package collada_urdf -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -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 `_ diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt deleted file mode 100644 index d230de8..0000000 --- a/collada_urdf/CMakeLists.txt +++ /dev/null @@ -1,88 +0,0 @@ -cmake_minimum_required(VERSION 2.8.12) -project(collada_urdf) - -find_package(catkin REQUIRED COMPONENTS angles collada_parser resource_retriever urdf geometric_shapes tf cmake_modules) - -find_package(TinyXML REQUIRED) - -catkin_package( - LIBRARIES ${PROJECT_NAME} - INCLUDE_DIRS include - DEPENDS angles collada_parser resource_retriever urdf geometric_shapes tf) - -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(COLLADA_DOM 2.3 COMPONENTS 1.5) -if( COLLADA_DOM_FOUND ) - include_directories(${COLLADA_DOM_INCLUDE_DIRS}) - link_directories(${COLLADA_DOM_LIBRARY_DIRS}) -endif() - -include_directories(${TinyXML_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) -link_directories(${catkin_LIBRARY_DIRS}) - -add_library(${PROJECT_NAME} src/collada_urdf.cpp) -target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${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}) diff --git a/collada_urdf/include/collada_urdf/collada_urdf.h b/collada_urdf/include/collada_urdf/collada_urdf.h deleted file mode 100644 index d54923d..0000000 --- a/collada_urdf/include/collada_urdf/collada_urdf.h +++ /dev/null @@ -1,69 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redstributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Authors: Tim Field */ - -#ifndef COLLADA_URDF_COLLADA_URDF_H -#define COLLADA_URDF_COLLADA_URDF_H - -#include -#include - -#ifndef _WIN32 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#include -#pragma GCC diagnostic pop -#endif - -#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 diff --git a/collada_urdf/package.xml b/collada_urdf/package.xml deleted file mode 100644 index 1bf480d..0000000 --- a/collada_urdf/package.xml +++ /dev/null @@ -1,49 +0,0 @@ - - collada_urdf - 1.12.9 - - 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 - - catkin - - angles - assimp-dev - resource_retriever - collada-dom - collada_parser - liburdfdom-dev - liburdfdom-headers-dev - roscpp - urdf - geometric_shapes - tf - cmake_modules - - angles - assimp - collada-dom - collada_parser - liburdfdom-dev - liburdfdom-headers-dev - resource_retriever - roscpp - urdf - tf - geometric_shapes - - diff --git a/collada_urdf/src/collada_to_urdf.cpp b/collada_urdf/src/collada_to_urdf.cpp deleted file mode 100644 index 616f58a..0000000 --- a/collada_urdf/src/collada_to_urdf.cpp +++ /dev/null @@ -1,657 +0,0 @@ -/* Author: Yohei Kakiuchi */ -#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 -#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_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.x, - link->inertial->origin.rotation.y, - link->inertial->origin.rotation.z, - link->inertial->origin.rotation.w); - tf::Matrix3x3 mat (qt); - tf::Matrix3x3 tmat (mat.transpose()); - tf::Matrix3x3 imat (link->inertial->ixx, link->inertial->ixy, link->inertial->ixz, - link->inertial->ixy, link->inertial->iyy, link->inertial->iyz, - link->inertial->ixz, link->inertial->iyz, 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 ) { - 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; -#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_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_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; -} diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp deleted file mode 100644 index bef2c0c..0000000 --- a/collada_urdf/src/collada_urdf.cpp +++ /dev/null @@ -1,1948 +0,0 @@ -// -*- 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 - -#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. -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); -} - -} diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp deleted file mode 100644 index e2eccd1..0000000 --- a/collada_urdf/src/urdf_to_collada.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redstributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Tim Field */ - -#ifndef _WIN32 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#include "collada_urdf/collada_urdf.h" -#pragma GCC diagnostic pop -#endif - -#include - -int main(int argc, char** argv) -{ - if (argc != 3) { - std::cerr << "Usage: urdf_to_collada input.urdf output.dae" << std::endl; - return -1; - } - - ros::init(argc, argv, "urdf_to_collada"); - - std::string input_filename(argv[1]); - std::string output_filename(argv[2]); - - urdf::Model robot_model; - if( !robot_model.initFile(input_filename) ) { - ROS_ERROR("failed to open urdf file %s", input_filename.c_str()); - return -2; - } - - collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename); - std::cout << std::endl << "Document successfully written to " << output_filename << std::endl; - - return 0; -} diff --git a/collada_urdf/test/pr2.urdf b/collada_urdf/test/pr2.urdf deleted file mode 100644 index 17d8633..0000000 --- a/collada_urdf/test/pr2.urdf +++ /dev/null @@ -1,3389 +0,0 @@ - - - - - - - - - - - - - - - - - - - - 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 - - - diff --git a/collada_urdf/test/test_collada_urdf.cpp b/collada_urdf/test/test_collada_urdf.cpp deleted file mode 100644 index 1542834..0000000 --- a/collada_urdf/test/test_collada_urdf.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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 - -#include -#include -#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(); -}