diff --git a/collada_parser/include/collada_parser/collada_parser.h b/collada_parser/include/collada_parser/collada_parser.h index e0c1145..84278ae 100644 --- a/collada_parser/include/collada_parser/collada_parser.h +++ b/collada_parser/include/collada_parser/collada_parser.h @@ -46,30 +46,8 @@ namespace urdf{ -class ColladaParser : public ModelInterface -{ -public: - /// \brief Load Model from string - bool initCollada(const std::string &xml_string ); - -protected: - /// non-const get Collada Link() - void getColladaLink(const std::string& name,boost::shared_ptr &link) const; - - /// non-const getColladaMaterial() - //boost::shared_ptr getColladaMaterial(const std::string& name) const; - - /// in initXml(), onece all links are loaded, - /// it's time to build a tree - bool initColladaTree(std::map &parent_link_tree); - - /// in initXml(), onece tree is built, - /// it's time to find the root Link - bool initColladaRoot(std::map &parent_link_tree); - - friend class ColladaModelReader; -}; + boost::shared_ptr parseCollada(const std::string &xml_string ); } diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp index b4e2f81..d221de5 100644 --- a/collada_parser/src/collada_parser.cpp +++ b/collada_parser/src/collada_parser.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* 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. -*********************************************************************/ + * 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 @@ -73,976 +73,976 @@ namespace urdf{ -class UnlinkFilename -{ -public: + class UnlinkFilename + { + public: UnlinkFilename(const std::string& filename) : _filename(filename) {} virtual ~UnlinkFilename() { unlink(_filename.c_str()); } std::string _filename; -}; -static std::list > _listTempFilenames; + }; + static std::list > _listTempFilenames; -class ColladaModelReader : public daeErrorHandler -{ + 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())); - } + 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(); } - daeElementRef pvisualtrans; - domAxis_constraintRef pkinematicaxis; - domCommon_float_or_paramRef jointvalue; - domNodeRef visualnode; - domKinematics_axis_infoRef kinematics_axis_info; - domMotion_axis_infoRef motion_axis_info; + 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 inter-collada bindings for a kinematics scene class KinematicsSceneBindings { public: - std::list< std::pair > listKinematicsVisualBindings; - std::list listAxisBindings; + std::list< std::pair > listKinematicsVisualBindings; + std::list listAxisBindings; - 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; + 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; - boost::shared_ptr p; ///< custom managed data + USERDATA() {} + USERDATA(double scale) : scale(scale) {} + double scale; + boost::shared_ptr p; ///< custom managed data }; enum GeomType { - GeomNone = 0, - GeomBox = 1, - GeomSphere = 2, - GeomCylinder = 3, - GeomTrimesh = 4, + 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; + 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 + ///< 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]; + // 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)); + 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 - }; + 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; + 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]); + } } - bool InitCollisionMesh(double fTessellation=1.0) - { - if( type == GeomTrimesh ) { - return true; + 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; + } } - 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 - }; + 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)); + } - 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; + 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(ColladaParser& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) { - daeErrorHandler::setErrorHandler(this); - _resourcedir = "."; + public: + ColladaModelReader(boost::shared_ptr model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) { + daeErrorHandler::setErrorHandler(this); + _resourcedir = "."; } virtual ~ColladaModelReader() { - _vuserdata.clear(); - _collada.reset(); - DAE::cleanup(); + _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 = _collada->open(filename); - if (!_dom) { - return false; - } - _filename=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 = _collada->open(filename); + if (!_dom) { + return false; + } + _filename=filename; - size_t maxchildren = _countChildren(_dom); - _vuserdata.resize(0); - _vuserdata.reserve(maxchildren); + 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(); + 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 = _collada->openFromMemory(".",pdata.c_str()); - if (!_dom) { - return false; - } + ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE)); + _collada.reset(new DAE); + _dom = _collada->openFromMemory(".",pdata.c_str()); + if (!_dom) { + return false; + } - size_t maxchildren = _countChildren(_dom); - _vuserdata.resize(0); - _vuserdata.reserve(maxchildren); + 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(); + double dScale = 1.0; + _processUserData(_dom, dScale); + ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren)); + return _Extract(); } -protected: + 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); - 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; - } - } - + _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); + 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 - if (!_model.initColladaTree(parent_link_tree)) { - ROS_ERROR("failed to build tree"); - } + std::map parent_link_tree; + // building tree: name mapping + if (!_model->initTree(parent_link_tree)) { + ROS_ERROR("failed to build tree"); + } - // find the root link - if (!_model.initColladaRoot(parent_link_tree)) { - ROS_ERROR("failed to find root link"); - } + // find the root link + if (!_model->initRoot(parent_link_tree)) { + ROS_ERROR("failed to find root link"); + } } /// \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; - } + 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))); - } + 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(); - } + // 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( !!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); } - if( !_ExtractArticulatedSystem(ias_new,bindings) ) { - return false; + else { + ROS_WARN_STREAM(str(boost::format("failed to find kinematics axis %s\n")%motion_axis_info->getAxis())); } + } } - 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); - } + 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; } - _ExtractRobotManipulators(articulated_system); - _ExtractRobotAttachedSensors(articulated_system); - 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); + } + } + + _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))); - } + 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; + // 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( _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.listAxisBindings)) { - ROS_WARN_STREAM(str(boost::format("failed to load kinbody from kinematics model %s\n")%kmodel->getID())); - return false; - } - return true; + if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings.listAxisBindings)) { + 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 std::list& listAxisBindings) { - 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())); + 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) { + _ExtractLink(ktec->getLink_array()[ilink], ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, listAxisBindings); + } + + // 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; } - // 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]); + // find the target joint + boost::shared_ptr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf); + if (!pjoint) { + continue; } - - // 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"); + + 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 { - vdomjoints.push_back(pelt); + ROS_WARN_STREAM(str(boost::format("unsupported formula element: %s\n")%pelt->getElementName())); } - } - - 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) { - _ExtractLink(ktec->getLink_array()[ilink], ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, listAxisBindings); - } - - // 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 - boost::shared_ptr 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; } - 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")); - boost::shared_ptr 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)); - } + 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")); + boost::shared_ptr 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; + } + return true; } /// \brief Extract Link info and add it to an existing body boost::shared_ptr _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector& vdomjoints, const std::list& 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(); - } - } - } - - boost::shared_ptr plink; - _model.getColladaLink(linkname,plink); - if( !plink ) { - plink.reset(new Link()); - plink->name = linkname; - plink->visual.reset(new Visual()); - _model.links_.insert(std::make_pair(linkname,plink)); - } - - _getUserData(pdomlink)->p = plink; + // 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 ) { - ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName())); + if (!!pdomnode->getName()) { + linkname = pdomnode->getName(); + } + if( linkname.size() == 0 && !!pdomnode->getID()) { + linkname = pdomnode->getID(); + } } + } - 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)); - 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); + boost::shared_ptr plink; + _model->getLink(linkname,plink); + if( !plink ) { + plink.reset(new Link()); + plink->name = linkname; + plink->visual.reset(new Visual()); + _model->links_.insert(std::make_pair(linkname,plink)); + } - // Get the geometry - _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,_poseMult(_poseMult(tParentWorldLink,tlink),plink->visual->origin)); + _getUserData(pdomlink)->p = plink; + if( !!pdomnode ) { + ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName())); + } + + 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)); + 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())); + 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]; + // 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)); + // get link kinematics transformation + Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull)); - // process attached links - daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt; - domJointRef pdomjoint = daeSafeCast (peltjoint); + // 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 boost::shared_ptr(); - } - - // 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; - } - } - - boost::shared_ptr pjoint(new Joint()); - pjoint->limits.reset(new JointLimits()); - 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; - _getUserData(pdomaxis)->p = boost::shared_ptr(new int(_model.joints_.size())); - _model.joints_[pjoint->name] = pjoint; - vjoints[ic] = pjoint; - } - - boost::shared_ptr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, listAxisBindings); - - 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)); - boost::shared_ptr pjoint = vjoints[ic]; - pjoint->child_link_name = pchildlink->name; - - // Axes and Anchor assignment. - pjoint->axis.x = pdomaxis->getAxis()->getValue()[0]; - pjoint->axis.y = pdomaxis->getAxis()->getValue()[1]; - pjoint->axis.z = pdomaxis->getAxis()->getValue()[2]; - - 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)); - } - } - } - - // 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; - } - } - - //ROS_INFO("joint %s axis: %f %f %f",pjoint->name.c_str(),pjoint->axis.x,pjoint->axis.y,pjoint->axis.z); - pjoint->parent_to_joint_origin_transform = tatt; - pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f; - pchildlink.reset(); - ++numjoints; - } + if (!pdomjoint) { + domInstance_jointRef pdomijoint = daeSafeCast (peltjoint); + if (!!pdomijoint) { + pdomjoint = daeSafeCast (pdomijoint->getUrl().getElement().cast()); } - } + } - 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"); - } + if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) { + ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint())); + return boost::shared_ptr(); + } - plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties); - return plink; + // 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; + } + } + + boost::shared_ptr pjoint(new Joint()); + pjoint->limits.reset(new JointLimits()); + 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; + _getUserData(pdomaxis)->p = boost::shared_ptr(new int(_model->joints_.size())); + _model->joints_[pjoint->name] = pjoint; + vjoints[ic] = pjoint; + } + + boost::shared_ptr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, listAxisBindings); + + 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)); + boost::shared_ptr pjoint = vjoints[ic]; + pjoint->child_link_name = pchildlink->name; + + // Axes and Anchor assignment. + pjoint->axis.x = pdomaxis->getAxis()->getValue()[0]; + pjoint->axis.y = pdomaxis->getAxis()->getValue()[1]; + pjoint->axis.z = pdomaxis->getAxis()->getValue()[2]; + + 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)); + } + } + } + + // 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; + } + } + + //ROS_INFO("joint %s axis: %f %f %f",pjoint->name.c_str(),pjoint->axis.x,pjoint->axis.y,pjoint->axis.z); + pjoint->parent_to_joint_origin_transform = tatt; + 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); + return plink; } boost::shared_ptr _CreateGeometry(const std::string& name, const std::list& listGeomProperties) { - boost::shared_ptr geometry(new Mesh()); - geometry->type = Geometry::MESH; - geometry->scale.x = 1; - geometry->scale.y = 1; - geometry->scale.z = 1; + boost::shared_ptr geometry(new Mesh()); + geometry->type = Geometry::MESH; + geometry->scale.x = 1; + geometry->scale.y = 1; + geometry->scale.z = 1; - std::vector vertices; - std::vector indices; - FOREACHC(it, listGeomProperties) { - int voffset = vertices.size(), ioffset = indices.size(); - vertices.resize(vertices.size()+it->vertices.size()); - for(size_t i = 0; i < it->vertices.size(); ++i) { - vertices[voffset+i] = _poseMult(it->_t, it->vertices[i]); - } - indices.resize(indices.size()+it->indices.size()); - for(size_t i = 0; i < it->indices.size(); ++i) { - indices[ioffset+i] = voffset+it->indices[i]; - } + std::vector vertices; + std::vector indices; + FOREACHC(it, listGeomProperties) { + int voffset = vertices.size(), ioffset = indices.size(); + vertices.resize(vertices.size()+it->vertices.size()); + for(size_t i = 0; i < it->vertices.size(); ++i) { + vertices[voffset+i] = _poseMult(it->_t, it->vertices[i]); } + indices.resize(indices.size()+it->indices.size()); + for(size_t i = 0; i < it->indices.size(); ++i) { + indices[ioffset+i] = voffset+it->indices[i]; + } + } - // 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\ + // 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\ @@ -1080,11 +1080,11 @@ protected: \n\ \n\ ")%(vertices.size()*3)); - // fill with vertices - FOREACH(it,vertices) { - daedata << it->x << " " << it->y << " " << it->z << " "; - } - daedata << str(boost::format("\n\ + // fill with vertices + FOREACH(it,vertices) { + daedata << it->x << " " << it->y << " " << it->z << " "; + } + daedata << str(boost::format("\n\ \n\ \n\ \n\ @@ -1100,11 +1100,11 @@ protected: \n\ \n\

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

\n\ + // fill with indices + FOREACH(it,indices) { + daedata << *it << " "; + } + daedata << str(boost::format("

\n\ \n\ \n\ \n\ @@ -1128,33 +1128,33 @@ protected: ")%name%name); #ifdef HAVE_MKSTEMPS - geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name); - int fd = mkstemps(&geometry->filename[0],4); + 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; - } - } + 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; + } + 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 @@ -1162,103 +1162,103 @@ protected: /// \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; + 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; } - ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName())); + _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! + } - // 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; - } + unsigned int nGeomBefore = listGeomProperties.size(); // #of Meshes already associated to this link - _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! + // 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; } - 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 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. + // 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(_getNodeParentTransform(pdomnode), _ExtractFullTransform(pdomnode))); + Pose tnodegeom; + Vector3 vscale(1,1,1); + _decompose(tmnodegeom, tnodegeom, vscale); + + // 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; } - - boost::array tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)), _poseMult(_getNodeParentTransform(pdomnode), _ExtractFullTransform(pdomnode))); - Pose tnodegeom; - Vector3 vscale(1,1,1); - _decompose(tmnodegeom, tnodegeom, vscale); - -// 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)); - } + 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 @@ -1266,28 +1266,28 @@ protected: /// \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]; - } - } + 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 @@ -1297,79 +1297,79 @@ protected: /// \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; + 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); } - 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; } - - 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; - } + if (offset> triangleIndexStride) { + triangleIndexStride = offset; } - triangleIndexStride++; + } + 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; + 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)); } - 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; + } } + } + 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; + } + 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 @@ -1378,89 +1378,89 @@ protected: /// \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(); + 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 @@ -1469,92 +1469,92 @@ protected: /// \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(); + 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 @@ -1563,78 +1563,78 @@ protected: /// \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;jgetMaterial() ) { + 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; + 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); } - 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; + 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"); } - 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(); + // 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); + // } - // 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++) { + 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) { - 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 ( 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)); } + } } + } } + } } - - if( vconvexhull.size()> 0 ) { - listGeomProperties.push_back(GEOMPROPERTIES()); - GEOMPROPERTIES& geom = listGeomProperties.back(); - geom.type = GeomTrimesh; - - //_computeConvexHull(vconvexhull,trimesh); - geom.InitCollisionMesh(); + } + } + 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)); + } + } } - return true; + } } - return false; + 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 manipulators void _ExtractRobotManipulators(const domArticulated_systemRef as) { - ROS_DEBUG("collada manipulators not supported yet"); + 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"); + ROS_DEBUG("collada sensors not supported yet"); } inline daeElementRef _getElementFromUrl(const daeURI &uri) { - return daeStandardURIResolver(*_collada).resolveElement(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; + 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. @@ -1826,360 +1826,360 @@ protected: /// \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())); + 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; - } + for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) { + daeElement* pelt = searchBinding(ref,paramArray[iikm].cast()); + if( !!pelt ) { + return pelt; } - return NULL; + } + 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; - } + for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) { + daeElement* pelt = searchBinding(ref,paramArray[iikm].cast()); + if( !!pelt ) { + return pelt; } - return NULL; + } + 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())); + 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())); + 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* 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"); } - 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())); - } - } + 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; + } + return false; } static boost::array _matrixIdentity() { - boost::array m = {{1,0,0,0,0,1,0,0,0,0,1,0}}; - return m; + 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"); - } - + 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)); + 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; + 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; + 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); + vscale.x = 1; vscale.y = 1; vscale.z = 1; + tout = _poseFromMatrix(tm); } virtual void handleError( daeString msg ) { - ROS_ERROR("COLLADA error: %s\n", msg); + ROS_ERROR("COLLADA error: %s\n", msg); } virtual void handleWarning( daeString msg ) { - ROS_WARN("COLLADA warning: %s\n", msg); + ROS_WARN("COLLADA warning: %s\n", msg); } inline static double _GetUnitScale(daeElement* pelt) { - return ((USERDATA*)pelt->getUserData())->scale; + 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]; - } + for(size_t i = 0; i < arr.getCount(); ++i) { + if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) { + return arr[i]; } - return domTechniqueRef(); + } + 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())); - } - } + 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(); + } + 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(); - } + std::string linkname; + if( !!pdomlink ) { + if( !!pdomlink->getName() ) { + linkname = pdomlink->getName(); } - return linkname; + 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; + 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; } boost::shared_ptr _getJointFromRef(xsToken targetref, daeElementRef peltref) { - daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt; - domJointRef pdomjoint = daeSafeCast (peltjoint); + 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) { + 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 boost::shared_ptr(); - } + if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) { + ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref)); + return boost::shared_ptr(); + } - boost::shared_ptr pjoint = _model.joints_[std::string(pdomjoint->getName())]; - if(!pjoint) { - ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName())); - } - return pjoint; + boost::shared_ptr pjoint = _model->joints_[std::string(pdomjoint->getName())]; + 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 @@ -2187,91 +2187,91 @@ protected: /// \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; + 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; } - 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; - } + // 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)); + // 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; } - // 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)); + 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)); + } } 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; + 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(); - } + // 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); - } + _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; + BOOST_ASSERT(!!pelt); + void* p = pelt->getUserData(); + BOOST_ASSERT(!!p); + return (USERDATA*)p; } // @@ -2280,232 +2280,232 @@ protected: 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; + 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; + 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; + 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; + 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; + 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; + 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)); + 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; + 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; + 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; + 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; + 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]); + 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 { - // 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]); - } + 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; + } + 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; + 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; + 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; + 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; + 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; + 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; @@ -2514,136 +2514,45 @@ protected: int _nGlobalSensorId, _nGlobalManipulatorId; std::string _filename; std::string _resourcedir; - ColladaParser& _model; + boost::shared_ptr _model; -}; + }; -bool urdfFromColladaFile(std::string const& daefilename, ColladaParser& model) -{ + bool urdfFromColladaFile(std::string const& daefilename, boost::shared_ptr model) + { ColladaModelReader reader(model); return reader.InitFromFile(daefilename); -} + } -bool urdfFromColladaData(std::string const& data, ColladaParser& model) -{ + bool urdfFromColladaData(std::string const& data, boost::shared_ptr model) + { ColladaModelReader reader(model); return reader.InitFromData(data); -} + } -bool IsColladaFile(const std::string& filename) -{ + bool IsColladaFile(const std::string& filename) + { size_t len = filename.size(); if( len < 4 ) - return false; - return filename[len-4] == '.' && ::tolower(filename[len-3]) == 'd' && ::tolower(filename[len-2]) == 'a' && ::tolower(filename[len-1]) == 'e'; -} - -bool IsColladaData(const std::string& data) -{ - return data.find(" &parent_link_tree) -{ - // loop through all joints, for every link, assign children links and children joints - for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) - { - std::string parent_link_name = joint->second->parent_link_name; - std::string child_link_name = joint->second->child_link_name; - - ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); - if (parent_link_name.empty() || child_link_name.empty()) - { - ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str()); return false; - } - else - { - // find child and parent links - boost::shared_ptr child_link, parent_link; - this->getColladaLink(child_link_name, child_link); - if (!child_link) - { - ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() ); - return false; - } - this->getColladaLink(parent_link_name, parent_link); - if (!parent_link) - { - ROS_ERROR(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() ); - return false; - } - - //set parent link for child link - child_link->setParent(parent_link); - - //set parent joint for child link - child_link->setParentJoint(joint->second); - - //set child joint for parent link - parent_link->addChildJoint(joint->second); - - //set child link for parent link - parent_link->addChild(child_link); - - // fill in child/parent string map - parent_link_tree[child_link->name] = parent_link_name; - - ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); - } + return filename[len-4] == '.' && ::tolower(filename[len-3]) == 'd' && ::tolower(filename[len-2]) == 'a' && ::tolower(filename[len-1]) == 'e'; } - return true; -} - - - -bool ColladaParser::initColladaRoot(std::map &parent_link_tree) -{ - - this->root_link_.reset(); - - // find the links that have no parent in the tree - for (std::map >::iterator l=this->links_.begin(); l!=this->links_.end(); l++) + bool IsColladaData(const std::string& data) { - std::map::iterator parent = parent_link_tree.find(l->first); - if (parent == parent_link_tree.end()) - { - // store root link - if (!this->root_link_) - { - getColladaLink(l->first, this->root_link_); - } - // we already found a root link - else{ - ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str()); - return false; - } - } + return data.find("root_link_) + + + + + boost::shared_ptr parseCollada(const std::string &xml_str) { - ROS_ERROR("No root link found. The robot xml is not a valid tree."); - return false; + boost::shared_ptr model(new ModelInterface); + + ColladaModelReader reader(model); + if (!reader.InitFromData(xml_str)) + model.reset(); + return model; } - ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str()); - - return true; -} - -void ColladaParser::getColladaLink(const std::string& name,boost::shared_ptr &link) const -{ - boost::shared_ptr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(); - else - ptr = this->links_.find(name)->second; - link = ptr; -} } diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 7c39b54..3dbcdff 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -21,7 +21,9 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) rosbuild_gensrv() +rosbuild_add_library(${PROJECT_NAME} src/model.cpp) + rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp) -rosbuild_add_gtest_build_flags(test_parser) target_link_libraries(test_parser ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_parser) rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch) diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h index 3fc4931..f75fc81 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -45,7 +45,7 @@ namespace urdf{ -class Model: public URDFParser, ColladaParser +class Model: public ModelInterface { public: /// \brief Load Model from TiXMLElement diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index 8b93437..ac4dd36 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -38,6 +38,8 @@ #include #include #include "urdf/model.h" +#include +#include namespace urdf{ @@ -49,7 +51,7 @@ bool Model::initFile(const std::string& filename) // get the entire file std::string xml_string; - std::fstream xml_file(filename, std::fstream::in); + std::fstream xml_file(filename.c_str(), std::fstream::in); if (xml_file.is_open()) { while ( xml_file.good() ) @@ -113,22 +115,36 @@ bool Model::initXml(TiXmlElement *robot_xml) } std::stringstream ss; - ss << *xml_doc; + ss << (*robot_xml); return Model::initString(ss.str()); } bool Model::initString(const std::string& xml_string) { + boost::shared_ptr model; + // necessary for COLLADA compatibility if( IsColladaData(xml_string) ) { ROS_DEBUG("Parsing robot collada xml string"); - return this->initCollada(xml_string); + model = parseCollada(xml_string); } else { ROS_DEBUG("Parsing robot urdf xml string"); - return this->initURDF(xml_string); + model = parseURDF(xml_string); } + + // copy data from model into this object + if (model){ + this->links_ = model->links_; + this->joints_ = model->joints_; + this->materials_ = model->materials_; + this->name_ = model->name_; + this->root_link_ = model->root_link_; + return true; + } + else + return false; } diff --git a/urdf_interface/include/urdf_interface/model.h b/urdf_interface/include/urdf_interface/model.h index 7416933..a12f3dd 100644 --- a/urdf_interface/include/urdf_interface/model.h +++ b/urdf_interface/include/urdf_interface/model.h @@ -88,14 +88,117 @@ public: this->root_link_.reset(); }; - /// \brief get parent Link of a Link given name - //virtual boost::shared_ptr getParentLink(const std::string& name) const = 0; - /// \brief get parent Joint of a Link given name - //virtual boost::shared_ptr getParentJoint(const std::string& name) const = 0; - /// \brief get child Link of a Link given name - //virtual boost::shared_ptr getChildLink(const std::string& name) const = 0; - /// \brief get child Joint of a Link given name - //virtual boost::shared_ptr getChildJoint(const std::string& name) const = 0; + /// non-const getLink() + void getLink(const std::string& name,boost::shared_ptr &link) const + { + boost::shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + link = ptr; + }; + + /// non-const getMaterial() + boost::shared_ptr getMaterial(const std::string& name) const + { + boost::shared_ptr ptr; + if (this->materials_.find(name) == this->materials_.end()) + ptr.reset(); + else + ptr = this->materials_.find(name)->second; + return ptr; + }; + + /// in initXml(), onece all links are loaded, + /// it's time to build a tree + bool initTree(std::map &parent_link_tree) + { + // loop through all joints, for every link, assign children links and children joints + for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) + { + std::string parent_link_name = joint->second->parent_link_name; + std::string child_link_name = joint->second->child_link_name; + + ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); + if (parent_link_name.empty() || child_link_name.empty()) + { + ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str()); + return false; + } + else + { + // find child and parent links + boost::shared_ptr child_link, parent_link; + this->getLink(child_link_name, child_link); + if (!child_link) + { + ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() ); + return false; + } + this->getLink(parent_link_name, parent_link); + if (!parent_link) + { + ROS_ERROR(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() ); + return false; + } + + //set parent link for child link + child_link->setParent(parent_link); + + //set parent joint for child link + child_link->setParentJoint(joint->second); + + //set child joint for parent link + parent_link->addChildJoint(joint->second); + + //set child link for parent link + parent_link->addChild(child_link); + + // fill in child/parent string map + parent_link_tree[child_link->name] = parent_link_name; + + ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); + } + } + + return true; + }; + + + /// in initXml(), onece tree is built, + /// it's time to find the root Link + bool initRoot(std::map &parent_link_tree) + { + this->root_link_.reset(); + + // find the links that have no parent in the tree + for (std::map >::iterator l=this->links_.begin(); l!=this->links_.end(); l++) + { + std::map::iterator parent = parent_link_tree.find(l->first); + if (parent == parent_link_tree.end()) + { + // store root link + if (!this->root_link_) + { + getLink(l->first, this->root_link_); + } + // we already found a root link + else{ + ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str()); + return false; + } + } + } + if (!this->root_link_) + { + ROS_ERROR("No root link found. The robot xml is not a valid tree."); + return false; + } + ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str()); + + return true; + }; /// \brief complete list of Links std::map > links_; @@ -112,6 +215,8 @@ public: /// hmm... boost::shared_ptr root_link_; + + }; } diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index ed3a363..b01a875 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -41,36 +41,12 @@ #include #include #include - #include namespace urdf{ -class URDFParser : public ModelInterface -{ -public: - URDFParser(); - - /// \brief Load Model from string - bool initURDF(const std::string &xml_string ); - -private: - /// non-const getLink() - void getURDFLink(const std::string& name,boost::shared_ptr &link) const; - - /// non-const getURDFMaterial() - boost::shared_ptr getURDFMaterial(const std::string& name) const; - - /// in initXml(), onece all links are loaded, - /// it's time to build a tree - bool initURDFTree(std::map &parent_link_tree); - - /// in initXml(), onece tree is built, - /// it's time to find the root Link - bool initURDFRoot(std::map &parent_link_tree); - -}; + boost::shared_ptr parseURDF(const std::string &xml_string); } diff --git a/urdf_parser/src/check_urdf.cpp b/urdf_parser/src/check_urdf.cpp index 0d919c2..46a9e43 100644 --- a/urdf_parser/src/check_urdf.cpp +++ b/urdf_parser/src/check_urdf.cpp @@ -70,8 +70,6 @@ int main(int argc, char** argv) return -1; } - URDFParser robot; - std::string xml_string; std::fstream xml_file(argv[1], std::fstream::in); while ( xml_file.good() ) @@ -82,17 +80,17 @@ int main(int argc, char** argv) } xml_file.close(); - if (!robot.initURDF(xml_string)){ + boost::shared_ptr robot = parseURDF(xml_string); + if (!robot){ std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; return -1; } - - std::cout << "robot name is: " << robot.getName() << std::endl; + std::cout << "robot name is: " << robot->getName() << std::endl; // get info from parser std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; // get root link - boost::shared_ptr root_link=robot.getRoot(); + boost::shared_ptr root_link=robot->getRoot(); if (!root_link) return -1; std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; diff --git a/urdf_parser/src/urdf_parser.cpp b/urdf_parser/src/urdf_parser.cpp index 51074fd..f00e7c4 100644 --- a/urdf_parser/src/urdf_parser.cpp +++ b/urdf_parser/src/urdf_parser.cpp @@ -42,14 +42,11 @@ namespace urdf{ -URDFParser::URDFParser() -{ - this->clear(); -} -bool URDFParser::initURDF(const std::string &xml_string) +boost::shared_ptr parseURDF(const std::string &xml_string) { - this->clear(); + boost::shared_ptr model(new ModelInterface); + model->clear(); TiXmlDocument xml_doc; xml_doc.Parse(xml_string.c_str()); @@ -58,20 +55,19 @@ bool URDFParser::initURDF(const std::string &xml_string) if (!robot_xml) { ROS_ERROR("Could not find the 'robot' element in the xml file"); - return false; + model.reset(); + return model; } - ROS_DEBUG("Parsing robot xml"); - if (!robot_xml) return false; - // Get robot name const char *name = robot_xml->Attribute("name"); if (!name) { ROS_ERROR("No name given for the robot."); - return false; + model.reset(); + return model; } - this->name_ = std::string(name); + model->name_ = std::string(name); // Get all Material elements for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) @@ -81,15 +77,16 @@ bool URDFParser::initURDF(const std::string &xml_string) if (material->initXml(material_xml)) { - if (this->getURDFMaterial(material->name)) + if (model->getMaterial(material->name)) { ROS_ERROR("material '%s' is not unique.", material->name.c_str()); material.reset(); - return false; + model.reset(); + return model; } else { - this->materials_.insert(make_pair(material->name,material)); + model->materials_.insert(make_pair(material->name,material)); ROS_DEBUG("successfully added a new material '%s'", material->name.c_str()); } } @@ -97,7 +94,8 @@ bool URDFParser::initURDF(const std::string &xml_string) { ROS_ERROR("material xml is not initialized correctly"); material.reset(); - return false; + model.reset(); + return model; } } @@ -109,11 +107,11 @@ bool URDFParser::initURDF(const std::string &xml_string) if (link->initXml(link_xml)) { - if (this->getLink(link->name)) + if (model->getLink(link->name)) { ROS_ERROR("link '%s' is not unique.", link->name.c_str()); - link.reset(); - return false; + model.reset(); + return model; } else { @@ -123,42 +121,43 @@ bool URDFParser::initURDF(const std::string &xml_string) { if (!link->visual->material_name.empty()) { - if (this->getURDFMaterial(link->visual->material_name)) + if (model->getMaterial(link->visual->material_name)) { ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); - link->visual->material = this->getURDFMaterial( link->visual->material_name.c_str() ); + link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); } else { if (link->visual->material) { ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); - this->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); + model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); } else { ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); - link.reset(); - return false; + model.reset(); + return model; } } } } - this->links_.insert(make_pair(link->name,link)); + model->links_.insert(make_pair(link->name,link)); ROS_DEBUG("successfully added a new link '%s'", link->name.c_str()); } } else { ROS_ERROR("link xml is not initialized correctly"); - link.reset(); - return false; + model.reset(); + return model; } } - if (this->links_.empty()){ + if (model->links_.empty()){ ROS_ERROR("No link elements found in urdf file"); - return false; + model.reset(); + return model; } // Get all Joint elements @@ -169,23 +168,23 @@ bool URDFParser::initURDF(const std::string &xml_string) if (joint->initXml(joint_xml)) { - if (this->getJoint(joint->name)) + if (model->getJoint(joint->name)) { ROS_ERROR("joint '%s' is not unique.", joint->name.c_str()); - joint.reset(); - return false; + model.reset(); + return model; } else { - this->joints_.insert(make_pair(joint->name,joint)); + model->joints_.insert(make_pair(joint->name,joint)); ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str()); } } else { ROS_ERROR("joint xml is not initialized correctly"); - joint.reset(); - return false; + model.reset(); + return model; } } @@ -196,128 +195,22 @@ bool URDFParser::initURDF(const std::string &xml_string) parent_link_tree.clear(); // building tree: name mapping - if (!this->initURDFTree(parent_link_tree)) + if (!model->initTree(parent_link_tree)) { ROS_ERROR("failed to build tree"); - return false; + model.reset(); + return model; } // find the root link - if (!this->initURDFRoot(parent_link_tree)) + if (!model->initRoot(parent_link_tree)) { ROS_ERROR("failed to find root link"); - return false; + model.reset(); + return model; } - return true; -} - -bool URDFParser::initURDFTree(std::map &parent_link_tree) -{ - // loop through all joints, for every link, assign children links and children joints - for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) - { - std::string parent_link_name = joint->second->parent_link_name; - std::string child_link_name = joint->second->child_link_name; - - ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); - if (parent_link_name.empty() || child_link_name.empty()) - { - ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str()); - return false; - } - else - { - // find child and parent links - boost::shared_ptr child_link, parent_link; - this->getURDFLink(child_link_name, child_link); - if (!child_link) - { - ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() ); - return false; - } - this->getURDFLink(parent_link_name, parent_link); - if (!parent_link) - { - ROS_ERROR(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() ); - return false; - } - - //set parent link for child link - child_link->setParent(parent_link); - - //set parent joint for child link - child_link->setParentJoint(joint->second); - - //set child joint for parent link - parent_link->addChildJoint(joint->second); - - //set child link for parent link - parent_link->addChild(child_link); - - // fill in child/parent string map - parent_link_tree[child_link->name] = parent_link_name; - - ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); - } - } - - return true; -} - - - -bool URDFParser::initURDFRoot(std::map &parent_link_tree) -{ - - this->root_link_.reset(); - - // find the links that have no parent in the tree - for (std::map >::iterator l=this->links_.begin(); l!=this->links_.end(); l++) - { - std::map::iterator parent = parent_link_tree.find(l->first); - if (parent == parent_link_tree.end()) - { - // store root link - if (!this->root_link_) - { - getURDFLink(l->first, this->root_link_); - } - // we already found a root link - else{ - ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str()); - return false; - } - } - } - if (!this->root_link_) - { - ROS_ERROR("No root link found. The robot xml is not a valid tree."); - return false; - } - ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str()); - - return true; -} - -boost::shared_ptr URDFParser::getURDFMaterial(const std::string& name) const -{ - boost::shared_ptr ptr; - if (this->materials_.find(name) == this->materials_.end()) - ptr.reset(); - else - ptr = this->materials_.find(name)->second; - return ptr; -} - -void URDFParser::getURDFLink(const std::string& name,boost::shared_ptr &link) const -{ - boost::shared_ptr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(); - else - ptr = this->links_.find(name)->second; - link = ptr; + return model; } } diff --git a/urdf_parser/src/urdf_to_graphiz.cpp b/urdf_parser/src/urdf_to_graphiz.cpp index 9f4eee1..55c0f4d 100644 --- a/urdf_parser/src/urdf_to_graphiz.cpp +++ b/urdf_parser/src/urdf_to_graphiz.cpp @@ -90,8 +90,6 @@ int main(int argc, char** argv) return -1; } - URDFParser robot; - // get the entire file std::string xml_string; std::fstream xml_file(argv[1], std::fstream::in); @@ -103,14 +101,15 @@ int main(int argc, char** argv) } xml_file.close(); - if (!robot.initURDF(xml_string)){ + boost::shared_ptr robot = parseURDF(xml_string); + if (!robot){ std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; return -1; } - string output = robot.getName(); + string output = robot->getName(); // print entire tree to file - printTree(robot.getRoot(), output+".gv"); + printTree(robot->getRoot(), output+".gv"); cout << "Created file " << output << ".gv" << endl; string command = "dot -Tpdf "+output+".gv -o "+output+".pdf"; diff --git a/urdf_parser/test/memtest.cpp b/urdf_parser/test/memtest.cpp index bfde1c6..c16871b 100644 --- a/urdf_parser/test/memtest.cpp +++ b/urdf_parser/test/memtest.cpp @@ -1,4 +1,4 @@ -#include +#include "urdf_parser/urdf_parser.h" #include #include #include @@ -6,8 +6,6 @@ int main(int argc, char** argv){ ros::init(argc, argv, "memtest"); while (ros::ok()){ - urdf::URDFParser urdf; - std::string xml_string; std::fstream xml_file(argv[1], std::fstream::in); while ( xml_file.good() ) @@ -19,6 +17,6 @@ int main(int argc, char** argv){ xml_file.close(); - urdf.initURDF(xml_string); + urdf::parseURDF(xml_string); } }