collada_parser.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, University of Tokyo
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redstributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Rosen Diankov, used OpenRAVE files for reference  */
00036 #include <vector>
00037 #include <list>
00038 #include <map>
00039 #include <stdint.h>
00040 #include <cstdlib>
00041 #include <cmath>
00042 #include <string>
00043 #include <sstream>
00044 
00045 #include <dae.h>
00046 #include <dae/daeErrorHandler.h>
00047 #include <dom/domCOLLADA.h>
00048 #include <dae/domAny.h>
00049 #include <dom/domConstants.h>
00050 #include <dom/domTriangles.h>
00051 #include <dae/daeStandardURIResolver.h>
00052 
00053 #include <boost/assert.hpp>
00054 #include <boost/format.hpp>
00055 #include <boost/shared_ptr.hpp>
00056 
00057 #include <ros/ros.h>
00058 #include <collada_parser/collada_parser.h>
00059 #include <urdf_model/model.h>
00060 
00061 #ifndef HAVE_MKSTEMPS
00062 #include <fstream>
00063 #include <fcntl.h>
00064 #endif
00065 #ifndef HAVE_MKSTEMPS
00066 #include <fstream>
00067 #include <fcntl.h>
00068 #endif
00069 
00070 #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++)
00071 #define FOREACHC FOREACH
00072 
00073 namespace ColladaDOM150 { }
00074 
00075 namespace urdf {
00076 
00077 using namespace ColladaDOM150;
00078 
00079 class UnlinkFilename
00080 {
00081 public:
00082     UnlinkFilename(const std::string& filename) : _filename(filename) {
00083     }
00084     virtual ~UnlinkFilename() {
00085         unlink(_filename.c_str());
00086     }
00087     std::string _filename;
00088 };
00089 static std::list<boost::shared_ptr<UnlinkFilename> > _listTempFilenames;
00090 
00091 class ColladaModelReader : public daeErrorHandler
00092 {
00093 
00094     class JointAxisBinding
00095     {
00096 public:
00097         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) {
00098             BOOST_ASSERT( !!pkinematicaxis );
00099             daeElement* pae = pvisualtrans->getParentElement();
00100             while (!!pae) {
00101                 visualnode = daeSafeCast<domNode> (pae);
00102                 if (!!visualnode) {
00103                     break;
00104                 }
00105                 pae = pae->getParentElement();
00106             }
00107 
00108             if (!visualnode) {
00109                 ROS_WARN_STREAM(str(boost::format("couldn't find parent node of element id %s, sid %s\n")%pkinematicaxis->getID()%pkinematicaxis->getSid()));
00110             }
00111         }
00112 
00113         daeElementRef pvisualtrans;
00114         domAxis_constraintRef pkinematicaxis;
00115         domCommon_float_or_paramRef jointvalue;
00116         domNodeRef visualnode;
00117         domKinematics_axis_infoRef kinematics_axis_info;
00118         domMotion_axis_infoRef motion_axis_info;
00119     };
00120 
00122     class LinkBinding
00123     {
00124 public:
00125         domNodeRef node;
00126         domLinkRef domlink;
00127         domInstance_rigid_bodyRef irigidbody;
00128         domRigid_bodyRef rigidbody;
00129         domNodeRef nodephysicsoffset;
00130     };
00131 
00133     class KinematicsSceneBindings
00134     {
00135 public:
00136         std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings;
00137         std::list<JointAxisBinding> listAxisBindings;
00138         std::list<LinkBinding> listLinkBindings;
00139 
00140         bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
00141         {
00142             if( !kinematics_axis_info ) {
00143                 return false;
00144             }
00145             for(size_t ik = 0; ik < arr.getCount(); ++ik) {
00146                 daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt;
00147                 if( !!pelt ) {
00148                     // look for the correct placement
00149                     bool bfound = false;
00150                     FOREACH(itbinding,listAxisBindings) {
00151                         if( itbinding->pkinematicaxis.cast() == pelt ) {
00152                             itbinding->kinematics_axis_info = kinematics_axis_info;
00153                             if( !!motion_axis_info ) {
00154                                 itbinding->motion_axis_info = motion_axis_info;
00155                             }
00156                             bfound = true;
00157                             break;
00158                         }
00159                     }
00160                     if( !bfound ) {
00161                         ROS_WARN_STREAM(str(boost::format("could not find binding for axis: %s, %s\n")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid")));
00162                         return false;
00163                     }
00164                     return true;
00165                 }
00166             }
00167             ROS_WARN_STREAM(str(boost::format("could not find kinematics axis target: %s\n")%kinematics_axis_info->getAxis()));
00168             return false;
00169         }
00170     };
00171 
00172     struct USERDATA
00173     {
00174         USERDATA() {
00175         }
00176         USERDATA(double scale) : scale(scale) {
00177         }
00178         double scale;
00179         boost::shared_ptr<void> p; 
00180     };
00181 
00182     enum GeomType {
00183         GeomNone = 0,
00184         GeomBox = 1,
00185         GeomSphere = 2,
00186         GeomCylinder = 3,
00187         GeomTrimesh = 4,
00188     };
00189 
00190     struct GEOMPROPERTIES
00191     {
00192         Pose _t;                
00193         Vector3 vGeomData; 
00194 
00195 
00196 
00197         Color diffuseColor, ambientColor; 
00198         std::vector<Vector3> vertices;
00199         std::vector<int> indices;
00200 
00202         GeomType type;         
00203 
00204 
00205         // generate a sphere triangulation starting with an icosahedron
00206         // all triangles are oriented counter clockwise
00207         static void GenerateSphereTriangulation(std::vector<Vector3> realvertices, std::vector<int> realindices, int levels)
00208         {
00209             const double GTS_M_ICOSAHEDRON_X = 0.850650808352039932181540497063011072240401406;
00210             const double GTS_M_ICOSAHEDRON_Y = 0.525731112119133606025669084847876607285497935;
00211             const double GTS_M_ICOSAHEDRON_Z  = 0;
00212             std::vector<Vector3> tempvertices[2];
00213             std::vector<int> tempindices[2];
00214 
00215             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
00216             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00217             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
00218             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
00219             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00220             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
00221             tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
00222             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
00223             tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00224             tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
00225             tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00226             tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
00227 
00228             const int nindices=60;
00229             int indices[nindices] = {
00230                 0, 1, 2,
00231                 1, 3, 4,
00232                 3, 5, 6,
00233                 2, 4, 7,
00234                 5, 6, 8,
00235                 2, 7, 9,
00236                 0, 5, 8,
00237                 7, 9, 10,
00238                 0, 1, 5,
00239                 7, 10, 11,
00240                 1, 3, 5,
00241                 6, 10, 11,
00242                 3, 6, 11,
00243                 9, 10, 8,
00244                 3, 4, 11,
00245                 6, 8, 10,
00246                 4, 7, 11,
00247                 1, 2, 4,
00248                 0, 8, 9,
00249                 0, 2, 9
00250             };
00251 
00252             Vector3 v[3];
00253             // make sure oriented CCW
00254             for(int i = 0; i < nindices; i += 3 ) {
00255                 v[0] = tempvertices[0][indices[i]];
00256                 v[1] = tempvertices[0][indices[i+1]];
00257                 v[2] = tempvertices[0][indices[i+2]];
00258                 if( _dot3(v[0], _cross3(_sub3(v[1],v[0]),_sub3(v[2],v[0]))) < 0 ) {
00259                     std::swap(indices[i], indices[i+1]);
00260                 }
00261             }
00262 
00263             tempindices[0].resize(nindices);
00264             std::copy(&indices[0],&indices[nindices],tempindices[0].begin());
00265             std::vector<Vector3>* curvertices = &tempvertices[0], *newvertices = &tempvertices[1];
00266             std::vector<int> *curindices = &tempindices[0], *newindices = &tempindices[1];
00267             while(levels-- > 0) {
00268 
00269                 newvertices->resize(0);
00270                 newvertices->reserve(2*curvertices->size());
00271                 newvertices->insert(newvertices->end(), curvertices->begin(), curvertices->end());
00272                 newindices->resize(0);
00273                 newindices->reserve(4*curindices->size());
00274 
00275                 std::map< uint64_t, int > mapnewinds;
00276                 std::map< uint64_t, int >::iterator it;
00277 
00278                 for(size_t i = 0; i < curindices->size(); i += 3) {
00279                     // for ever tri, create 3 new vertices and 4 new triangles.
00280                     v[0] = curvertices->at(curindices->at(i));
00281                     v[1] = curvertices->at(curindices->at(i+1));
00282                     v[2] = curvertices->at(curindices->at(i+2));
00283 
00284                     int inds[3];
00285                     for(int j = 0; j < 3; ++j) {
00286                         uint64_t key = ((uint64_t)curindices->at(i+j)<<32)|(uint64_t)curindices->at(i + ((j+1)%3));
00287                         it = mapnewinds.find(key);
00288 
00289                         if( it == mapnewinds.end() ) {
00290                             inds[j] = mapnewinds[key] = mapnewinds[(key<<32)|(key>>32)] = (int)newvertices->size();
00291                             newvertices->push_back(_normalize3(_add3(v[j],v[(j+1)%3 ])));
00292                         }
00293                         else {
00294                             inds[j] = it->second;
00295                         }
00296                     }
00297 
00298                     newindices->push_back(curindices->at(i));    newindices->push_back(inds[0]);    newindices->push_back(inds[2]);
00299                     newindices->push_back(inds[0]);    newindices->push_back(curindices->at(i+1));    newindices->push_back(inds[1]);
00300                     newindices->push_back(inds[2]);    newindices->push_back(inds[0]);    newindices->push_back(inds[1]);
00301                     newindices->push_back(inds[2]);    newindices->push_back(inds[1]);    newindices->push_back(curindices->at(i+2));
00302                 }
00303 
00304                 std::swap(newvertices,curvertices);
00305                 std::swap(newindices,curindices);
00306             }
00307 
00308             realvertices = *curvertices;
00309             realindices = *curindices;
00310         }
00311 
00312         bool InitCollisionMesh(double fTessellation=1.0)
00313         {
00314             if( type == GeomTrimesh ) {
00315                 return true;
00316             }
00317             indices.clear();
00318             vertices.clear();
00319 
00320             if( fTessellation < 0.01f ) {
00321                 fTessellation = 0.01f;
00322             }
00323             // start tesselating
00324             switch(type) {
00325             case GeomSphere: {
00326                 // log_2 (1+ tess)
00327                 GenerateSphereTriangulation(vertices,indices, 3 + (int)(logf(fTessellation) / logf(2.0f)) );
00328                 double fRadius = vGeomData.x;
00329                 FOREACH(it, vertices) {
00330                     it->x *= fRadius;
00331                     it->y *= fRadius;
00332                     it->z *= fRadius;
00333                 }
00334                 break;
00335             }
00336             case GeomBox: {
00337                 // trivial
00338                 Vector3 ex = vGeomData;
00339                 Vector3 v[8] = { Vector3(ex.x, ex.y, ex.z),
00340                                  Vector3(ex.x, ex.y, -ex.z),
00341                                  Vector3(ex.x, -ex.y, ex.z),
00342                                  Vector3(ex.x, -ex.y, -ex.z),
00343                                  Vector3(-ex.x, ex.y, ex.z),
00344                                  Vector3(-ex.x, ex.y, -ex.z),
00345                                  Vector3(-ex.x, -ex.y, ex.z),
00346                                  Vector3(-ex.x, -ex.y, -ex.z) };
00347                 const int nindices = 36;
00348                 int startindices[] = {
00349                     0, 1, 2,
00350                     1, 2, 3,
00351                     4, 5, 6,
00352                     5, 6, 7,
00353                     0, 1, 4,
00354                     1, 4, 5,
00355                     2, 3, 6,
00356                     3, 6, 7,
00357                     0, 2, 4,
00358                     2, 4, 6,
00359                     1, 3, 5,
00360                     3, 5, 7
00361                 };
00362 
00363                 for(int i = 0; i < nindices; i += 3 ) {
00364                     Vector3 v1 = v[startindices[i]];
00365                     Vector3 v2 = v[startindices[i+1]];
00366                     Vector3 v3 = v[startindices[i+2]];
00367                     if( _dot3(v1, _sub3(v2,_cross3(v1, _sub3(v3,v1)))) < 0 ) {
00368                         std::swap(indices[i], indices[i+1]);
00369                     }
00370                 }
00371 
00372                 vertices.resize(8);
00373                 std::copy(&v[0],&v[8],vertices.begin());
00374                 indices.resize(nindices);
00375                 std::copy(&startindices[0],&startindices[nindices],indices.begin());
00376                 break;
00377             }
00378             case GeomCylinder: {
00379                 // cylinder is on y axis
00380                 double rad = vGeomData.x, len = vGeomData.y*0.5f;
00381 
00382                 int numverts = (int)(fTessellation*24.0f) + 3;
00383                 double dtheta = 2 * M_PI / (double)numverts;
00384                 vertices.push_back(Vector3(0,0,len));
00385                 vertices.push_back(Vector3(0,0,-len));
00386                 vertices.push_back(Vector3(rad,0,len));
00387                 vertices.push_back(Vector3(rad,0,-len));
00388 
00389                 for(int i = 0; i < numverts+1; ++i) {
00390                     double s = rad * std::sin(dtheta * (double)i);
00391                     double c = rad * std::cos(dtheta * (double)i);
00392 
00393                     int off = (int)vertices.size();
00394                     vertices.push_back(Vector3(c, s, len));
00395                     vertices.push_back(Vector3(c, s, -len));
00396 
00397                     indices.push_back(0);       indices.push_back(off);       indices.push_back(off-2);
00398                     indices.push_back(1);       indices.push_back(off-1);       indices.push_back(off+1);
00399                     indices.push_back(off-2);   indices.push_back(off);         indices.push_back(off-1);
00400                     indices.push_back(off);   indices.push_back(off-1);         indices.push_back(off+1);
00401                 }
00402                 break;
00403             }
00404             default:
00405                 BOOST_ASSERT(0);
00406             }
00407             return true;
00408         }
00409     };
00410 
00411 public:
00412     ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
00413         daeErrorHandler::setErrorHandler(this);
00414         _resourcedir = ".";
00415     }
00416     virtual ~ColladaModelReader() {
00417         _vuserdata.clear();
00418         _collada.reset();
00419         DAE::cleanup();
00420     }
00421 
00422     bool InitFromFile(const std::string& filename) {
00423         ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE%filename));
00424         _collada.reset(new DAE);
00425         _dom = (domCOLLADA*)_collada->open(filename);
00426         if (!_dom) {
00427             return false;
00428         }
00429         _filename=filename;
00430 
00431         size_t maxchildren = _countChildren(_dom);
00432         _vuserdata.resize(0);
00433         _vuserdata.reserve(maxchildren);
00434 
00435         double dScale = 1.0;
00436         _processUserData(_dom, dScale);
00437         ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
00438         return _Extract();
00439     }
00440 
00441     bool InitFromData(const std::string& pdata) {
00442         ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE));
00443         _collada.reset(new DAE);
00444         _dom = (domCOLLADA*)_collada->openFromMemory(".",pdata.c_str());
00445         if (!_dom) {
00446             return false;
00447         }
00448 
00449         size_t maxchildren = _countChildren(_dom);
00450         _vuserdata.resize(0);
00451         _vuserdata.reserve(maxchildren);
00452 
00453         double dScale = 1.0;
00454         _processUserData(_dom, dScale);
00455         ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
00456         return _Extract();
00457     }
00458 
00459 protected:
00460 
00462     bool _Extract()
00463     {
00464         _model->clear();
00465         std::list< std::pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
00466         domCOLLADA::domSceneRef allscene = _dom->getScene();
00467         if( !allscene ) {
00468             return false;
00469         }
00470 
00471         //  parse each instance kinematics scene, prioritize real robots
00472         for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) {
00473             domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene];
00474             domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
00475             if (!kscene) {
00476                 continue;
00477             }
00478             boost::shared_ptr<KinematicsSceneBindings> bindings(new KinematicsSceneBindings());
00479             _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings);
00480             _ExtractPhysicsBindings(allscene,*bindings);
00481             for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) {
00482                 if( _ExtractArticulatedSystem(kscene->getInstance_articulated_system_array()[ias], *bindings) ) {
00483                     _PostProcess();
00484                     return true;
00485                 }
00486             }
00487             for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) {
00488                 listPossibleBodies.push_back(std::make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings));
00489             }
00490         }
00491 
00492         FOREACH(it, listPossibleBodies) {
00493             if( _ExtractKinematicsModel(it->first, *it->second) ) {
00494                 _PostProcess();
00495                 return true;
00496             }
00497         }
00498 
00499         return false;
00500     }
00501 
00502     void _PostProcess()
00503     {
00504         std::map<std::string, std::string> parent_link_tree;
00505         // building tree: name mapping
00506         try
00507         {
00508             _model->initTree(parent_link_tree);
00509         }
00510         catch(ParseError &e)
00511         {
00512             ROS_ERROR("Failed to build tree: %s", e.what());
00513         }
00514 
00515         // find the root link
00516         try
00517         {
00518             _model->initRoot(parent_link_tree);
00519         }
00520         catch(ParseError &e)
00521         {
00522             ROS_ERROR("Failed to find root link: %s", e.what());
00523         }
00524     }
00525 
00527     bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings)
00528     {
00529         if( !ias ) {
00530             return false;
00531         }
00532         ROS_DEBUG_STREAM(str(boost::format("instance articulated system sid %s\n")%ias->getSid()));
00533         domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system> (ias->getUrl().getElement().cast());
00534         if( !articulated_system ) {
00535             return false;
00536         }
00537 
00538         boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ias->getExtra_array());
00539         if( !pinterface_type ) {
00540             pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array());
00541         }
00542         if( !!pinterface_type ) {
00543             ROS_DEBUG_STREAM(str(boost::format("robot type: %s")%(*pinterface_type)));
00544         }
00545 
00546         // set the name
00547         if( _model->name_.size() == 0 && !!ias->getName() ) {
00548             _model->name_ = ias->getName();
00549         }
00550         if( _model->name_.size() == 0 && !!ias->getSid()) {
00551             _model->name_ = ias->getSid();
00552         }
00553         if( _model->name_.size() == 0 && !!articulated_system->getName() ) {
00554             _model->name_ = articulated_system->getName();
00555         }
00556         if( _model->name_.size() == 0 && !!articulated_system->getId()) {
00557             _model->name_ = articulated_system->getId();
00558         }
00559 
00560         if( !!articulated_system->getMotion() ) {
00561             domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system();
00562             if( !!articulated_system->getMotion()->getTechnique_common() ) {
00563                 for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
00564                     domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i];
00565                     // this should point to a kinematics axis_info
00566                     domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast<domKinematics_axis_info>(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt);
00567                     if( !!kinematics_axis_info ) {
00568                         // find the parent kinematics and go through all its instance kinematics models
00569                         daeElement* pparent = kinematics_axis_info->getParent();
00570                         while(!!pparent && pparent->typeID() != domKinematics::ID()) {
00571                             pparent = pparent->getParent();
00572                         }
00573                         BOOST_ASSERT(!!pparent);
00574                         bindings.AddAxisInfo(daeSafeCast<domKinematics>(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info);
00575                     }
00576                     else {
00577                         ROS_WARN_STREAM(str(boost::format("failed to find kinematics axis %s\n")%motion_axis_info->getAxis()));
00578                     }
00579                 }
00580             }
00581             if( !_ExtractArticulatedSystem(ias_new,bindings) ) {
00582                 return false;
00583             }
00584         }
00585         else {
00586             if( !articulated_system->getKinematics() ) {
00587                 ROS_WARN_STREAM(str(boost::format("collada <kinematics> tag empty? instance_articulated_system=%s\n")%ias->getID()));
00588                 return true;
00589             }
00590 
00591             if( !!articulated_system->getKinematics()->getTechnique_common() ) {
00592                 for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
00593                     bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL);
00594                 }
00595             }
00596 
00597             for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) {
00598                 _ExtractKinematicsModel(articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings);
00599             }
00600         }
00601 
00602         _ExtractRobotAttachedActuators(articulated_system);
00603         _ExtractRobotManipulators(articulated_system);
00604         _ExtractRobotAttachedSensors(articulated_system);
00605         return true;
00606     }
00607 
00608     bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings)
00609     {
00610         if( !ikm ) {
00611             return false;
00612         }
00613         ROS_DEBUG_STREAM(str(boost::format("instance kinematics model sid %s\n")%ikm->getSid()));
00614         domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model> (ikm->getUrl().getElement().cast());
00615         if (!kmodel) {
00616             ROS_WARN_STREAM(str(boost::format("%s does not reference valid kinematics\n")%ikm->getSid()));
00617             return false;
00618         }
00619         domPhysics_modelRef pmodel;
00620         boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ikm->getExtra_array());
00621         if( !pinterface_type ) {
00622             pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array());
00623         }
00624         if( !!pinterface_type ) {
00625             ROS_DEBUG_STREAM(str(boost::format("kinbody interface type: %s")%(*pinterface_type)));
00626         }
00627 
00628         // find matching visual node
00629         domNodeRef pvisualnode;
00630         FOREACH(it, bindings.listKinematicsVisualBindings) {
00631             if( it->second == ikm ) {
00632                 pvisualnode = it->first;
00633                 break;
00634             }
00635         }
00636         if( !pvisualnode ) {
00637             ROS_WARN_STREAM(str(boost::format("failed to find visual node for instance kinematics model %s\n")%ikm->getSid()));
00638             return false;
00639         }
00640 
00641         if( _model->name_.size() == 0 && !!ikm->getName() ) {
00642             _model->name_ = ikm->getName();
00643         }
00644         if( _model->name_.size() == 0 && !!ikm->getID() ) {
00645             _model->name_ = ikm->getID();
00646         }
00647 
00648         if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings)) {
00649             ROS_WARN_STREAM(str(boost::format("failed to load kinbody from kinematics model %s\n")%kmodel->getID()));
00650             return false;
00651         }
00652         return true;
00653     }
00654 
00656     bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings& bindings)
00657     {
00658         std::vector<domJointRef> vdomjoints;
00659         ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model->name_));
00660         if( !!pnode ) {
00661             ROS_DEBUG_STREAM(str(boost::format("node name: %s\n")%pnode->getId()));
00662         }
00663 
00664         //  Process joint of the kinbody
00665         domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
00666 
00667         //  Store joints
00668         for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
00669             vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
00670         }
00671 
00672         //  Store instances of joints
00673         for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) {
00674             domJointRef pelt = daeSafeCast<domJoint> (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement());
00675             if (!pelt) {
00676                 ROS_WARN_STREAM("failed to get joint from instance\n");
00677             }
00678             else {
00679                 vdomjoints.push_back(pelt);
00680             }
00681         }
00682 
00683         ROS_DEBUG_STREAM(str(boost::format("Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount()));
00684         for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) {
00685             domLinkRef pdomlink = ktec->getLink_array()[ilink];
00686             _RootOrigin  = _poseFromMatrix(_ExtractFullTransform(pdomlink));
00687             ROS_DEBUG("RootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
00688                       pdomlink->getName(),
00689                       _RootOrigin.position.x, _RootOrigin.position.y, _RootOrigin.position.z,
00690                       _RootOrigin.rotation.x, _RootOrigin.rotation.y, _RootOrigin.rotation.z, _RootOrigin.rotation.w);
00691 
00692             domNodeRef pvisualnode;
00693             FOREACH(it, bindings.listKinematicsVisualBindings) {
00694               if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) {
00695                 pvisualnode = it->first;
00696                 break;
00697               }
00698             }
00699             if (!!pvisualnode) {
00700               _VisualRootOrigin  = _poseFromMatrix(_getNodeParentTransform(pvisualnode));
00701               ROS_DEBUG("VisualRootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
00702                         pdomlink->getName(),
00703                         _VisualRootOrigin.position.x, _VisualRootOrigin.position.y, _VisualRootOrigin.position.z,
00704                         _VisualRootOrigin.rotation.x, _VisualRootOrigin.rotation.y, _VisualRootOrigin.rotation.z, _VisualRootOrigin.rotation.w);
00705             }
00706             _ExtractLink(pdomlink, ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings);
00707         }
00708 
00709         //  TODO: implement mathml
00710         for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) {
00711             domFormulaRef pf = ktec->getFormula_array()[iform];
00712             if (!pf->getTarget()) {
00713                 ROS_WARN_STREAM("formula target not valid\n");
00714                 continue;
00715             }
00716 
00717             // find the target joint
00718             boost::shared_ptr<Joint> pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
00719             if (!pjoint) {
00720                 continue;
00721             }
00722 
00723             if (!!pf->getTechnique_common()) {
00724                 daeElementRef peltmath;
00725                 daeTArray<daeElementRef> children;
00726                 pf->getTechnique_common()->getChildren(children);
00727                 for (size_t ichild = 0; ichild < children.getCount(); ++ichild) {
00728                     daeElementRef pelt = children[ichild];
00729                     if (_checkMathML(pelt,std::string("math")) ) {
00730                         peltmath = pelt;
00731                     }
00732                     else {
00733                         ROS_WARN_STREAM(str(boost::format("unsupported formula element: %s\n")%pelt->getElementName()));
00734                     }
00735                 }
00736                 if (!!peltmath) {
00737                     // full math xml spec not supported, only looking for ax+b pattern:
00738                     // <apply> <plus/> <apply> <times/> <ci>a</ci> x </apply> <ci>b</ci> </apply>
00739                     double a = 1, b = 0;
00740                     daeElementRef psymboljoint;
00741                     BOOST_ASSERT(peltmath->getChildren().getCount()>0);
00742                     daeElementRef papplyelt = peltmath->getChildren()[0];
00743                     BOOST_ASSERT(_checkMathML(papplyelt,"apply"));
00744                     BOOST_ASSERT(papplyelt->getChildren().getCount()>0);
00745                     if( _checkMathML(papplyelt->getChildren()[0],"plus") ) {
00746                         BOOST_ASSERT(papplyelt->getChildren().getCount()==3);
00747                         daeElementRef pa = papplyelt->getChildren()[1];
00748                         daeElementRef pb = papplyelt->getChildren()[2];
00749                         if( !_checkMathML(papplyelt->getChildren()[1],"apply") ) {
00750                             std::swap(pa,pb);
00751                         }
00752                         if( !_checkMathML(pa,"csymbol") ) {
00753                             BOOST_ASSERT(_checkMathML(pa,"apply"));
00754                             BOOST_ASSERT(_checkMathML(pa->getChildren()[0],"times"));
00755                             if( _checkMathML(pa->getChildren()[1],"csymbol") ) {
00756                                 psymboljoint = pa->getChildren()[1];
00757                                 BOOST_ASSERT(_checkMathML(pa->getChildren()[2],"cn"));
00758                                 std::stringstream ss(pa->getChildren()[2]->getCharData());
00759                                 ss >> a;
00760                             }
00761                             else {
00762                                 psymboljoint = pa->getChildren()[2];
00763                                 BOOST_ASSERT(_checkMathML(pa->getChildren()[1],"cn"));
00764                                 std::stringstream ss(pa->getChildren()[1]->getCharData());
00765                                 ss >> a;
00766                             }
00767                         }
00768                         else {
00769                             psymboljoint = pa;
00770                         }
00771                         BOOST_ASSERT(_checkMathML(pb,"cn"));
00772                         {
00773                             std::stringstream ss(pb->getCharData());
00774                             ss >> b;
00775                         }
00776                     }
00777                     else if( _checkMathML(papplyelt->getChildren()[0],"minus") ) {
00778                         BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[1],"csymbol"));
00779                         a = -1;
00780                         psymboljoint = papplyelt->getChildren()[1];
00781                     }
00782                     else {
00783                         BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[0],"csymbol"));
00784                         psymboljoint = papplyelt->getChildren()[0];
00785                     }
00786                     BOOST_ASSERT(psymboljoint->hasAttribute("encoding"));
00787                     BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA"));
00788                     boost::shared_ptr<Joint> pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
00789                     if( !!pbasejoint ) {
00790                         // set the mimic properties
00791                         pjoint->mimic.reset(new JointMimic());
00792                         pjoint->mimic->joint_name = pbasejoint->name;
00793                         pjoint->mimic->multiplier = a;
00794                         pjoint->mimic->offset = b;
00795                         ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b));
00796                     }
00797                 }
00798             }
00799         }
00800         return true;
00801     }
00802 
00804     boost::shared_ptr<Link> _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) {
00805         const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
00806         //  Set link name with the name of the COLLADA's Link
00807         std::string linkname = _ExtractLinkName(pdomlink);
00808         if( linkname.size() == 0 ) {
00809             ROS_WARN_STREAM("<link> has no name or id, falling back to <node>!\n");
00810             if( !!pdomnode ) {
00811                 if (!!pdomnode->getName()) {
00812                     linkname = pdomnode->getName();
00813                 }
00814                 if( linkname.size() == 0 && !!pdomnode->getID()) {
00815                     linkname = pdomnode->getID();
00816                 }
00817             }
00818         }
00819 
00820         boost::shared_ptr<Link> plink;
00821         _model->getLink(linkname,plink);
00822         if( !plink ) {
00823             plink.reset(new Link());
00824             plink->name = linkname;
00825             plink->visual.reset(new Visual());
00826             plink->visual->material_name = "";
00827             plink->visual->material.reset(new Material());
00828             plink->visual->material->name = "Red";
00829             plink->visual->material->color.r = 0.0;
00830             plink->visual->material->color.g = 1.0;
00831             plink->visual->material->color.b = 0.0;
00832             plink->visual->material->color.a = 1.0;
00833             plink->inertial.reset();
00834             _model->links_.insert(std::make_pair(linkname,plink));
00835         }
00836 
00837         _getUserData(pdomlink)->p = plink;
00838         if( !!pdomnode ) {
00839             ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName()));
00840         }
00841 
00842         // physics
00843         domInstance_rigid_bodyRef irigidbody;
00844         domRigid_bodyRef rigidbody;
00845         bool bFoundBinding = false;
00846         FOREACH(itlinkbinding, bindings.listLinkBindings) {
00847             if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) {
00848                 bFoundBinding = true;
00849                 irigidbody = itlinkbinding->irigidbody;
00850                 rigidbody = itlinkbinding->rigidbody;
00851             }
00852         }
00853 
00854         if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
00855             domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
00856             if( !!rigiddata->getMass() ) {
00857                 if ( !plink->inertial ) {
00858                     plink->inertial.reset(new Inertial());
00859                 }
00860                 plink->inertial->mass = rigiddata->getMass()->getValue();
00861             }
00862             if( !!rigiddata->getInertia() ) {
00863                 if ( !plink->inertial ) {
00864                     plink->inertial.reset(new Inertial());
00865                 }
00866                 plink->inertial->ixx = rigiddata->getInertia()->getValue()[0];
00867                 plink->inertial->iyy = rigiddata->getInertia()->getValue()[1];
00868                 plink->inertial->izz = rigiddata->getInertia()->getValue()[2];
00869             }
00870             if( !!rigiddata->getMass_frame() ) {
00871                 if ( !plink->inertial ) {
00872                     plink->inertial.reset(new Inertial());
00873                 }
00874                 //plink->inertial->origin = _poseMult(_poseInverse(tParentWorldLink), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
00875                 Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
00876                 plink->inertial->origin = _poseMult(_poseInverse(_poseMult(_poseInverse(_RootOrigin),
00877                                                                            _poseMult(tParentWorldLink, tlink))),
00878                                                     _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
00879             }
00880         }
00881 
00882         std::list<GEOMPROPERTIES> listGeomProperties;
00883         if (!pdomlink) {
00884             ROS_WARN_STREAM("Extract object NOT kinematics !!!\n");
00885             _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,Pose());
00886         }
00887         else {
00888             ROS_DEBUG_STREAM(str(boost::format("Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
00889             Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
00890             ROS_DEBUG("tlink: %s: %lf %lf %lf %lf %lf %lf %lf",
00891                       linkname.c_str(),
00892                       tlink.position.x,  tlink.position.y, tlink.position.z,
00893                       tlink.rotation.x, tlink.rotation.y, tlink.rotation.z, tlink.rotation.w);
00894             plink->visual->origin = _poseMult(tParentLink, tlink); // use the kinematics coordinate system for each link
00895             //            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);
00896             //            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);
00897 
00898             // Get the geometry
00899             _ExtractGeometry(pdomnode, listGeomProperties, listAxisBindings,
00900                              _poseMult(_poseMult(tParentWorldLink,tlink), plink->visual->origin));
00901 
00902             ROS_DEBUG_STREAM(str(boost::format("After ExtractGeometry Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
00903 
00904             //  Process all atached links
00905             for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) {
00906                 domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt];
00907 
00908                 // get link kinematics transformation
00909                 Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull));
00910 
00911                 // process attached links
00912                 daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt;
00913                 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
00914 
00915                 if (!pdomjoint) {
00916                     domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
00917                     if (!!pdomijoint) {
00918                         pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
00919                     }
00920                 }
00921 
00922                 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) {
00923                     ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint()));
00924                     return boost::shared_ptr<Link>();
00925                 }
00926 
00927                 // get direct child link
00928                 if (!pattfull->getLink()) {
00929                     ROS_WARN_STREAM(str(boost::format("joint %s needs to be attached to a valid link\n")%pdomjoint->getID()));
00930                     continue;
00931                 }
00932 
00933                 // find the correct joint in the bindings
00934                 daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
00935                 domNodeRef pchildnode;
00936 
00937                 // see if joint has a binding to a visual node
00938                 FOREACHC(itaxisbinding,listAxisBindings) {
00939                     for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
00940                         //  If the binding for the joint axis is found, retrieve the info
00941                         if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00942                             pchildnode = itaxisbinding->visualnode;
00943                             break;
00944                         }
00945                     }
00946                     if( !!pchildnode ) {
00947                         break;
00948                     }
00949                 }
00950                 if (!pchildnode) {
00951                     ROS_DEBUG_STREAM(str(boost::format("joint %s has no visual binding\n")%pdomjoint->getID()));
00952                 }
00953 
00954                 // create the joints before creating the child links
00955                 std::vector<boost::shared_ptr<Joint> > vjoints(vdomaxes.getCount());
00956                 for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
00957                     bool joint_active = true; // if not active, put into the passive list
00958                     FOREACHC(itaxisbinding,listAxisBindings) {
00959                         if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00960                             if( !!itaxisbinding->kinematics_axis_info ) {
00961                                 if( !!itaxisbinding->kinematics_axis_info->getActive() ) {
00962                                     joint_active = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info);
00963                                 }
00964                             }
00965                             break;
00966                         }
00967                     }
00968 
00969                     boost::shared_ptr<Joint> pjoint(new Joint());
00970                     pjoint->limits.reset(new JointLimits());
00971                     pjoint->limits->velocity = 0.0;
00972                     pjoint->limits->effort = 0.0;
00973                     pjoint->parent_link_name = plink->name;
00974 
00975                     if( !!pdomjoint->getName() ) {
00976                         pjoint->name = pdomjoint->getName();
00977                     }
00978                     else {
00979                         pjoint->name = str(boost::format("dummy%d")%_model->joints_.size());
00980                     }
00981 
00982                     if( !joint_active ) {
00983                         ROS_INFO_STREAM(str(boost::format("joint %s is passive, but adding to hierarchy\n")%pjoint->name));
00984                     }
00985 
00986                     domAxis_constraintRef pdomaxis = vdomaxes[ic];
00987                     if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) {
00988                         pjoint->type = Joint::REVOLUTE;
00989                     }
00990                     else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) {
00991                         pjoint->type = Joint::PRISMATIC;
00992                     }
00993                     else {
00994                         ROS_WARN_STREAM(str(boost::format("unsupported joint type: %s\n")%pdomaxis->getElementName()));
00995                     }
00996 
00997                     _getUserData(pdomjoint)->p = pjoint;
00998                     _getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size()));
00999                     _model->joints_[pjoint->name] = pjoint;
01000                     vjoints[ic] = pjoint;
01001                 }
01002 
01003                 boost::shared_ptr<Link> pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
01004 
01005                 if (!pchildlink) {
01006                     ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name));
01007                     continue;
01008                 }
01009 
01010                 int numjoints = 0;
01011                 for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
01012                     domKinematics_axis_infoRef kinematics_axis_info;
01013                     domMotion_axis_infoRef motion_axis_info;
01014                     FOREACHC(itaxisbinding,listAxisBindings) {
01015                         bool bfound = false;
01016                         if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
01017                             kinematics_axis_info = itaxisbinding->kinematics_axis_info;
01018                             motion_axis_info = itaxisbinding->motion_axis_info;
01019                             bfound = true;
01020                             break;
01021                         }
01022                     }
01023                     domAxis_constraintRef pdomaxis = vdomaxes[ic];
01024                     if (!pchildlink) {
01025                         // create dummy child link
01026                         // multiple axes can be easily done with "empty links"
01027                         ROS_WARN_STREAM(str(boost::format("creating dummy link %s, num joints %d\n")%plink->name%numjoints));
01028 
01029                         std::stringstream ss;
01030                         ss << plink->name;
01031                         ss <<"_dummy" << numjoints;
01032                         pchildlink.reset(new Link());
01033                         pchildlink->name = ss.str();
01034                         _model->links_.insert(std::make_pair(pchildlink->name,pchildlink));
01035                     }
01036 
01037                     ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic));
01038                     boost::shared_ptr<Joint> pjoint = vjoints[ic];
01039                     pjoint->child_link_name = pchildlink->name;
01040 
01041 #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \
01042                                            apose.position.x, apose.position.y, apose.position.z, \
01043                                            apose.rotation.x, apose.rotation.y, apose.rotation.z, apose.rotation.w);
01044                     {
01045                       PRINT_POSE("tatt", tatt);
01046                       PRINT_POSE("trans_joint_to_child", tatt);
01047                       Pose trans_joint_to_child = _poseFromMatrix(_ExtractFullTransform(pattfull->getLink()));
01048 
01049                       pjoint->parent_to_joint_origin_transform = _poseMult(tatt, trans_joint_to_child);
01050                       //  Axes and Anchor assignment.
01051                       Vector3 ax(pdomaxis->getAxis()->getValue()[0],
01052                                  pdomaxis->getAxis()->getValue()[1],
01053                                  pdomaxis->getAxis()->getValue()[2]);
01054                       Pose pinv = _poseInverse(trans_joint_to_child);
01055                       // rotate axis
01056                       ax = pinv.rotation * ax;
01057 
01058                       pjoint->axis.x = ax.x;
01059                       pjoint->axis.y = ax.y;
01060                       pjoint->axis.z = ax.z;
01061 
01062                       ROS_DEBUG("joint %s axis: %f %f %f -> %f %f %f\n", pjoint->name.c_str(),
01063                                 pdomaxis->getAxis()->getValue()[0],
01064                                 pdomaxis->getAxis()->getValue()[1],
01065                                 pdomaxis->getAxis()->getValue()[2],
01066                                 pjoint->axis.x, pjoint->axis.y, pjoint->axis.z);
01067                     }
01068 
01069                     if (!motion_axis_info) {
01070                         ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name));
01071                     }
01072 
01073                     //  Sets the Speed and the Acceleration of the joint
01074                     if (!!motion_axis_info) {
01075                         if (!!motion_axis_info->getSpeed()) {
01076                             pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
01077                             ROS_DEBUG("... Joint Speed: %f...\n",pjoint->limits->velocity);
01078                         }
01079                         if (!!motion_axis_info->getAcceleration()) {
01080                             pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info);
01081                             ROS_DEBUG("... Joint effort: %f...\n",pjoint->limits->effort);
01082                         }
01083                     }
01084 
01085                     bool joint_locked = false; // if locked, joint angle is static
01086                     bool kinematics_limits = false;
01087 
01088                     if (!!kinematics_axis_info) {
01089                         if (!!kinematics_axis_info->getLocked()) {
01090                             joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
01091                         }
01092 
01093                         if (joint_locked) { // If joint is locked set limits to the static value.
01094                             if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
01095                                 ROS_WARN_STREAM("lock joint!!\n");
01096                                 pjoint->limits->lower = 0;
01097                                 pjoint->limits->upper = 0;
01098                             }
01099                         }
01100                         else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits
01101                             kinematics_limits   = true;
01102                             double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(kinematics_axis_info);
01103                             if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
01104                                 pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
01105                                 pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
01106                                 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
01107                                     pjoint->type = Joint::FIXED;
01108                                 }
01109                             }
01110                         }
01111                     }
01112 
01113                     //  Search limits in the joints section
01114                     if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
01115                         //  If there are NO LIMITS
01116                         if( !pdomaxis->getLimits() ) {
01117                             ROS_DEBUG_STREAM(str(boost::format("There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits));
01118                             if( pjoint->type == Joint::REVOLUTE ) {
01119                                 pjoint->type = Joint::CONTINUOUS; // continuous means revolute?
01120                                 pjoint->limits->lower = -M_PI;
01121                                 pjoint->limits->upper = M_PI;
01122                             }
01123                             else {
01124                                 pjoint->limits->lower = -100000;
01125                                 pjoint->limits->upper = 100000;
01126                             }
01127                         }
01128                         else {
01129                             ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name));
01130                             double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(pdomaxis);
01131                             pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale;
01132                             pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale;
01133                             if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
01134                                 pjoint->type = Joint::FIXED;
01135                             }
01136                         }
01137                     }
01138 
01139                     if (pjoint->limits->velocity == 0.0) {
01140                       pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
01141                     }
01142                     pchildlink.reset();
01143                     ++numjoints;
01144                 }
01145             }
01146         }
01147 
01148         if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
01149             ROS_WARN("urdf collada reader does not support attachment_start\n");
01150         }
01151         if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
01152             ROS_WARN("urdf collada reader does not support attachment_end\n");
01153         }
01154 
01155         plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
01156         // visual_groups deprecated
01157         //boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss;
01158         //viss.reset(new std::vector<boost::shared_ptr<Visual > >);
01159         //viss->push_back(plink->visual);
01160         //plink->visual_groups.insert(std::make_pair("default", viss));
01161 
01162         if( !plink->visual->geometry ) {
01163           plink->visual.reset();
01164           plink->collision.reset();
01165         } else {
01166           // collision
01167           plink->collision.reset(new Collision());
01168           plink->collision->geometry = plink->visual->geometry;
01169           plink->collision->origin   = plink->visual->origin;
01170         }
01171 
01172         // collision_groups deprecated
01173         //boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols;
01174         //cols.reset(new std::vector<boost::shared_ptr<Collision > >);
01175         //cols->push_back(plink->collision);
01176         //plink->collision_groups.insert(std::make_pair("default", cols));
01177 
01178         return plink;
01179     }
01180 
01181     boost::shared_ptr<Geometry> _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
01182     {
01183         std::vector<std::vector<Vector3> > vertices;
01184         std::vector<std::vector<int> > indices;
01185         std::vector<Color> ambients;
01186         std::vector<Color> diffuses;
01187         unsigned int index, vert_counter;
01188         vertices.resize(listGeomProperties.size());
01189         indices.resize(listGeomProperties.size());
01190         ambients.resize(listGeomProperties.size());
01191         diffuses.resize(listGeomProperties.size());
01192         index = 0;
01193         vert_counter = 0;
01194         FOREACHC(it, listGeomProperties) {
01195             vertices[index].resize(it->vertices.size());
01196             for(size_t i = 0; i < it->vertices.size(); ++i) {
01197                 vertices[index][i] = _poseMult(it->_t, it->vertices[i]);
01198                 vert_counter++;
01199             }
01200             indices[index].resize(it->indices.size());
01201             for(size_t i = 0; i < it->indices.size(); ++i) {
01202                 indices[index][i] = it->indices[i];
01203             }
01204             ambients[index] = it->ambientColor;
01205             diffuses[index] = it->diffuseColor;
01206 // workaround for mesh_loader.cpp:421
01207 // Most of are DAE files don't have ambient color defined
01208             ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
01209             if ( ambients[index].r == 0.0 ) {
01210                 ambients[index].r = 0.0001;
01211             }
01212             if ( ambients[index].g == 0.0 ) {
01213                 ambients[index].g = 0.0001;
01214             }
01215             if ( ambients[index].b == 0.0 ) {
01216                 ambients[index].b = 0.0001;
01217             }
01218             index++;
01219         }
01220 
01221         if (vert_counter == 0) {
01222           boost::shared_ptr<Mesh> ret;
01223           ret.reset();
01224           return ret;
01225         }
01226 
01227         boost::shared_ptr<Mesh> geometry(new Mesh());
01228         geometry->type = Geometry::MESH;
01229         geometry->scale.x = 1;
01230         geometry->scale.y = 1;
01231         geometry->scale.z = 1;
01232 
01233         // have to save the geometry into individual collada 1.4 files since URDF does not allow triangle meshes to be specified
01234         std::stringstream daedata;
01235         daedata << str(boost::format("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\
01236 <COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n\
01237   <asset>\n\
01238     <contributor>\n\
01239       <author>Rosen Diankov</author>\n\
01240       <comments>\n\
01241         robot_model/urdf temporary collada geometry\n\
01242       </comments>\n\
01243     </contributor>\n\
01244     <unit name=\"meter\" meter=\"1.0\"/>\n\
01245     <up_axis>Y_UP</up_axis>\n\
01246   </asset>\n\
01247   <library_materials>\n"));
01248         for(unsigned int i=0; i < index; i++) {
01249             daedata << str(boost::format("\
01250     <material id=\"blinn2%d\" name=\"blinn2%d\">\n\
01251       <instance_effect url=\"#blinn2-fx%d\"/>\n\
01252     </material>\n")%i%i%i);
01253         }
01254         daedata << "\
01255   </library_materials>\n\
01256   <library_effects>\n";
01257         for(unsigned int i=0; i < index; i++) {
01258             daedata << str(boost::format("\
01259     <effect id=\"blinn2-fx%d\">\n\
01260           <profile_COMMON>\n\
01261                 <technique sid=\"common\">\n\
01262                   <phong>\n\
01263                         <ambient>\n\
01264                           <color>%f %f %f %f</color>\n\
01265                         </ambient>\n\
01266                         <diffuse>\n\
01267                           <color>%f %f %f %f</color>\n\
01268                         </diffuse>\n\
01269                         <specular>\n\
01270                           <color>0 0 0 1</color>\n\
01271                         </specular>\n\
01272                   </phong>\n\
01273                 </technique>\n\
01274       </profile_COMMON>\n\
01275     </effect>\n")%i%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a);
01276         }
01277         daedata << str(boost::format("\
01278   </library_effects>\n\
01279   <library_geometries>\n"));
01280         // fill with vertices
01281         for(unsigned int i=0; i < index; i++) {
01282             daedata << str(boost::format("\
01283     <geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\
01284           <mesh>\n\
01285                 <source id=\"geo0.positions\">\n\
01286                   <float_array id=\"geo0.positions-array\" count=\"%d\">")%i%i%(vertices[i].size()*3));
01287             FOREACH(it,vertices[i]) {
01288                 daedata << it->x << " " << it->y << " " << it->z << " ";
01289             }
01290             daedata << str(boost::format("\n\
01291           </float_array>\n\
01292                   <technique_common>\n\
01293                         <accessor count=\"%d\" source=\"#geo0.positions-array\" stride=\"3\">\n\
01294                           <param name=\"X\" type=\"float\"/>\n\
01295                           <param name=\"Y\" type=\"float\"/>\n\
01296                           <param name=\"Z\" type=\"float\"/>\n\
01297                         </accessor>\n\
01298                   </technique_common>\n\
01299                 </source>\n\
01300                 <vertices id=\"geo0.vertices\">\n\
01301                   <input semantic=\"POSITION\" source=\"#geo0.positions\"/>\n\
01302                 </vertices>\n\
01303                 <triangles count=\"%d\" material=\"lambert2SG\">\n\
01304                   <input offset=\"0\" semantic=\"VERTEX\" source=\"#geo0.vertices\"/>\n\
01305           <p>")%vertices[i].size()%(indices[i].size()/3));
01306             // fill with indices
01307             FOREACH(it,indices[i]) {
01308                 daedata << *it << " ";
01309             }
01310             daedata << str(boost::format("</p>\n\
01311                 </triangles>\n\
01312           </mesh>\n\
01313     </geometry>\n"));
01314         }
01315         daedata << str(boost::format("\
01316   </library_geometries>\n\
01317   <library_visual_scenes>\n\
01318     <visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\
01319       <node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name);
01320         for(unsigned int i=0; i < index; i++) {
01321             daedata << str(boost::format("\
01322         <instance_geometry url=\"#base2_M1KShape%i\">\n\
01323           <bind_material>\n\
01324             <technique_common>\n\
01325               <instance_material symbol=\"lambert2SG\" target=\"#blinn2%i\"/>\n\
01326             </technique_common>\n\
01327           </bind_material>\n\
01328         </instance_geometry>\n")%i%i);
01329         }
01330         daedata << str(boost::format("\
01331       </node>\n\
01332     </visual_scene>\n\
01333   </library_visual_scenes>\n\
01334   <scene>\n\
01335     <instance_visual_scene url=\"#VisualSceneNode\"/>\n\
01336   </scene>\n\
01337 </COLLADA>"));
01338 
01339 #ifdef HAVE_MKSTEMPS
01340         geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name);
01341         int fd = mkstemps(&geometry->filename[0],4);
01342 #else
01343         int fd = -1;
01344         for(int iter = 0; iter < 1000; ++iter) {
01345             geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_%d.dae")%name%rand());
01346             if( !!std::ifstream(geometry->filename.c_str())) {
01347                 fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL);
01348                 if( fd != -1 ) {
01349                     break;
01350                 }
01351             }
01352         }
01353         if( fd == -1 ) {
01354             ROS_ERROR("failed to open geometry dae file %s",geometry->filename.c_str());
01355             return geometry;
01356         }
01357 #endif
01358         //ROS_INFO("temp file: %s",geometry->filename.c_str());
01359         std::string daedatastr = daedata.str();
01360         if( (size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) {
01361             ROS_ERROR("failed to write to geometry dae file %s",geometry->filename.c_str());
01362         }
01363         close(fd);
01364         _listTempFilenames.push_back(boost::shared_ptr<UnlinkFilename>(new UnlinkFilename(geometry->filename)));
01365         geometry->filename = std::string("file://") + geometry->filename;
01366         return geometry;
01367     }
01368 
01372     void _ExtractGeometry(const domNodeRef pdomnode,std::list<GEOMPROPERTIES>& listGeomProperties, const std::list<JointAxisBinding>& listAxisBindings, const Pose& tlink)
01373     {
01374         if( !pdomnode ) {
01375             return;
01376         }
01377 
01378         ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName()));
01379 
01380         // For all child nodes of pdomnode
01381         for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
01382             // check if contains a joint
01383             bool contains=false;
01384             FOREACHC(it,listAxisBindings) {
01385                 // don't check ID's check if the reference is the same!
01386                 if ( (pdomnode->getNode_array()[i])  == (it->visualnode)) {
01387                     contains=true;
01388                     break;
01389                 }
01390             }
01391             if (contains) {
01392                 continue;
01393             }
01394 
01395             _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink);
01396             // Plink stayes the same for all children
01397             // replace pdomnode by child = pdomnode->getNode_array()[i]
01398             // hope for the best!
01399             // put everything in a subroutine in order to process pdomnode too!
01400         }
01401 
01402         unsigned int nGeomBefore =  listGeomProperties.size(); // #of Meshes already associated to this link
01403 
01404         // get the geometry
01405         for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
01406             domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
01407             domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
01408             if (!domgeom) {
01409                 continue;
01410             }
01411 
01412             //  Gets materials
01413             std::map<std::string, domMaterialRef> mapmaterials;
01414             if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
01415                 const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
01416                 for (size_t imat = 0; imat < matarray.getCount(); ++imat) {
01417                     domMaterialRef pmat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
01418                     if (!!pmat) {
01419                         mapmaterials[matarray[imat]->getSymbol()] = pmat;
01420                     }
01421                 }
01422             }
01423 
01424             //  Gets the geometry
01425             _ExtractGeometry(domgeom, mapmaterials, listGeomProperties);
01426         }
01427 
01428         std::list<GEOMPROPERTIES>::iterator itgeom= listGeomProperties.begin();
01429         for (unsigned int i=0; i< nGeomBefore; i++) {
01430             itgeom++; // change only the transformations of the newly found geometries.
01431         }
01432 
01433         boost::array<double,12> tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)),
01434                                                        _poseMult(_poseMult(_matrixFromPose(_poseInverse(_VisualRootOrigin)),
01435                                                                            _getNodeParentTransform(pdomnode)),
01436                                                                  _ExtractFullTransform(pdomnode)));
01437         Pose tnodegeom;
01438         Vector3 vscale(1,1,1);
01439         _decompose(tmnodegeom, tnodegeom, vscale);
01440 
01441         ROS_DEBUG_STREAM("tnodegeom: " << pdomnode->getName()
01442                          << tnodegeom.position.x << " " << tnodegeom.position.y << " " << tnodegeom.position.z << " / "
01443                          << tnodegeom.rotation.x << " " << tnodegeom.rotation.y << " " << tnodegeom.rotation.z << " "
01444                          << tnodegeom.rotation.w);
01445 
01446         //        std::stringstream ss; ss << "geom: ";
01447         //        for(int i = 0; i < 4; ++i) {
01448         //            ss << tmnodegeom[4*0+i] << " " << tmnodegeom[4*1+i] << " " << tmnodegeom[4*2+i] << " ";
01449         //        }
01450         //        ROS_INFO(ss.str().c_str());
01451 
01452         //  Switch between different type of geometry PRIMITIVES
01453         for (; itgeom != listGeomProperties.end(); itgeom++) {
01454             itgeom->_t = tnodegeom;
01455             switch (itgeom->type) {
01456             case GeomBox:
01457                 itgeom->vGeomData.x *= vscale.x;
01458                 itgeom->vGeomData.y *= vscale.y;
01459                 itgeom->vGeomData.z *= vscale.z;
01460                 break;
01461             case GeomSphere: {
01462                 itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y));
01463                 break;
01464             }
01465             case GeomCylinder:
01466                 itgeom->vGeomData.x *= std::max(vscale.x, vscale.y);
01467                 itgeom->vGeomData.y *= vscale.z;
01468                 break;
01469             case GeomTrimesh:
01470                 for(size_t i = 0; i < itgeom->vertices.size(); ++i ) {
01471                     itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]);
01472                 }
01473                 itgeom->_t = Pose(); // reset back to identity
01474                 break;
01475             default:
01476                 ROS_WARN_STREAM(str(boost::format("unknown geometry type: %d\n")%itgeom->type));
01477             }
01478         }
01479     }
01480 
01484     void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES& geom)
01485     {
01486         if( !!pmat && !!pmat->getInstance_effect() ) {
01487             domEffectRef peffect = daeSafeCast<domEffect>(pmat->getInstance_effect()->getUrl().getElement().cast());
01488             if( !!peffect ) {
01489                 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
01490                 if( !!pphong ) {
01491                     if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
01492                         domFx_color c = pphong->getAmbient()->getColor()->getValue();
01493                         geom.ambientColor.r = c[0];
01494                         geom.ambientColor.g = c[1];
01495                         geom.ambientColor.b = c[2];
01496                         geom.ambientColor.a = c[3];
01497                     }
01498                     if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
01499                         domFx_color c = pphong->getDiffuse()->getColor()->getValue();
01500                         geom.diffuseColor.r = c[0];
01501                         geom.diffuseColor.g = c[1];
01502                         geom.diffuseColor.b = c[2];
01503                         geom.diffuseColor.a = c[3];
01504                     }
01505                 }
01506             }
01507         }
01508     }
01509 
01515     bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01516     {
01517         if( !triRef ) {
01518             return false;
01519         }
01520         listGeomProperties.push_back(GEOMPROPERTIES());
01521         GEOMPROPERTIES& geom = listGeomProperties.back();
01522         geom.type = GeomTrimesh;
01523 
01524         // resolve the material and assign correct colors to the geometry
01525         if( !!triRef->getMaterial() ) {
01526             std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01527             if( itmat != mapmaterials.end() ) {
01528                 _FillGeometryColor(itmat->second,geom);
01529             }
01530         }
01531 
01532         size_t triangleIndexStride = 0, vertexoffset = -1;
01533         domInput_local_offsetRef indexOffsetRef;
01534 
01535         for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01536             size_t offset = triRef->getInput_array()[w]->getOffset();
01537             daeString str = triRef->getInput_array()[w]->getSemantic();
01538             if (!strcmp(str,"VERTEX")) {
01539                 indexOffsetRef = triRef->getInput_array()[w];
01540                 vertexoffset = offset;
01541             }
01542             if (offset> triangleIndexStride) {
01543                 triangleIndexStride = offset;
01544             }
01545         }
01546         triangleIndexStride++;
01547 
01548         const domList_of_uints& indexArray =triRef->getP()->getValue();
01549         geom.indices.reserve(triRef->getCount()*3);
01550         geom.vertices.reserve(triRef->getCount()*3);
01551         for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01552             domInput_localRef localRef = vertsRef->getInput_array()[i];
01553             daeString str = localRef->getSemantic();
01554             if ( strcmp(str,"POSITION") == 0 ) {
01555                 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01556                 if( !node ) {
01557                     continue;
01558                 }
01559                 double fUnitScale = _GetUnitScale(node);
01560                 const domFloat_arrayRef flArray = node->getFloat_array();
01561                 if (!!flArray) {
01562                     const domList_of_floats& listFloats = flArray->getValue();
01563                     int k=vertexoffset;
01564                     int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
01565                     for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01566                         if(k+2*triangleIndexStride < indexArray.getCount() ) {
01567                             for (int j=0; j<3; j++) {
01568                                 int index0 = indexArray.get(k)*vertexStride;
01569                                 domFloat fl0 = listFloats.get(index0);
01570                                 domFloat fl1 = listFloats.get(index0+1);
01571                                 domFloat fl2 = listFloats.get(index0+2);
01572                                 k+=triangleIndexStride;
01573                                 geom.indices.push_back(geom.vertices.size());
01574                                 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01575                             }
01576                         }
01577                     }
01578                 }
01579                 else {
01580                     ROS_WARN_STREAM("float array not defined!\n");
01581                 }
01582                 break;
01583             }
01584         }
01585         if( geom.indices.size() != 3*triRef->getCount() ) {
01586             ROS_WARN_STREAM("triangles declares wrong count!\n");
01587         }
01588         geom.InitCollisionMesh();
01589         return true;
01590     }
01591 
01596     bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01597     {
01598         if( !triRef ) {
01599             return false;
01600         }
01601         listGeomProperties.push_back(GEOMPROPERTIES());
01602         GEOMPROPERTIES& geom = listGeomProperties.back();
01603         geom.type = GeomTrimesh;
01604 
01605         // resolve the material and assign correct colors to the geometry
01606         if( !!triRef->getMaterial() ) {
01607             std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01608             if( itmat != mapmaterials.end() ) {
01609                 _FillGeometryColor(itmat->second,geom);
01610             }
01611         }
01612 
01613         size_t triangleIndexStride = 0, vertexoffset = -1;
01614         domInput_local_offsetRef indexOffsetRef;
01615 
01616         for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01617             size_t offset = triRef->getInput_array()[w]->getOffset();
01618             daeString str = triRef->getInput_array()[w]->getSemantic();
01619             if (!strcmp(str,"VERTEX")) {
01620                 indexOffsetRef = triRef->getInput_array()[w];
01621                 vertexoffset = offset;
01622             }
01623             if (offset> triangleIndexStride) {
01624                 triangleIndexStride = offset;
01625             }
01626         }
01627         triangleIndexStride++;
01628         size_t primitivecount = triRef->getCount();
01629         if( primitivecount > triRef->getP_array().getCount() ) {
01630             ROS_WARN_STREAM("trifans has incorrect count\n");
01631             primitivecount = triRef->getP_array().getCount();
01632         }
01633         for(size_t ip = 0; ip < primitivecount; ++ip) {
01634             domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
01635             for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01636                 domInput_localRef localRef = vertsRef->getInput_array()[i];
01637                 daeString str = localRef->getSemantic();
01638                 if ( strcmp(str,"POSITION") == 0 ) {
01639                     const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01640                     if( !node ) {
01641                         continue;
01642                     }
01643                     double fUnitScale = _GetUnitScale(node);
01644                     const domFloat_arrayRef flArray = node->getFloat_array();
01645                     if (!!flArray) {
01646                         const domList_of_floats& listFloats = flArray->getValue();
01647                         int k=vertexoffset;
01648                         int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
01649                         size_t usedindices = 3*(indexArray.getCount()-2);
01650                         if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
01651                             geom.indices.reserve(geom.indices.size()+usedindices);
01652                         }
01653                         if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
01654                             geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
01655                         }
01656                         size_t startoffset = (int)geom.vertices.size();
01657                         while(k < (int)indexArray.getCount() ) {
01658                             int index0 = indexArray.get(k)*vertexStride;
01659                             domFloat fl0 = listFloats.get(index0);
01660                             domFloat fl1 = listFloats.get(index0+1);
01661                             domFloat fl2 = listFloats.get(index0+2);
01662                             k+=triangleIndexStride;
01663                             geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01664                         }
01665                         for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01666                             geom.indices.push_back(startoffset);
01667                             geom.indices.push_back(ivert-1);
01668                             geom.indices.push_back(ivert);
01669                         }
01670                     }
01671                     else {
01672                         ROS_WARN_STREAM("float array not defined!\n");
01673                     }
01674                     break;
01675                 }
01676             }
01677         }
01678 
01679         geom.InitCollisionMesh();
01680         return false;
01681     }
01682 
01687     bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01688     {
01689         if( !triRef ) {
01690             return false;
01691         }
01692         listGeomProperties.push_back(GEOMPROPERTIES());
01693         GEOMPROPERTIES& geom = listGeomProperties.back();
01694         geom.type = GeomTrimesh;
01695 
01696         // resolve the material and assign correct colors to the geometry
01697         if( !!triRef->getMaterial() ) {
01698             std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01699             if( itmat != mapmaterials.end() ) {
01700                 _FillGeometryColor(itmat->second,geom);
01701             }
01702         }
01703         size_t triangleIndexStride = 0, vertexoffset = -1;
01704         domInput_local_offsetRef indexOffsetRef;
01705 
01706         for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01707             size_t offset = triRef->getInput_array()[w]->getOffset();
01708             daeString str = triRef->getInput_array()[w]->getSemantic();
01709             if (!strcmp(str,"VERTEX")) {
01710                 indexOffsetRef = triRef->getInput_array()[w];
01711                 vertexoffset = offset;
01712             }
01713             if (offset> triangleIndexStride) {
01714                 triangleIndexStride = offset;
01715             }
01716         }
01717         triangleIndexStride++;
01718         size_t primitivecount = triRef->getCount();
01719         if( primitivecount > triRef->getP_array().getCount() ) {
01720             ROS_WARN_STREAM("tristrips has incorrect count\n");
01721             primitivecount = triRef->getP_array().getCount();
01722         }
01723         for(size_t ip = 0; ip < primitivecount; ++ip) {
01724             domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
01725             for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01726                 domInput_localRef localRef = vertsRef->getInput_array()[i];
01727                 daeString str = localRef->getSemantic();
01728                 if ( strcmp(str,"POSITION") == 0 ) {
01729                     const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01730                     if( !node ) {
01731                         continue;
01732                     }
01733                     double fUnitScale = _GetUnitScale(node);
01734                     const domFloat_arrayRef flArray = node->getFloat_array();
01735                     if (!!flArray) {
01736                         const domList_of_floats& listFloats = flArray->getValue();
01737                         int k=vertexoffset;
01738                         int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
01739                         size_t usedindices = indexArray.getCount()-2;
01740                         if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
01741                             geom.indices.reserve(geom.indices.size()+usedindices);
01742                         }
01743                         if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
01744                             geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
01745                         }
01746 
01747                         size_t startoffset = (int)geom.vertices.size();
01748                         while(k < (int)indexArray.getCount() ) {
01749                             int index0 = indexArray.get(k)*vertexStride;
01750                             domFloat fl0 = listFloats.get(index0);
01751                             domFloat fl1 = listFloats.get(index0+1);
01752                             domFloat fl2 = listFloats.get(index0+2);
01753                             k+=triangleIndexStride;
01754                             geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01755                         }
01756 
01757                         bool bFlip = false;
01758                         for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01759                             geom.indices.push_back(ivert-2);
01760                             geom.indices.push_back(bFlip ? ivert : ivert-1);
01761                             geom.indices.push_back(bFlip ? ivert-1 : ivert);
01762                             bFlip = !bFlip;
01763                         }
01764                     }
01765                     else {
01766                         ROS_WARN_STREAM("float array not defined!\n");
01767                     }
01768                     break;
01769                 }
01770             }
01771         }
01772 
01773         geom.InitCollisionMesh();
01774         return false;
01775     }
01776 
01781     bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01782     {
01783         if( !triRef ) {
01784             return false;
01785         }
01786         listGeomProperties.push_back(GEOMPROPERTIES());
01787         GEOMPROPERTIES& geom = listGeomProperties.back();
01788         geom.type = GeomTrimesh;
01789 
01790         // resolve the material and assign correct colors to the geometry
01791         if( !!triRef->getMaterial() ) {
01792             std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01793             if( itmat != mapmaterials.end() ) {
01794                 _FillGeometryColor(itmat->second,geom);
01795             }
01796         }
01797 
01798         size_t triangleIndexStride = 0,vertexoffset = -1;
01799         domInput_local_offsetRef indexOffsetRef;
01800         for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01801             size_t offset = triRef->getInput_array()[w]->getOffset();
01802             daeString str = triRef->getInput_array()[w]->getSemantic();
01803             if (!strcmp(str,"VERTEX")) {
01804                 indexOffsetRef = triRef->getInput_array()[w];
01805                 vertexoffset = offset;
01806             }
01807             if (offset> triangleIndexStride) {
01808                 triangleIndexStride = offset;
01809             }
01810         }
01811         triangleIndexStride++;
01812         const domList_of_uints& indexArray =triRef->getP()->getValue();
01813         for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01814             domInput_localRef localRef = vertsRef->getInput_array()[i];
01815             daeString str = localRef->getSemantic();
01816             if ( strcmp(str,"POSITION") == 0 ) {
01817                 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01818                 if( !node ) {
01819                     continue;
01820                 }
01821                 double fUnitScale = _GetUnitScale(node);
01822                 const domFloat_arrayRef flArray = node->getFloat_array();
01823                 if (!!flArray) {
01824                     const domList_of_floats& listFloats = flArray->getValue();
01825                     size_t k=vertexoffset;
01826                     int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
01827                     for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
01828                         size_t numverts = triRef->getVcount()->getValue()[ipoly];
01829                         if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
01830                             size_t startoffset = geom.vertices.size();
01831                             for (size_t j=0; j<numverts; j++) {
01832                                 int index0 = indexArray.get(k)*vertexStride;
01833                                 domFloat fl0 = listFloats.get(index0);
01834                                 domFloat fl1 = listFloats.get(index0+1);
01835                                 domFloat fl2 = listFloats.get(index0+2);
01836                                 k+=triangleIndexStride;
01837                                 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01838                             }
01839                             for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01840                                 geom.indices.push_back(startoffset);
01841                                 geom.indices.push_back(ivert-1);
01842                                 geom.indices.push_back(ivert);
01843                             }
01844                         }
01845                     }
01846                 }
01847                 else {
01848                     ROS_WARN_STREAM("float array not defined!\n");
01849                 }
01850                 break;
01851             }
01852         }
01853         geom.InitCollisionMesh();
01854         return false;
01855     }
01856 
01860     bool _ExtractGeometry(const domGeometryRef geom, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01861     {
01862         if( !geom ) {
01863             return false;
01864         }
01865         std::vector<Vector3> vconvexhull;
01866         if (geom->getMesh()) {
01867             const domMeshRef meshRef = geom->getMesh();
01868             for (size_t tg = 0; tg<meshRef->getTriangles_array().getCount(); tg++) {
01869                 _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01870             }
01871             for (size_t tg = 0; tg<meshRef->getTrifans_array().getCount(); tg++) {
01872                 _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01873             }
01874             for (size_t tg = 0; tg<meshRef->getTristrips_array().getCount(); tg++) {
01875                 _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01876             }
01877             for (size_t tg = 0; tg<meshRef->getPolylist_array().getCount(); tg++) {
01878                 _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01879             }
01880             if( meshRef->getPolygons_array().getCount()> 0 ) {
01881                 ROS_WARN_STREAM("openrave does not support collada polygons\n");
01882             }
01883 
01884             //            if( alltrimesh.vertices.size() == 0 ) {
01885             //                const domVerticesRef vertsRef = meshRef->getVertices();
01886             //                for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
01887             //                    domInput_localRef localRef = vertsRef->getInput_array()[i];
01888             //                    daeString str = localRef->getSemantic();
01889             //                    if ( strcmp(str,"POSITION") == 0 ) {
01890             //                        const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01891             //                        if( !node )
01892             //                            continue;
01893             //                        double fUnitScale = _GetUnitScale(node);
01894             //                        const domFloat_arrayRef flArray = node->getFloat_array();
01895             //                        if (!!flArray) {
01896             //                            const domList_of_floats& listFloats = flArray->getValue();
01897             //                            int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01898             //                            vconvexhull.reserve(vconvexhull.size()+listFloats.getCount());
01899             //                            for (size_t vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride) {
01900             //                                //btVector3 verts[3];
01901             //                                domFloat fl0 = listFloats.get(vertIndex);
01902             //                                domFloat fl1 = listFloats.get(vertIndex+1);
01903             //                                domFloat fl2 = listFloats.get(vertIndex+2);
01904             //                                vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01905             //                            }
01906             //                        }
01907             //                    }
01908             //                }
01909             //
01910             //                _computeConvexHull(vconvexhull,alltrimesh);
01911             //            }
01912 
01913             return true;
01914         }
01915         else if (geom->getConvex_mesh()) {
01916             {
01917                 const domConvex_meshRef convexRef = geom->getConvex_mesh();
01918                 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01919                 if ( !!otherElemRef ) {
01920                     domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
01921                     ROS_WARN_STREAM( "otherLinked\n");
01922                 }
01923                 else {
01924                     ROS_WARN("convexMesh polyCount = %d\n",(int)convexRef->getPolygons_array().getCount());
01925                     ROS_WARN("convexMesh triCount = %d\n",(int)convexRef->getTriangles_array().getCount());
01926                 }
01927             }
01928 
01929             const domConvex_meshRef convexRef = geom->getConvex_mesh();
01930             //daeString urlref = convexRef->getConvex_hull_of().getURI();
01931             daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
01932             if (urlref2) {
01933                 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01934 
01935                 // Load all the geometry libraries
01936                 for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
01937                     domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
01938                     for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
01939                         domGeometryRef lib = libgeom->getGeometry_array()[i];
01940                         if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2
01941                             //found convex_hull geometry
01942                             domMesh *meshElement = lib->getMesh(); //linkedGeom->getMesh();
01943                             if (meshElement) {
01944                                 const domVerticesRef vertsRef = meshElement->getVertices();
01945                                 for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
01946                                     domInput_localRef localRef = vertsRef->getInput_array()[i];
01947                                     daeString str = localRef->getSemantic();
01948                                     if ( strcmp(str,"POSITION") == 0) {
01949                                         const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01950                                         if( !node ) {
01951                                             continue;
01952                                         }
01953                                         double fUnitScale = _GetUnitScale(node);
01954                                         const domFloat_arrayRef flArray = node->getFloat_array();
01955                                         if (!!flArray) {
01956                                             vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01957                                             const domList_of_floats& listFloats = flArray->getValue();
01958                                             for (size_t k=0; k+2<flArray->getCount(); k+=3) {
01959                                                 domFloat fl0 = listFloats.get(k);
01960                                                 domFloat fl1 = listFloats.get(k+1);
01961                                                 domFloat fl2 = listFloats.get(k+2);
01962                                                 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01963                                             }
01964                                         }
01965                                     }
01966                                 }
01967                             }
01968                         }
01969                     }
01970                 }
01971             }
01972             else {
01973                 //no getConvex_hull_of but direct vertices
01974                 const domVerticesRef vertsRef = convexRef->getVertices();
01975                 for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
01976                     domInput_localRef localRef = vertsRef->getInput_array()[i];
01977                     daeString str = localRef->getSemantic();
01978                     if ( strcmp(str,"POSITION") == 0 ) {
01979                         const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01980                         if( !node ) {
01981                             continue;
01982                         }
01983                         double fUnitScale = _GetUnitScale(node);
01984                         const domFloat_arrayRef flArray = node->getFloat_array();
01985                         if (!!flArray) {
01986                             const domList_of_floats& listFloats = flArray->getValue();
01987                             vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01988                             for (size_t k=0; k+2<flArray->getCount(); k+=3) {
01989                                 domFloat fl0 = listFloats.get(k);
01990                                 domFloat fl1 = listFloats.get(k+1);
01991                                 domFloat fl2 = listFloats.get(k+2);
01992                                 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01993                             }
01994                         }
01995                     }
01996                 }
01997             }
01998 
01999             if( vconvexhull.size()> 0 ) {
02000                 listGeomProperties.push_back(GEOMPROPERTIES());
02001                 GEOMPROPERTIES& geom = listGeomProperties.back();
02002                 geom.type = GeomTrimesh;
02003 
02004                 //_computeConvexHull(vconvexhull,trimesh);
02005                 geom.InitCollisionMesh();
02006             }
02007             return true;
02008         }
02009 
02010         return false;
02011     }
02012 
02014     void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
02015     {
02016         // over write joint parameters by elements in instance_actuator
02017         for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
02018             domExtraRef pextra = as->getExtra_array()[ie];
02019             if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
02020                 //std::string aname = pextra->getAttribute("name");
02021                 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
02022                 if( !!tec ) {
02023                     boost::shared_ptr<Joint> pjoint;
02024                     daeElementRef domactuator;
02025                     {
02026                         daeElementRef bact = tec->getChild("bind_actuator");
02027                         pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as);
02028                         if (!pjoint) continue;
02029                     }
02030                     {
02031                         daeElementRef iact = tec->getChild("instance_actuator");
02032                         if(!iact) continue;
02033                         std::string instance_url = iact->getAttribute("url");
02034                         domactuator = daeURI(*iact, instance_url).getElement();
02035                         if(!domactuator) continue;
02036                     }
02037                     daeElement *nom_torque = domactuator->getChild("nominal_torque");
02038                     if ( !! nom_torque ) {
02039                         if( !! pjoint->limits ) {
02040                             pjoint->limits->effort = boost::lexical_cast<double>(nom_torque->getCharData());
02041                             ROS_DEBUG("effort limit at joint (%s) is over written by %f",
02042                                       pjoint->name.c_str(), pjoint->limits->effort);
02043                         }
02044                     }
02045                 }
02046             }
02047         }
02048     }
02049 
02051     void _ExtractRobotManipulators(const domArticulated_systemRef as)
02052     {
02053         ROS_DEBUG("collada manipulators not supported yet");
02054     }
02055 
02057     void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
02058     {
02059         ROS_DEBUG("collada sensors not supported yet");
02060     }
02061 
02062     inline daeElementRef _getElementFromUrl(const daeURI &uri)
02063     {
02064         return daeStandardURIResolver(*_collada).resolveElement(uri);
02065     }
02066 
02067     static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
02068     {
02069         if( !!paddr->getSIDREF() ) {
02070             return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt;
02071         }
02072         if (!!paddr->getParam()) {
02073             return searchBinding(paddr->getParam()->getValue(),parent);
02074         }
02075         return NULL;
02076     }
02077 
02081     static daeElement* searchBinding(daeString ref, daeElementRef parent)
02082     {
02083         if( !parent ) {
02084             return NULL;
02085         }
02086         daeElement* pelt = NULL;
02087         domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
02088         if( !!kscene ) {
02089             pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
02090             if( !!pelt ) {
02091                 return pelt;
02092             }
02093             return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
02094         }
02095         domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
02096         if( !!articulated_system ) {
02097             if( !!articulated_system->getKinematics() ) {
02098                 pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
02099                 if( !!pelt ) {
02100                     return pelt;
02101                 }
02102             }
02103             if( !!articulated_system->getMotion() ) {
02104                 return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
02105             }
02106             return NULL;
02107         }
02108         // try to get a bind array
02109         daeElementRef pbindelt;
02110         const domKinematics_bind_Array* pbindarray = NULL;
02111         const domKinematics_newparam_Array* pnewparamarray = NULL;
02112         domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
02113         if( !!ias ) {
02114             pbindarray = &ias->getBind_array();
02115             pbindelt = ias->getUrl().getElement();
02116             pnewparamarray = &ias->getNewparam_array();
02117         }
02118         if( !pbindarray || !pbindelt ) {
02119             domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
02120             if( !!ikm ) {
02121                 pbindarray = &ikm->getBind_array();
02122                 pbindelt = ikm->getUrl().getElement();
02123                 pnewparamarray = &ikm->getNewparam_array();
02124             }
02125         }
02126         if( !!pbindarray && !!pbindelt ) {
02127             for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
02128                 domKinematics_bindRef pbind = (*pbindarray)[ibind];
02129                 if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
02130                     // found a match
02131                     if( !!pbind->getParam() ) {
02132                         return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
02133                     }
02134                     else if( !!pbind->getSIDREF() ) {
02135                         return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
02136                     }
02137                 }
02138             }
02139             for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
02140                 domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
02141                 if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
02142                     if( !!newparam->getSIDREF() ) { // can only bind with SIDREF
02143                         return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt;
02144                     }
02145                     ROS_WARN_STREAM(str(boost::format("newparam sid=%s does not have SIDREF\n")%newparam->getSid()));
02146                 }
02147             }
02148         }
02149         ROS_WARN_STREAM(str(boost::format("failed to get binding '%s' for element: %s\n")%ref%parent->getElementName()));
02150         return NULL;
02151     }
02152 
02153     static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray)
02154     {
02155         for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02156             daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02157             if( !!pelt ) {
02158                 return pelt;
02159             }
02160         }
02161         return NULL;
02162     }
02163 
02164     static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray)
02165     {
02166         for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02167             daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02168             if( !!pelt ) {
02169                 return pelt;
02170             }
02171         }
02172         return NULL;
02173     }
02174 
02175     template <typename U> static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) {
02176         if( !!paddr->getBool() ) {
02177             return paddr->getBool()->getValue();
02178         }
02179         if( !paddr->getParam() ) {
02180             ROS_WARN_STREAM("param not specified, setting to 0\n");
02181             return false;
02182         }
02183         for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02184             domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02185             if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02186                 if( !!pnewparam->getBool() ) {
02187                     return pnewparam->getBool()->getValue();
02188                 }
02189                 else if( !!pnewparam->getSIDREF() ) {
02190                     domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02191                     if( !ptarget ) {
02192                         ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
02193                         continue;
02194                     }
02195                     return ptarget->getValue();
02196                 }
02197             }
02198         }
02199         ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
02200         return false;
02201     }
02202     template <typename U> static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) {
02203         if( !!paddr->getFloat() ) {
02204             return paddr->getFloat()->getValue();
02205         }
02206         if( !paddr->getParam() ) {
02207             ROS_WARN_STREAM("param not specified, setting to 0\n");
02208             return 0;
02209         }
02210         for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02211             domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02212             if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02213                 if( !!pnewparam->getFloat() ) {
02214                     return pnewparam->getFloat()->getValue();
02215                 }
02216                 else if( !!pnewparam->getSIDREF() ) {
02217                     domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02218                     if( !ptarget ) {
02219                         ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
02220                         continue;
02221                     }
02222                     return ptarget->getValue();
02223                 }
02224             }
02225         }
02226         ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
02227         return 0;
02228     }
02229 
02230     static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f)
02231     {
02232         daeElement* pfloat = pcommon->getChild("float");
02233         if( !!pfloat ) {
02234             std::stringstream sfloat(pfloat->getCharData());
02235             sfloat >> f;
02236             return !!sfloat;
02237         }
02238         daeElement* pparam = pcommon->getChild("param");
02239         if( !!pparam ) {
02240             if( pparam->hasAttribute("ref") ) {
02241                 ROS_WARN_STREAM("cannot process param ref\n");
02242             }
02243             else {
02244                 daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt;
02245                 if( !!pelt ) {
02246                     ROS_WARN_STREAM(str(boost::format("found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData()));
02247                 }
02248             }
02249         }
02250         return false;
02251     }
02252 
02253     static boost::array<double,12> _matrixIdentity()
02254     {
02255         boost::array<double,12> m = {{1,0,0,0,0,1,0,0,0,0,1,0}};
02256         return m;
02257     };
02258 
02260     static boost::array<double,12> _getTransform(daeElementRef pelt)
02261     {
02262         boost::array<double,12> m = _matrixIdentity();
02263         domRotateRef protate = daeSafeCast<domRotate>(pelt);
02264         if( !!protate ) {
02265             m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (double)(protate->getValue()[3]*(M_PI/180.0)));
02266             return m;
02267         }
02268 
02269         domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
02270         if( !!ptrans ) {
02271             double scale = _GetUnitScale(pelt);
02272             m[3] = ptrans->getValue()[0]*scale;
02273             m[7] = ptrans->getValue()[1]*scale;
02274             m[11] = ptrans->getValue()[2]*scale;
02275             return m;
02276         }
02277 
02278         domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
02279         if( !!pmat ) {
02280             double scale = _GetUnitScale(pelt);
02281             for(int i = 0; i < 3; ++i) {
02282                 m[4*i+0] = pmat->getValue()[4*i+0];
02283                 m[4*i+1] = pmat->getValue()[4*i+1];
02284                 m[4*i+2] = pmat->getValue()[4*i+2];
02285                 m[4*i+3] = pmat->getValue()[4*i+3]*scale;
02286             }
02287             return m;
02288         }
02289 
02290         domScaleRef pscale = daeSafeCast<domScale>(pelt);
02291         if( !!pscale ) {
02292             m[0] = pscale->getValue()[0];
02293             m[4*1+1] = pscale->getValue()[1];
02294             m[4*2+2] = pscale->getValue()[2];
02295             return m;
02296         }
02297 
02298         domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
02299         if( pelt->typeID() == domLookat::ID() ) {
02300             ROS_ERROR_STREAM("look at transform not implemented\n");
02301             return m;
02302         }
02303 
02304         domSkewRef pskew = daeSafeCast<domSkew>(pelt);
02305         if( !!pskew ) {
02306             ROS_ERROR_STREAM("skew transform not implemented\n");
02307         }
02308 
02309         return m;
02310     }
02311 
02314     template <typename T> static boost::array<double,12> _getNodeParentTransform(const T pelt) {
02315         domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
02316         if( !pnode ) {
02317             return _matrixIdentity();
02318         }
02319         return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode));
02320     }
02321 
02323     template <typename T> static boost::array<double,12> _ExtractFullTransform(const T pelt) {
02324         boost::array<double,12> t = _matrixIdentity();
02325         for(size_t i = 0; i < pelt->getContents().getCount(); ++i) {
02326             t = _poseMult(t, _getTransform(pelt->getContents()[i]));
02327         }
02328         return t;
02329     }
02330 
02332     template <typename T> static boost::array<double,12> _ExtractFullTransformFromChildren(const T pelt) {
02333         boost::array<double,12> t = _matrixIdentity();
02334         daeTArray<daeElementRef> children; pelt->getChildren(children);
02335         for(size_t i = 0; i < children.getCount(); ++i) {
02336             t = _poseMult(t, _getTransform(children[i]));
02337         }
02338         return t;
02339     }
02340 
02341     // decompose a matrix into a scale and rigid transform (necessary for model scales)
02342     void _decompose(const boost::array<double,12>& tm, Pose& tout, Vector3& vscale)
02343     {
02344         vscale.x = 1; vscale.y = 1; vscale.z = 1;
02345         tout = _poseFromMatrix(tm);
02346     }
02347 
02348     virtual void handleError( daeString msg )
02349     {
02350         ROS_ERROR("COLLADA error: %s\n", msg);
02351     }
02352 
02353     virtual void handleWarning( daeString msg )
02354     {
02355         ROS_WARN("COLLADA warning: %s\n", msg);
02356     }
02357 
02358     inline static double _GetUnitScale(daeElement* pelt)
02359     {
02360         return ((USERDATA*)pelt->getUserData())->scale;
02361     }
02362 
02363     domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr)
02364     {
02365         for(size_t i = 0; i < arr.getCount(); ++i) {
02366             if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) {
02367                 return arr[i];
02368             }
02369         }
02370         return domTechniqueRef();
02371     }
02372 
02374     boost::shared_ptr<std::string> _ExtractInterfaceType(const domExtra_Array& arr) {
02375         for(size_t i = 0; i < arr.getCount(); ++i) {
02376             if( strcmp(arr[i]->getType(),"interface_type") == 0 ) {
02377                 domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
02378                 if( !!tec ) {
02379                     daeElement* ptype = tec->getChild("interface");
02380                     if( !!ptype ) {
02381                         return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
02382                     }
02383                 }
02384             }
02385         }
02386         return boost::shared_ptr<std::string>();
02387     }
02388 
02389     std::string _ExtractLinkName(domLinkRef pdomlink) {
02390         std::string linkname;
02391         if( !!pdomlink ) {
02392             if( !!pdomlink->getName() ) {
02393                 linkname = pdomlink->getName();
02394             }
02395             if( linkname.size() == 0 && !!pdomlink->getID() ) {
02396                 linkname = pdomlink->getID();
02397             }
02398         }
02399         return linkname;
02400     }
02401 
02402     bool _checkMathML(daeElementRef pelt,const std::string& type)
02403     {
02404         if( pelt->getElementName()==type ) {
02405             return true;
02406         }
02407         // check the substring after ':'
02408         std::string name = pelt->getElementName();
02409         std::size_t pos = name.find_last_of(':');
02410         if( pos == std::string::npos ) {
02411             return false;
02412         }
02413         return name.substr(pos+1)==type;
02414     }
02415 
02416     boost::shared_ptr<Joint> _getJointFromRef(xsToken targetref, daeElementRef peltref) {
02417         daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
02418         domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
02419 
02420         if (!pdomjoint) {
02421             domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
02422             if (!!pdomijoint) {
02423                 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
02424             }
02425         }
02426 
02427         if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
02428             ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref));
02429             return boost::shared_ptr<Joint>();
02430         }
02431 
02432         boost::shared_ptr<Joint> pjoint;
02433         std::string name(pdomjoint->getName());
02434         if (_model->joints_.find(name) == _model->joints_.end()) {
02435             pjoint.reset();
02436         } else {
02437             pjoint = _model->joints_.find(name)->second;
02438         }
02439         if(!pjoint) {
02440             ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
02441         }
02442         return pjoint;
02443     }
02444 
02448     static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings)
02449     {
02450         domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
02451         if (!kscene) {
02452             return;
02453         }
02454         for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
02455             domArticulated_systemRef articulated_system; // if filled, contains robot-specific information, so create a robot
02456             domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
02457             if (!kbindmodel->getNode()) {
02458                 ROS_WARN_STREAM("do not support kinematics models without references to nodes\n");
02459                 continue;
02460             }
02461 
02462             // visual information
02463             domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt);
02464             if (!node) {
02465                 ROS_WARN_STREAM(str(boost::format("bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode()));
02466                 continue;
02467             }
02468 
02469             //  kinematics information
02470             daeElement* pelt = searchBinding(kbindmodel,kscene);
02471             domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
02472             if (!kimodel) {
02473                 if( !pelt ) {
02474                     ROS_WARN_STREAM("bind_kinematics_model does not reference element\n");
02475                 }
02476                 else {
02477                     ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName()));
02478                 }
02479                 continue;
02480             }
02481             bindings.listKinematicsVisualBindings.push_back(std::make_pair(node,kimodel));
02482         }
02483         // axis info
02484         for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
02485             domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
02486             daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt;
02487             if (!pjtarget) {
02488                 ROS_ERROR_STREAM(str(boost::format("Target Node %s NOT found!!!\n")%bindjoint->getTarget()));
02489                 continue;
02490             }
02491             daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
02492             domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
02493             if (!pjointaxis) {
02494                 continue;
02495             }
02496             bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL));
02497         }
02498     }
02499 
02500     static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings)
02501     {
02502         for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
02503             domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
02504             for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
02505                 domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
02506                 domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
02507                 domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
02508                 for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
02509                     LinkBinding lb;
02510                     lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
02511                     lb.node = daeSafeCast<domNode>(lb.irigidbody->getTarget().getElement().cast());
02512                     lb.rigidbody = daeSafeCast<domRigid_body>(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt);
02513                     lb.nodephysicsoffset = nodephysicsoffset;
02514                     if( !!lb.rigidbody && !!lb.node ) {
02515                         bindings.listLinkBindings.push_back(lb);
02516                     }
02517                 }
02518             }
02519         }
02520     }
02521 
02522 
02523     size_t _countChildren(daeElement* pelt) {
02524         size_t c = 1;
02525         daeTArray<daeElementRef> children;
02526         pelt->getChildren(children);
02527         for (size_t i = 0; i < children.getCount(); ++i) {
02528             c += _countChildren(children[i]);
02529         }
02530         return c;
02531     }
02532 
02533     void _processUserData(daeElement* pelt, double scale)
02534     {
02535         // getChild could be optimized since asset tag is supposed to appear as the first element
02536         domAssetRef passet = daeSafeCast<domAsset> (pelt->getChild("asset"));
02537         if (!!passet && !!passet->getUnit()) {
02538             scale = passet->getUnit()->getMeter();
02539         }
02540 
02541         _vuserdata.push_back(USERDATA(scale));
02542         pelt->setUserData(&_vuserdata.back());
02543         daeTArray<daeElementRef> children;
02544         pelt->getChildren(children);
02545         for (size_t i = 0; i < children.getCount(); ++i) {
02546             if (children[i] != passet) {
02547                 _processUserData(children[i], scale);
02548             }
02549         }
02550     }
02551 
02552     USERDATA* _getUserData(daeElement* pelt)
02553     {
02554         BOOST_ASSERT(!!pelt);
02555         void* p = pelt->getUserData();
02556         BOOST_ASSERT(!!p);
02557         return (USERDATA*)p;
02558     }
02559 
02560     //
02561     // openrave math functions (from geometry.h)
02562     //
02563 
02564     static Vector3 _poseMult(const Pose& p, const Vector3& v)
02565     {
02566         double ww = 2 * p.rotation.x * p.rotation.x;
02567         double wx = 2 * p.rotation.x * p.rotation.y;
02568         double wy = 2 * p.rotation.x * p.rotation.z;
02569         double wz = 2 * p.rotation.x * p.rotation.w;
02570         double xx = 2 * p.rotation.y * p.rotation.y;
02571         double xy = 2 * p.rotation.y * p.rotation.z;
02572         double xz = 2 * p.rotation.y * p.rotation.w;
02573         double yy = 2 * p.rotation.z * p.rotation.z;
02574         double yz = 2 * p.rotation.z * p.rotation.w;
02575         Vector3 vnew;
02576         vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
02577         vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
02578         vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
02579         return vnew;
02580     }
02581 
02582     static Vector3 _poseMult(const boost::array<double,12>& m, const Vector3& v)
02583     {
02584         Vector3 vnew;
02585         vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3];
02586         vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3];
02587         vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3];
02588         return vnew;
02589     }
02590 
02591     static boost::array<double,12> _poseMult(const boost::array<double,12>& m0, const boost::array<double,12>& m1)
02592     {
02593         boost::array<double,12> mres;
02594         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];
02595         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];
02596         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];
02597         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];
02598         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];
02599         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];
02600         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];
02601         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];
02602         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];
02603         mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
02604         mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
02605         mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
02606         return mres;
02607     }
02608 
02609     static Pose _poseMult(const Pose& p0, const Pose& p1)
02610     {
02611         Pose p;
02612         p.position = _poseMult(p0,p1.position);
02613         p.rotation = _quatMult(p0.rotation,p1.rotation);
02614         return p;
02615     }
02616 
02617     static Pose _poseInverse(const Pose& p)
02618     {
02619         Pose pinv;
02620         pinv.rotation.x = -p.rotation.x;
02621         pinv.rotation.y = -p.rotation.y;
02622         pinv.rotation.z = -p.rotation.z;
02623         pinv.rotation.w = p.rotation.w;
02624         Vector3 t = _poseMult(pinv,p.position);
02625         pinv.position.x = -t.x;
02626         pinv.position.y = -t.y;
02627         pinv.position.z = -t.z;
02628         return pinv;
02629     }
02630 
02631     static Rotation _quatMult(const Rotation& quat0, const Rotation& quat1)
02632     {
02633         Rotation q;
02634         q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
02635         q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
02636         q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
02637         q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
02638         double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
02639         // don't touch the divides
02640         q.x /= fnorm;
02641         q.y /= fnorm;
02642         q.z /= fnorm;
02643         q.w /= fnorm;
02644         return q;
02645     }
02646 
02647     static boost::array<double,12> _matrixFromAxisAngle(const Vector3& axis, double angle)
02648     {
02649         return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle));
02650     }
02651 
02652     static boost::array<double,12> _matrixFromQuat(const Rotation& quat)
02653     {
02654         boost::array<double,12> m;
02655         double qq1 = 2*quat.x*quat.x;
02656         double qq2 = 2*quat.y*quat.y;
02657         double qq3 = 2*quat.z*quat.z;
02658         m[4*0+0] = 1 - qq2 - qq3;
02659         m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z);
02660         m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y);
02661         m[4*0+3] = 0;
02662         m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z);
02663         m[4*1+1] = 1 - qq1 - qq3;
02664         m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x);
02665         m[4*1+3] = 0;
02666         m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y);
02667         m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x);
02668         m[4*2+2] = 1 - qq1 - qq2;
02669         m[4*2+3] = 0;
02670         return m;
02671     }
02672 
02673     static Pose _poseFromMatrix(const boost::array<double,12>& m)
02674     {
02675         Pose t;
02676         t.rotation = _quatFromMatrix(m);
02677         t.position.x = m[3];
02678         t.position.y = m[7];
02679         t.position.z = m[11];
02680         return t;
02681     }
02682 
02683     static boost::array<double,12> _matrixFromPose(const Pose& t)
02684     {
02685         boost::array<double,12> m = _matrixFromQuat(t.rotation);
02686         m[3] = t.position.x;
02687         m[7] = t.position.y;
02688         m[11] = t.position.z;
02689         return m;
02690     }
02691 
02692     static Rotation _quatFromAxisAngle(double x, double y, double z, double angle)
02693     {
02694         Rotation q;
02695         double axislen = std::sqrt(x*x+y*y+z*z);
02696         if( axislen == 0 ) {
02697             return q;
02698         }
02699         angle *= 0.5;
02700         double sang = std::sin(angle)/axislen;
02701         q.w = std::cos(angle);
02702         q.x = x*sang;
02703         q.y = y*sang;
02704         q.z = z*sang;
02705         return q;
02706     }
02707 
02708     static Rotation _quatFromMatrix(const boost::array<double,12>& mat)
02709     {
02710         Rotation rot;
02711         double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
02712         if (tr >= 0) {
02713             rot.w = tr + 1;
02714             rot.x = (mat[4*2+1] - mat[4*1+2]);
02715             rot.y = (mat[4*0+2] - mat[4*2+0]);
02716             rot.z = (mat[4*1+0] - mat[4*0+1]);
02717         }
02718         else {
02719             // find the largest diagonal element and jump to the appropriate case
02720             if (mat[4*1+1] > mat[4*0+0]) {
02721                 if (mat[4*2+2] > mat[4*1+1]) {
02722                     rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
02723                     rot.x = (mat[4*2+0] + mat[4*0+2]);
02724                     rot.y = (mat[4*1+2] + mat[4*2+1]);
02725                     rot.w = (mat[4*1+0] - mat[4*0+1]);
02726                 }
02727                 else {
02728                     rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
02729                     rot.z = (mat[4*1+2] + mat[4*2+1]);
02730                     rot.x = (mat[4*0+1] + mat[4*1+0]);
02731                     rot.w = (mat[4*0+2] - mat[4*2+0]);
02732                 }
02733             }
02734             else if (mat[4*2+2] > mat[4*0+0]) {
02735                 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
02736                 rot.x = (mat[4*2+0] + mat[4*0+2]);
02737                 rot.y = (mat[4*1+2] + mat[4*2+1]);
02738                 rot.w = (mat[4*1+0] - mat[4*0+1]);
02739             }
02740             else {
02741                 rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
02742                 rot.y = (mat[4*0+1] + mat[4*1+0]);
02743                 rot.z = (mat[4*2+0] + mat[4*0+2]);
02744                 rot.w = (mat[4*2+1] - mat[4*1+2]);
02745             }
02746         }
02747         double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
02748         // don't touch the divides
02749         rot.x /= fnorm;
02750         rot.y /= fnorm;
02751         rot.z /= fnorm;
02752         rot.w /= fnorm;
02753         return rot;
02754     }
02755 
02756     static double _dot3(const Vector3& v0, const Vector3& v1)
02757     {
02758         return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
02759     }
02760     static Vector3 _cross3(const Vector3& v0, const Vector3& v1)
02761     {
02762         Vector3 v;
02763         v.x = v0.y * v1.z - v0.z * v1.y;
02764         v.y = v0.z * v1.x - v0.x * v1.z;
02765         v.z = v0.x * v1.y - v0.y * v1.x;
02766         return v;
02767     }
02768     static Vector3 _sub3(const Vector3& v0, const Vector3& v1)
02769     {
02770         Vector3 v;
02771         v.x = v0.x-v1.x;
02772         v.y = v0.y-v1.y;
02773         v.z = v0.z-v1.z;
02774         return v;
02775     }
02776     static Vector3 _add3(const Vector3& v0, const Vector3& v1)
02777     {
02778         Vector3 v;
02779         v.x = v0.x+v1.x;
02780         v.y = v0.y+v1.y;
02781         v.z = v0.z+v1.z;
02782         return v;
02783     }
02784     static Vector3 _normalize3(const Vector3& v0)
02785     {
02786         Vector3 v;
02787         double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z);
02788         v.x = v0.x/norm;
02789         v.y = v0.y/norm;
02790         v.z = v0.z/norm;
02791         return v;
02792     }
02793 
02794     boost::shared_ptr<DAE> _collada;
02795     domCOLLADA* _dom;
02796     std::vector<USERDATA> _vuserdata; // all userdata
02797     int _nGlobalSensorId, _nGlobalManipulatorId;
02798     std::string _filename;
02799     std::string _resourcedir;
02800     boost::shared_ptr<ModelInterface> _model;
02801     Pose _RootOrigin;
02802     Pose _VisualRootOrigin;
02803 };
02804 
02805 
02806 
02807 
02808 boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str)
02809 {
02810     boost::shared_ptr<ModelInterface> model(new ModelInterface);
02811 
02812     ColladaModelReader reader(model);
02813     if (!reader.InitFromData(xml_str))
02814         model.reset();
02815     return model;
02816 }
02817 }


collada_parser
Author(s): Rosen Diankov, Kei Okada
autogenerated on Thu Aug 27 2015 14:40:54