collada_parser.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, University of Tokyo
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redstributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Rosen Diankov, used OpenRAVE files for reference */
36 #include <cmath>
37 #include <cstdint>
38 #include <cstdlib>
39 #include <list>
40 #include <map>
41 #include <string>
42 #include <sstream>
43 #include <vector>
44 
45 /* disable deprecated auto_ptr warnings */
46 #pragma GCC diagnostic push
47 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
48 #include <dae.h>
49 #include <dae/daeErrorHandler.h>
50 #pragma GCC diagnostic pop
51 
52 #include <dom/domCOLLADA.h>
53 #include <dae/domAny.h>
54 #include <dom/domConstants.h>
55 #include <dom/domTriangles.h>
56 #include <dae/daeStandardURIResolver.h>
57 
58 #include <boost/array.hpp>
59 #include <boost/assert.hpp>
60 #include <boost/format.hpp>
61 #include <boost/lexical_cast.hpp>
62 #include <boost/shared_ptr.hpp>
63 
64 #include <ros/console.h>
66 #include <urdf_model/model.h>
67 
68 #ifndef HAVE_MKSTEMPS
69 #include <fstream>
70 #include <fcntl.h>
71 #endif
72 
73 #define typeof __typeof__
74 #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++)
75 #define FOREACHC FOREACH
76 
77 namespace ColladaDOM150 { }
78 
79 namespace urdf {
80 
81 using namespace ColladaDOM150;
82 
84 {
85 public:
86  UnlinkFilename(const std::string& filename) : _filename(filename) {
87  }
88  virtual ~UnlinkFilename() {
89  unlink(_filename.c_str());
90  }
91  std::string _filename;
92 };
93 static std::list<boost::shared_ptr<UnlinkFilename> > _listTempFilenames;
94 
95 class ColladaModelReader : public daeErrorHandler
96 {
97 
99  {
100 public:
101  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) {
102  BOOST_ASSERT( !!pkinematicaxis );
103  daeElement* pae = pvisualtrans->getParentElement();
104  while (!!pae) {
105  visualnode = daeSafeCast<domNode> (pae);
106  if (!!visualnode) {
107  break;
108  }
109  pae = pae->getParentElement();
110  }
111 
112  if (!visualnode) {
113  ROS_WARN_STREAM(str(boost::format("couldn't find parent node of element id %s, sid %s\n")%pkinematicaxis->getID()%pkinematicaxis->getSid()));
114  }
115  }
116 
117  daeElementRef pvisualtrans;
118  domAxis_constraintRef pkinematicaxis;
119  domCommon_float_or_paramRef jointvalue;
120  domNodeRef visualnode;
121  domKinematics_axis_infoRef kinematics_axis_info;
122  domMotion_axis_infoRef motion_axis_info;
123  };
124 
127  {
128 public:
129  domNodeRef node;
130  domLinkRef domlink;
131  domInstance_rigid_bodyRef irigidbody;
132  domRigid_bodyRef rigidbody;
133  domNodeRef nodephysicsoffset;
134  };
135 
138  {
139 public:
140  std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings;
141  std::list<JointAxisBinding> listAxisBindings;
142  std::list<LinkBinding> listLinkBindings;
143 
144  bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
145  {
146  if( !kinematics_axis_info ) {
147  return false;
148  }
149  for(size_t ik = 0; ik < arr.getCount(); ++ik) {
150  daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt;
151  if( !!pelt ) {
152  // look for the correct placement
153  bool bfound = false;
154  FOREACH(itbinding,listAxisBindings) {
155  if( itbinding->pkinematicaxis.cast() == pelt ) {
156  itbinding->kinematics_axis_info = kinematics_axis_info;
157  if( !!motion_axis_info ) {
158  itbinding->motion_axis_info = motion_axis_info;
159  }
160  bfound = true;
161  break;
162  }
163  }
164  if( !bfound ) {
165  ROS_WARN_STREAM(str(boost::format("could not find binding for axis: %s, %s\n")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid")));
166  return false;
167  }
168  return true;
169  }
170  }
171  ROS_WARN_STREAM(str(boost::format("could not find kinematics axis target: %s\n")%kinematics_axis_info->getAxis()));
172  return false;
173  }
174  };
175 
176  struct USERDATA
177  {
179  }
180  USERDATA(double scale) : scale(scale) {
181  }
182  double scale;
183 #if URDFDOM_HEADERS_MAJOR_VERSION < 1
185 #else
186  std::shared_ptr<void> p;
187 #endif
188  };
189 
190  enum GeomType {
191  GeomNone = 0,
192  GeomBox = 1,
193  GeomSphere = 2,
194  GeomCylinder = 3,
195  GeomTrimesh = 4,
196  };
197 
199  {
200  Pose _t;
201  Vector3 vGeomData;
202  Color diffuseColor, ambientColor;
206  std::vector<Vector3> vertices;
207  std::vector<int> indices;
208 
211 
212 
213  // generate a sphere triangulation starting with an icosahedron
214  // all triangles are oriented counter clockwise
215  static void GenerateSphereTriangulation(std::vector<Vector3> realvertices, std::vector<int> realindices, int levels)
216  {
217  const double GTS_M_ICOSAHEDRON_X = 0.850650808352039932181540497063011072240401406;
218  const double GTS_M_ICOSAHEDRON_Y = 0.525731112119133606025669084847876607285497935;
219  const double GTS_M_ICOSAHEDRON_Z = 0;
220  std::vector<Vector3> tempvertices[2];
221  std::vector<int> tempindices[2];
222 
223  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
224  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
225  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
226  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
227  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
228  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
229  tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
230  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
231  tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
232  tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
233  tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
234  tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
235 
236  const int nindices=60;
237  int indices[nindices] = {
238  0, 1, 2,
239  1, 3, 4,
240  3, 5, 6,
241  2, 4, 7,
242  5, 6, 8,
243  2, 7, 9,
244  0, 5, 8,
245  7, 9, 10,
246  0, 1, 5,
247  7, 10, 11,
248  1, 3, 5,
249  6, 10, 11,
250  3, 6, 11,
251  9, 10, 8,
252  3, 4, 11,
253  6, 8, 10,
254  4, 7, 11,
255  1, 2, 4,
256  0, 8, 9,
257  0, 2, 9
258  };
259 
260  Vector3 v[3];
261  // make sure oriented CCW
262  for(int i = 0; i < nindices; i += 3 ) {
263  v[0] = tempvertices[0][indices[i]];
264  v[1] = tempvertices[0][indices[i+1]];
265  v[2] = tempvertices[0][indices[i+2]];
266  if( _dot3(v[0], _cross3(_sub3(v[1],v[0]),_sub3(v[2],v[0]))) < 0 ) {
267  std::swap(indices[i], indices[i+1]);
268  }
269  }
270 
271  tempindices[0].resize(nindices);
272  std::copy(&indices[0],&indices[nindices],tempindices[0].begin());
273  std::vector<Vector3>* curvertices = &tempvertices[0], *newvertices = &tempvertices[1];
274  std::vector<int> *curindices = &tempindices[0], *newindices = &tempindices[1];
275  while(levels-- > 0) {
276 
277  newvertices->resize(0);
278  newvertices->reserve(2*curvertices->size());
279  newvertices->insert(newvertices->end(), curvertices->begin(), curvertices->end());
280  newindices->resize(0);
281  newindices->reserve(4*curindices->size());
282 
283  std::map< uint64_t, int > mapnewinds;
284  std::map< uint64_t, int >::iterator it;
285 
286  for(size_t i = 0; i < curindices->size(); i += 3) {
287  // for ever tri, create 3 new vertices and 4 new triangles.
288  v[0] = curvertices->at(curindices->at(i));
289  v[1] = curvertices->at(curindices->at(i+1));
290  v[2] = curvertices->at(curindices->at(i+2));
291 
292  int inds[3];
293  for(int j = 0; j < 3; ++j) {
294  uint64_t key = ((uint64_t)curindices->at(i+j)<<32)|(uint64_t)curindices->at(i + ((j+1)%3));
295  it = mapnewinds.find(key);
296 
297  if( it == mapnewinds.end() ) {
298  inds[j] = mapnewinds[key] = mapnewinds[(key<<32)|(key>>32)] = (int)newvertices->size();
299  newvertices->push_back(_normalize3(_add3(v[j],v[(j+1)%3 ])));
300  }
301  else {
302  inds[j] = it->second;
303  }
304  }
305 
306  newindices->push_back(curindices->at(i)); newindices->push_back(inds[0]); newindices->push_back(inds[2]);
307  newindices->push_back(inds[0]); newindices->push_back(curindices->at(i+1)); newindices->push_back(inds[1]);
308  newindices->push_back(inds[2]); newindices->push_back(inds[0]); newindices->push_back(inds[1]);
309  newindices->push_back(inds[2]); newindices->push_back(inds[1]); newindices->push_back(curindices->at(i+2));
310  }
311 
312  std::swap(newvertices,curvertices);
313  std::swap(newindices,curindices);
314  }
315 
316  realvertices = *curvertices;
317  realindices = *curindices;
318  }
319 
320  bool InitCollisionMesh(double fTessellation=1.0)
321  {
322  if( type == GeomTrimesh ) {
323  return true;
324  }
325  indices.clear();
326  vertices.clear();
327 
328  if( fTessellation < 0.01f ) {
329  fTessellation = 0.01f;
330  }
331  // start tesselating
332  switch(type) {
333  case GeomSphere: {
334  // log_2 (1+ tess)
335  GenerateSphereTriangulation(vertices,indices, 3 + (int)(logf(fTessellation) / logf(2.0f)) );
336  double fRadius = vGeomData.x;
337  FOREACH(it, vertices) {
338  it->x *= fRadius;
339  it->y *= fRadius;
340  it->z *= fRadius;
341  }
342  break;
343  }
344  case GeomBox: {
345  // trivial
346  Vector3 ex = vGeomData;
347  Vector3 v[8] = { Vector3(ex.x, ex.y, ex.z),
348  Vector3(ex.x, ex.y, -ex.z),
349  Vector3(ex.x, -ex.y, ex.z),
350  Vector3(ex.x, -ex.y, -ex.z),
351  Vector3(-ex.x, ex.y, ex.z),
352  Vector3(-ex.x, ex.y, -ex.z),
353  Vector3(-ex.x, -ex.y, ex.z),
354  Vector3(-ex.x, -ex.y, -ex.z) };
355  const int nindices = 36;
356  int startindices[] = {
357  0, 1, 2,
358  1, 2, 3,
359  4, 5, 6,
360  5, 6, 7,
361  0, 1, 4,
362  1, 4, 5,
363  2, 3, 6,
364  3, 6, 7,
365  0, 2, 4,
366  2, 4, 6,
367  1, 3, 5,
368  3, 5, 7
369  };
370 
371  for(int i = 0; i < nindices; i += 3 ) {
372  Vector3 v1 = v[startindices[i]];
373  Vector3 v2 = v[startindices[i+1]];
374  Vector3 v3 = v[startindices[i+2]];
375  if( _dot3(v1, _sub3(v2,_cross3(v1, _sub3(v3,v1)))) < 0 ) {
376  std::swap(indices[i], indices[i+1]);
377  }
378  }
379 
380  vertices.resize(8);
381  std::copy(&v[0],&v[8],vertices.begin());
382  indices.resize(nindices);
383  std::copy(&startindices[0],&startindices[nindices],indices.begin());
384  break;
385  }
386  case GeomCylinder: {
387  // cylinder is on y axis
388  double rad = vGeomData.x, len = vGeomData.y*0.5f;
389 
390  int numverts = (int)(fTessellation*24.0f) + 3;
391  double dtheta = 2 * M_PI / (double)numverts;
392  vertices.push_back(Vector3(0,0,len));
393  vertices.push_back(Vector3(0,0,-len));
394  vertices.push_back(Vector3(rad,0,len));
395  vertices.push_back(Vector3(rad,0,-len));
396 
397  for(int i = 0; i < numverts+1; ++i) {
398  double s = rad * std::sin(dtheta * (double)i);
399  double c = rad * std::cos(dtheta * (double)i);
400 
401  int off = (int)vertices.size();
402  vertices.push_back(Vector3(c, s, len));
403  vertices.push_back(Vector3(c, s, -len));
404 
405  indices.push_back(0); indices.push_back(off); indices.push_back(off-2);
406  indices.push_back(1); indices.push_back(off-1); indices.push_back(off+1);
407  indices.push_back(off-2); indices.push_back(off); indices.push_back(off-1);
408  indices.push_back(off); indices.push_back(off-1); indices.push_back(off+1);
409  }
410  break;
411  }
412  default:
413  BOOST_ASSERT(0);
414  }
415  return true;
416  }
417  };
418 
419 public:
420  ColladaModelReader(urdf::ModelInterfaceSharedPtr model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
421  daeErrorHandler::setErrorHandler(this);
422  _resourcedir = ".";
423  }
425  _vuserdata.clear();
426  _collada.reset();
427  DAE::cleanup();
428  }
429 
430  bool InitFromFile(const std::string& filename) {
431  ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE%filename));
432  _collada.reset(new DAE);
433  _dom = (domCOLLADA*)_collada->open(filename);
434  if (!_dom) {
435  return false;
436  }
437  _filename=filename;
438 
439  size_t maxchildren = _countChildren(_dom);
440  _vuserdata.resize(0);
441  _vuserdata.reserve(maxchildren);
442 
443  double dScale = 1.0;
444  _processUserData(_dom, dScale);
445  ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
446  return _Extract();
447  }
448 
449  bool InitFromData(const std::string& pdata) {
450  ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE));
451  _collada.reset(new DAE);
452  _dom = (domCOLLADA*)_collada->openFromMemory(".",pdata.c_str());
453  if (!_dom) {
454  return false;
455  }
456 
457  size_t maxchildren = _countChildren(_dom);
458  _vuserdata.resize(0);
459  _vuserdata.reserve(maxchildren);
460 
461  double dScale = 1.0;
462  _processUserData(_dom, dScale);
463  ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
464  return _Extract();
465  }
466 
467 protected:
468 
470  bool _Extract()
471  {
472  _model->clear();
473  std::list< std::pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
474  domCOLLADA::domSceneRef allscene = _dom->getScene();
475  if( !allscene ) {
476  return false;
477  }
478 
479  // parse each instance kinematics scene, prioritize real robots
480  for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) {
481  domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene];
482  domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
483  if (!kscene) {
484  continue;
485  }
487  _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings);
488  _ExtractPhysicsBindings(allscene,*bindings);
489  for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) {
490  if( _ExtractArticulatedSystem(kscene->getInstance_articulated_system_array()[ias], *bindings) ) {
491  _PostProcess();
492  return true;
493  }
494  }
495  for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) {
496  listPossibleBodies.push_back(std::make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings));
497  }
498  }
499 
500  FOREACH(it, listPossibleBodies) {
501  if( _ExtractKinematicsModel(it->first, *it->second) ) {
502  _PostProcess();
503  return true;
504  }
505  }
506 
507  return false;
508  }
509 
511  {
512  std::map<std::string, std::string> parent_link_tree;
513  // building tree: name mapping
514  try
515  {
516  _model->initTree(parent_link_tree);
517  }
518  catch(ParseError &e)
519  {
520  ROS_ERROR("Failed to build tree: %s", e.what());
521  }
522 
523  // find the root link
524  try
525  {
526  _model->initRoot(parent_link_tree);
527  }
528  catch(ParseError &e)
529  {
530  ROS_ERROR("Failed to find root link: %s", e.what());
531  }
532  }
533 
535  bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings)
536  {
537  if( !ias ) {
538  return false;
539  }
540  ROS_DEBUG_STREAM(str(boost::format("instance articulated system sid %s\n")%ias->getSid()));
541  domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system> (ias->getUrl().getElement().cast());
542  if( !articulated_system ) {
543  return false;
544  }
545 
546  boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ias->getExtra_array());
547  if( !pinterface_type ) {
548  pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array());
549  }
550  if( !!pinterface_type ) {
551  ROS_DEBUG_STREAM(str(boost::format("robot type: %s")%(*pinterface_type)));
552  }
553 
554  // set the name
555  if( _model->name_.size() == 0 && !!ias->getName() ) {
556  _model->name_ = ias->getName();
557  }
558  if( _model->name_.size() == 0 && !!ias->getSid()) {
559  _model->name_ = ias->getSid();
560  }
561  if( _model->name_.size() == 0 && !!articulated_system->getName() ) {
562  _model->name_ = articulated_system->getName();
563  }
564  if( _model->name_.size() == 0 && !!articulated_system->getId()) {
565  _model->name_ = articulated_system->getId();
566  }
567 
568  if( !!articulated_system->getMotion() ) {
569  domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system();
570  if( !!articulated_system->getMotion()->getTechnique_common() ) {
571  for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
572  domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i];
573  // this should point to a kinematics axis_info
574  domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast<domKinematics_axis_info>(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt);
575  if( !!kinematics_axis_info ) {
576  // find the parent kinematics and go through all its instance kinematics models
577  daeElement* pparent = kinematics_axis_info->getParent();
578  while(!!pparent && pparent->typeID() != domKinematics::ID()) {
579  pparent = pparent->getParent();
580  }
581  BOOST_ASSERT(!!pparent);
582  bindings.AddAxisInfo(daeSafeCast<domKinematics>(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info);
583  }
584  else {
585  ROS_WARN_STREAM(str(boost::format("failed to find kinematics axis %s\n")%motion_axis_info->getAxis()));
586  }
587  }
588  }
589  if( !_ExtractArticulatedSystem(ias_new,bindings) ) {
590  return false;
591  }
592  }
593  else {
594  if( !articulated_system->getKinematics() ) {
595  ROS_WARN_STREAM(str(boost::format("collada <kinematics> tag empty? instance_articulated_system=%s\n")%ias->getID()));
596  return true;
597  }
598 
599  if( !!articulated_system->getKinematics()->getTechnique_common() ) {
600  for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
601  bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL);
602  }
603  }
604 
605  for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) {
606  _ExtractKinematicsModel(articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings);
607  }
608  }
609 
610  _ExtractRobotAttachedActuators(articulated_system);
611  _ExtractRobotManipulators(articulated_system);
612  _ExtractRobotAttachedSensors(articulated_system);
613  return true;
614  }
615 
616  bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings)
617  {
618  if( !ikm ) {
619  return false;
620  }
621  ROS_DEBUG_STREAM(str(boost::format("instance kinematics model sid %s\n")%ikm->getSid()));
622  domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model> (ikm->getUrl().getElement().cast());
623  if (!kmodel) {
624  ROS_WARN_STREAM(str(boost::format("%s does not reference valid kinematics\n")%ikm->getSid()));
625  return false;
626  }
627  domPhysics_modelRef pmodel;
628  boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ikm->getExtra_array());
629  if( !pinterface_type ) {
630  pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array());
631  }
632  if( !!pinterface_type ) {
633  ROS_DEBUG_STREAM(str(boost::format("kinbody interface type: %s")%(*pinterface_type)));
634  }
635 
636  // find matching visual node
637  domNodeRef pvisualnode;
638  FOREACH(it, bindings.listKinematicsVisualBindings) {
639  if( it->second == ikm ) {
640  pvisualnode = it->first;
641  break;
642  }
643  }
644  if( !pvisualnode ) {
645  ROS_WARN_STREAM(str(boost::format("failed to find visual node for instance kinematics model %s\n")%ikm->getSid()));
646  return false;
647  }
648 
649  if( _model->name_.size() == 0 && !!ikm->getName() ) {
650  _model->name_ = ikm->getName();
651  }
652  if( _model->name_.size() == 0 && !!ikm->getID() ) {
653  _model->name_ = ikm->getID();
654  }
655 
656  if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings)) {
657  ROS_WARN_STREAM(str(boost::format("failed to load kinbody from kinematics model %s\n")%kmodel->getID()));
658  return false;
659  }
660  return true;
661  }
662 
664  bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings& bindings)
665  {
666  std::vector<domJointRef> vdomjoints;
667  ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model->name_));
668  if( !!pnode ) {
669  ROS_DEBUG_STREAM(str(boost::format("node name: %s\n")%pnode->getId()));
670  }
671 
672  // Process joint of the kinbody
673  domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
674 
675  // Store joints
676  for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
677  vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
678  }
679 
680  // Store instances of joints
681  for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) {
682  domJointRef pelt = daeSafeCast<domJoint> (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement());
683  if (!pelt) {
684  ROS_WARN_STREAM("failed to get joint from instance\n");
685  }
686  else {
687  vdomjoints.push_back(pelt);
688  }
689  }
690 
691  ROS_DEBUG_STREAM(str(boost::format("Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount()));
692  for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) {
693  domLinkRef pdomlink = ktec->getLink_array()[ilink];
694  _RootOrigin = _poseFromMatrix(_ExtractFullTransform(pdomlink));
695  ROS_DEBUG("RootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
696  pdomlink->getName(),
697  _RootOrigin.position.x, _RootOrigin.position.y, _RootOrigin.position.z,
698  _RootOrigin.rotation.x, _RootOrigin.rotation.y, _RootOrigin.rotation.z, _RootOrigin.rotation.w);
699 
700  domNodeRef pvisualnode;
701  FOREACH(it, bindings.listKinematicsVisualBindings) {
702  if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) {
703  pvisualnode = it->first;
704  break;
705  }
706  }
707  if (!!pvisualnode) {
708  _VisualRootOrigin = _poseFromMatrix(_getNodeParentTransform(pvisualnode));
709  ROS_DEBUG("VisualRootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
710  pdomlink->getName(),
711  _VisualRootOrigin.position.x, _VisualRootOrigin.position.y, _VisualRootOrigin.position.z,
712  _VisualRootOrigin.rotation.x, _VisualRootOrigin.rotation.y, _VisualRootOrigin.rotation.z, _VisualRootOrigin.rotation.w);
713  }
714  _ExtractLink(pdomlink, ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings);
715  }
716 
717  // TODO: implement mathml
718  for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) {
719  domFormulaRef pf = ktec->getFormula_array()[iform];
720  if (!pf->getTarget()) {
721  ROS_WARN_STREAM("formula target not valid\n");
722  continue;
723  }
724 
725  // find the target joint
726  urdf::JointSharedPtr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
727  if (!pjoint) {
728  continue;
729  }
730 
731  if (!!pf->getTechnique_common()) {
732  daeElementRef peltmath;
733  daeTArray<daeElementRef> children;
734  pf->getTechnique_common()->getChildren(children);
735  for (size_t ichild = 0; ichild < children.getCount(); ++ichild) {
736  daeElementRef pelt = children[ichild];
737  if (_checkMathML(pelt,std::string("math")) ) {
738  peltmath = pelt;
739  }
740  else {
741  ROS_WARN_STREAM(str(boost::format("unsupported formula element: %s\n")%pelt->getElementName()));
742  }
743  }
744  if (!!peltmath) {
745  // full math xml spec not supported, only looking for ax+b pattern:
746  // <apply> <plus/> <apply> <times/> <ci>a</ci> x </apply> <ci>b</ci> </apply>
747  double a = 1, b = 0;
748  daeElementRef psymboljoint;
749  BOOST_ASSERT(peltmath->getChildren().getCount()>0);
750  daeElementRef papplyelt = peltmath->getChildren()[0];
751  BOOST_ASSERT(_checkMathML(papplyelt,"apply"));
752  BOOST_ASSERT(papplyelt->getChildren().getCount()>0);
753  if( _checkMathML(papplyelt->getChildren()[0],"plus") ) {
754  BOOST_ASSERT(papplyelt->getChildren().getCount()==3);
755  daeElementRef pa = papplyelt->getChildren()[1];
756  daeElementRef pb = papplyelt->getChildren()[2];
757  if( !_checkMathML(papplyelt->getChildren()[1],"apply") ) {
758  std::swap(pa,pb);
759  }
760  if( !_checkMathML(pa,"csymbol") ) {
761  BOOST_ASSERT(_checkMathML(pa,"apply"));
762  BOOST_ASSERT(_checkMathML(pa->getChildren()[0],"times"));
763  if( _checkMathML(pa->getChildren()[1],"csymbol") ) {
764  psymboljoint = pa->getChildren()[1];
765  BOOST_ASSERT(_checkMathML(pa->getChildren()[2],"cn"));
766  std::stringstream ss(pa->getChildren()[2]->getCharData());
767  ss >> a;
768  }
769  else {
770  psymboljoint = pa->getChildren()[2];
771  BOOST_ASSERT(_checkMathML(pa->getChildren()[1],"cn"));
772  std::stringstream ss(pa->getChildren()[1]->getCharData());
773  ss >> a;
774  }
775  }
776  else {
777  psymboljoint = pa;
778  }
779  BOOST_ASSERT(_checkMathML(pb,"cn"));
780  {
781  std::stringstream ss(pb->getCharData());
782  ss >> b;
783  }
784  }
785  else if( _checkMathML(papplyelt->getChildren()[0],"minus") ) {
786  BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[1],"csymbol"));
787  a = -1;
788  psymboljoint = papplyelt->getChildren()[1];
789  }
790  else {
791  BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[0],"csymbol"));
792  psymboljoint = papplyelt->getChildren()[0];
793  }
794  BOOST_ASSERT(psymboljoint->hasAttribute("encoding"));
795  BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA"));
796  urdf::JointSharedPtr pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
797  if( !!pbasejoint ) {
798  // set the mimic properties
799  pjoint->mimic.reset(new JointMimic());
800  pjoint->mimic->joint_name = pbasejoint->name;
801  pjoint->mimic->multiplier = a;
802  pjoint->mimic->offset = b;
803  ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b));
804  }
805  }
806  }
807  }
808  return true;
809  }
810 
812  urdf::LinkSharedPtr _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) {
813  const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
814  // Set link name with the name of the COLLADA's Link
815  std::string linkname = _ExtractLinkName(pdomlink);
816  if( linkname.size() == 0 ) {
817  ROS_WARN_STREAM("<link> has no name or id, falling back to <node>!\n");
818  if( !!pdomnode ) {
819  if (!!pdomnode->getName()) {
820  linkname = pdomnode->getName();
821  }
822  if( linkname.size() == 0 && !!pdomnode->getID()) {
823  linkname = pdomnode->getID();
824  }
825  }
826  }
827 
828  urdf::LinkSharedPtr plink;
829  _model->getLink(linkname,plink);
830  if( !plink ) {
831  plink.reset(new Link());
832  plink->name = linkname;
833  plink->visual.reset(new Visual());
834  plink->visual->material_name = "";
835  plink->visual->material.reset(new Material());
836  plink->visual->material->name = "Red";
837  plink->visual->material->color.r = 0.0;
838  plink->visual->material->color.g = 1.0;
839  plink->visual->material->color.b = 0.0;
840  plink->visual->material->color.a = 1.0;
841  plink->inertial.reset();
842  _model->links_.insert(std::make_pair(linkname,plink));
843  }
844 
845  _getUserData(pdomlink)->p = plink;
846  if( !!pdomnode ) {
847  ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName()));
848  }
849 
850  // physics
851  domInstance_rigid_bodyRef irigidbody;
852  domRigid_bodyRef rigidbody;
853  bool bFoundBinding = false;
854  FOREACH(itlinkbinding, bindings.listLinkBindings) {
855  if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) {
856  bFoundBinding = true;
857  irigidbody = itlinkbinding->irigidbody;
858  rigidbody = itlinkbinding->rigidbody;
859  }
860  }
861 
862  if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
863  domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
864  if( !!rigiddata->getMass() ) {
865  if ( !plink->inertial ) {
866  plink->inertial.reset(new Inertial());
867  }
868  plink->inertial->mass = rigiddata->getMass()->getValue();
869  }
870  if( !!rigiddata->getInertia() ) {
871  if ( !plink->inertial ) {
872  plink->inertial.reset(new Inertial());
873  }
874  plink->inertial->ixx = rigiddata->getInertia()->getValue()[0];
875  plink->inertial->iyy = rigiddata->getInertia()->getValue()[1];
876  plink->inertial->izz = rigiddata->getInertia()->getValue()[2];
877  }
878  if( !!rigiddata->getMass_frame() ) {
879  if ( !plink->inertial ) {
880  plink->inertial.reset(new Inertial());
881  }
882  //plink->inertial->origin = _poseMult(_poseInverse(tParentWorldLink), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
883  Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
884  plink->inertial->origin = _poseMult(_poseInverse(_poseMult(_poseInverse(_RootOrigin),
885  _poseMult(tParentWorldLink, tlink))),
886  _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
887  }
888  }
889 
890  std::list<GEOMPROPERTIES> listGeomProperties;
891  if (!pdomlink) {
892  ROS_WARN_STREAM("Extract object NOT kinematics !!!\n");
893  _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,Pose());
894  }
895  else {
896  ROS_DEBUG_STREAM(str(boost::format("Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
897  Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
898  ROS_DEBUG("tlink: %s: %lf %lf %lf %lf %lf %lf %lf",
899  linkname.c_str(),
900  tlink.position.x, tlink.position.y, tlink.position.z,
901  tlink.rotation.x, tlink.rotation.y, tlink.rotation.z, tlink.rotation.w);
902  plink->visual->origin = _poseMult(tParentLink, tlink); // use the kinematics coordinate system for each link
903  // 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);
904  // 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);
905 
906  // Get the geometry
907  _ExtractGeometry(pdomnode, listGeomProperties, listAxisBindings,
908  _poseMult(_poseMult(tParentWorldLink,tlink), plink->visual->origin));
909 
910  ROS_DEBUG_STREAM(str(boost::format("After ExtractGeometry Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
911 
912  // Process all atached links
913  for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) {
914  domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt];
915 
916  // get link kinematics transformation
917  Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull));
918 
919  // process attached links
920  daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt;
921  domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
922 
923  if (!pdomjoint) {
924  domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
925  if (!!pdomijoint) {
926  pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
927  }
928  }
929 
930  if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) {
931  ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint()));
932  return urdf::LinkSharedPtr();
933  }
934 
935  // get direct child link
936  if (!pattfull->getLink()) {
937  ROS_WARN_STREAM(str(boost::format("joint %s needs to be attached to a valid link\n")%pdomjoint->getID()));
938  continue;
939  }
940 
941  // find the correct joint in the bindings
942  daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
943  domNodeRef pchildnode;
944 
945  // see if joint has a binding to a visual node
946  FOREACHC(itaxisbinding,listAxisBindings) {
947  for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
948  // If the binding for the joint axis is found, retrieve the info
949  if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
950  pchildnode = itaxisbinding->visualnode;
951  break;
952  }
953  }
954  if( !!pchildnode ) {
955  break;
956  }
957  }
958  if (!pchildnode) {
959  ROS_DEBUG_STREAM(str(boost::format("joint %s has no visual binding\n")%pdomjoint->getID()));
960  }
961 
962  // create the joints before creating the child links
963  std::vector<urdf::JointSharedPtr > vjoints(vdomaxes.getCount());
964  for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
965  bool joint_active = true; // if not active, put into the passive list
966  FOREACHC(itaxisbinding,listAxisBindings) {
967  if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
968  if( !!itaxisbinding->kinematics_axis_info ) {
969  if( !!itaxisbinding->kinematics_axis_info->getActive() ) {
970  joint_active = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info);
971  }
972  }
973  break;
974  }
975  }
976 
977  urdf::JointSharedPtr pjoint(new Joint());
978  pjoint->limits.reset(new JointLimits());
979  pjoint->limits->velocity = 0.0;
980  pjoint->limits->effort = 0.0;
981  pjoint->parent_link_name = plink->name;
982 
983  if( !!pdomjoint->getName() ) {
984  pjoint->name = pdomjoint->getName();
985  }
986  else {
987  pjoint->name = str(boost::format("dummy%d")%_model->joints_.size());
988  }
989 
990  if( !joint_active ) {
991  ROS_INFO_STREAM(str(boost::format("joint %s is passive, but adding to hierarchy\n")%pjoint->name));
992  }
993 
994  domAxis_constraintRef pdomaxis = vdomaxes[ic];
995  if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) {
996  pjoint->type = Joint::REVOLUTE;
997  }
998  else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) {
999  pjoint->type = Joint::PRISMATIC;
1000  }
1001  else {
1002  ROS_WARN_STREAM(str(boost::format("unsupported joint type: %s\n")%pdomaxis->getElementName()));
1003  }
1004 
1005  _getUserData(pdomjoint)->p = pjoint;
1006 #if URDFDOM_HEADERS_MAJOR_VERSION < 1
1007  _getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size()));
1008 #else
1009  _getUserData(pdomaxis)->p = std::shared_ptr<int>(new int(_model->joints_.size()));
1010 #endif
1011  _model->joints_[pjoint->name] = pjoint;
1012  vjoints[ic] = pjoint;
1013  }
1014 
1015  urdf::LinkSharedPtr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
1016 
1017  if (!pchildlink) {
1018  ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name));
1019  continue;
1020  }
1021 
1022  int numjoints = 0;
1023  for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
1024  domKinematics_axis_infoRef kinematics_axis_info;
1025  domMotion_axis_infoRef motion_axis_info;
1026  FOREACHC(itaxisbinding,listAxisBindings) {
1027  bool bfound = false;
1028  if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
1029  kinematics_axis_info = itaxisbinding->kinematics_axis_info;
1030  motion_axis_info = itaxisbinding->motion_axis_info;
1031  bfound = true;
1032  break;
1033  }
1034  }
1035  domAxis_constraintRef pdomaxis = vdomaxes[ic];
1036  if (!pchildlink) {
1037  // create dummy child link
1038  // multiple axes can be easily done with "empty links"
1039  ROS_WARN_STREAM(str(boost::format("creating dummy link %s, num joints %d\n")%plink->name%numjoints));
1040 
1041  std::stringstream ss;
1042  ss << plink->name;
1043  ss <<"_dummy" << numjoints;
1044  pchildlink.reset(new Link());
1045  pchildlink->name = ss.str();
1046  _model->links_.insert(std::make_pair(pchildlink->name,pchildlink));
1047  }
1048 
1049  ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic));
1050  urdf::JointSharedPtr pjoint = vjoints[ic];
1051  pjoint->child_link_name = pchildlink->name;
1052 
1053 #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \
1054  apose.position.x, apose.position.y, apose.position.z, \
1055  apose.rotation.x, apose.rotation.y, apose.rotation.z, apose.rotation.w);
1056  {
1057  PRINT_POSE("tatt", tatt);
1058  PRINT_POSE("trans_joint_to_child", tatt);
1059  Pose trans_joint_to_child = _poseFromMatrix(_ExtractFullTransform(pattfull->getLink()));
1060 
1061  pjoint->parent_to_joint_origin_transform = _poseMult(tatt, trans_joint_to_child);
1062  // Axes and Anchor assignment.
1063  Vector3 ax(pdomaxis->getAxis()->getValue()[0],
1064  pdomaxis->getAxis()->getValue()[1],
1065  pdomaxis->getAxis()->getValue()[2]);
1066  Pose pinv = _poseInverse(trans_joint_to_child);
1067  // rotate axis
1068  ax = pinv.rotation * ax;
1069 
1070  pjoint->axis.x = ax.x;
1071  pjoint->axis.y = ax.y;
1072  pjoint->axis.z = ax.z;
1073 
1074  ROS_DEBUG("joint %s axis: %f %f %f -> %f %f %f\n", pjoint->name.c_str(),
1075  pdomaxis->getAxis()->getValue()[0],
1076  pdomaxis->getAxis()->getValue()[1],
1077  pdomaxis->getAxis()->getValue()[2],
1078  pjoint->axis.x, pjoint->axis.y, pjoint->axis.z);
1079  }
1080 
1081  if (!motion_axis_info) {
1082  ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name));
1083  }
1084 
1085  // Sets the Speed and the Acceleration of the joint
1086  if (!!motion_axis_info) {
1087  if (!!motion_axis_info->getSpeed()) {
1088  pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
1089  ROS_DEBUG("... Joint Speed: %f...\n",pjoint->limits->velocity);
1090  }
1091  if (!!motion_axis_info->getAcceleration()) {
1092  pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info);
1093  ROS_DEBUG("... Joint effort: %f...\n",pjoint->limits->effort);
1094  }
1095  }
1096 
1097  bool joint_locked = false; // if locked, joint angle is static
1098  bool kinematics_limits = false;
1099 
1100  if (!!kinematics_axis_info) {
1101  if (!!kinematics_axis_info->getLocked()) {
1102  joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
1103  }
1104 
1105  if (joint_locked) { // If joint is locked set limits to the static value.
1106  if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
1107  ROS_WARN_STREAM("lock joint!!\n");
1108  pjoint->limits->lower = 0;
1109  pjoint->limits->upper = 0;
1110  }
1111  }
1112  else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits
1113  kinematics_limits = true;
1114  double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(kinematics_axis_info);
1115  if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
1116  pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
1117  pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
1118  if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
1119  pjoint->type = Joint::FIXED;
1120  }
1121  }
1122  }
1123  }
1124 
1125  // Search limits in the joints section
1126  if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
1127  // If there are NO LIMITS
1128  if( !pdomaxis->getLimits() ) {
1129  ROS_DEBUG_STREAM(str(boost::format("There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits));
1130  if( pjoint->type == Joint::REVOLUTE ) {
1131  pjoint->type = Joint::CONTINUOUS; // continuous means revolute?
1132  pjoint->limits->lower = -M_PI;
1133  pjoint->limits->upper = M_PI;
1134  }
1135  else {
1136  pjoint->limits->lower = -100000;
1137  pjoint->limits->upper = 100000;
1138  }
1139  }
1140  else {
1141  ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name));
1142  double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(pdomaxis);
1143  pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale;
1144  pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale;
1145  if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
1146  pjoint->type = Joint::FIXED;
1147  }
1148  }
1149  }
1150 
1151  if (pjoint->limits->velocity == 0.0) {
1152  pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
1153  }
1154  pchildlink.reset();
1155  ++numjoints;
1156  }
1157  }
1158  }
1159 
1160  if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
1161  ROS_WARN("urdf collada reader does not support attachment_start\n");
1162  }
1163  if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
1164  ROS_WARN("urdf collada reader does not support attachment_end\n");
1165  }
1166 
1167  plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
1168  // visual_groups deprecated
1169  //boost::shared_ptr<std::vector<urdf::VisualSharedPtr > > viss;
1170  //viss.reset(new std::vector<urdf::VisualSharedPtr >);
1171  //viss->push_back(plink->visual);
1172  //plink->visual_groups.insert(std::make_pair("default", viss));
1173 
1174  if( !plink->visual->geometry ) {
1175  plink->visual.reset();
1176  plink->collision.reset();
1177  } else {
1178  // collision
1179  plink->collision.reset(new Collision());
1180  plink->collision->geometry = plink->visual->geometry;
1181  plink->collision->origin = plink->visual->origin;
1182  }
1183 
1184  // collision_groups deprecated
1185  //boost::shared_ptr<std::vector<urdf::CollisionSharedPtr > > cols;
1186  //cols.reset(new std::vector<urdf::CollisionSharedPtr >);
1187  //cols->push_back(plink->collision);
1188  //plink->collision_groups.insert(std::make_pair("default", cols));
1189 
1190  return plink;
1191  }
1192 
1193  urdf::GeometrySharedPtr _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
1194  {
1195  std::vector<std::vector<Vector3> > vertices;
1196  std::vector<std::vector<int> > indices;
1197  std::vector<Color> ambients;
1198  std::vector<Color> diffuses;
1199  unsigned int index, vert_counter;
1200  vertices.resize(listGeomProperties.size());
1201  indices.resize(listGeomProperties.size());
1202  ambients.resize(listGeomProperties.size());
1203  diffuses.resize(listGeomProperties.size());
1204  index = 0;
1205  vert_counter = 0;
1206  FOREACHC(it, listGeomProperties) {
1207  vertices[index].resize(it->vertices.size());
1208  for(size_t i = 0; i < it->vertices.size(); ++i) {
1209  vertices[index][i] = _poseMult(it->_t, it->vertices[i]);
1210  vert_counter++;
1211  }
1212  indices[index].resize(it->indices.size());
1213  for(size_t i = 0; i < it->indices.size(); ++i) {
1214  indices[index][i] = it->indices[i];
1215  }
1216  ambients[index] = it->ambientColor;
1217  diffuses[index] = it->diffuseColor;
1218 // workaround for mesh_loader.cpp:421
1219 // Most of are DAE files don't have ambient color defined
1220  ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
1221  if ( ambients[index].r == 0.0 ) {
1222  ambients[index].r = 0.0001;
1223  }
1224  if ( ambients[index].g == 0.0 ) {
1225  ambients[index].g = 0.0001;
1226  }
1227  if ( ambients[index].b == 0.0 ) {
1228  ambients[index].b = 0.0001;
1229  }
1230  index++;
1231  }
1232 
1233  if (vert_counter == 0) {
1234  urdf::MeshSharedPtr ret;
1235  ret.reset();
1236  return ret;
1237  }
1238 
1239  urdf::MeshSharedPtr geometry(new Mesh());
1240  geometry->type = Geometry::MESH;
1241  geometry->scale.x = 1;
1242  geometry->scale.y = 1;
1243  geometry->scale.z = 1;
1244 
1245  // have to save the geometry into individual collada 1.4 files since URDF does not allow triangle meshes to be specified
1246  std::stringstream daedata;
1247  daedata << str(boost::format("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\
1248 <COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n\
1249  <asset>\n\
1250  <contributor>\n\
1251  <author>Rosen Diankov</author>\n\
1252  <comments>\n\
1253  robot_model/urdf temporary collada geometry\n\
1254  </comments>\n\
1255  </contributor>\n\
1256  <unit name=\"meter\" meter=\"1.0\"/>\n\
1257  <up_axis>Y_UP</up_axis>\n\
1258  </asset>\n\
1259  <library_materials>\n"));
1260  for(unsigned int i=0; i < index; i++) {
1261  daedata << str(boost::format("\
1262  <material id=\"blinn2%d\" name=\"blinn2%d\">\n\
1263  <instance_effect url=\"#blinn2-fx%d\"/>\n\
1264  </material>\n")%i%i%i);
1265  }
1266  daedata << "\
1267  </library_materials>\n\
1268  <library_effects>\n";
1269  for(unsigned int i=0; i < index; i++) {
1270  daedata << str(boost::format("\
1271  <effect id=\"blinn2-fx%d\">\n\
1272  <profile_COMMON>\n\
1273  <technique sid=\"common\">\n\
1274  <phong>\n\
1275  <ambient>\n\
1276  <color>%f %f %f %f</color>\n\
1277  </ambient>\n\
1278  <diffuse>\n\
1279  <color>%f %f %f %f</color>\n\
1280  </diffuse>\n\
1281  <specular>\n\
1282  <color>0 0 0 1</color>\n\
1283  </specular>\n\
1284  </phong>\n\
1285  </technique>\n\
1286  </profile_COMMON>\n\
1287  </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);
1288  }
1289  daedata << str(boost::format("\
1290  </library_effects>\n\
1291  <library_geometries>\n"));
1292  // fill with vertices
1293  for(unsigned int i=0; i < index; i++) {
1294  daedata << str(boost::format("\
1295  <geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\
1296  <mesh>\n\
1297  <source id=\"geo0.positions\">\n\
1298  <float_array id=\"geo0.positions-array\" count=\"%d\">")%i%i%(vertices[i].size()*3));
1299  FOREACH(it,vertices[i]) {
1300  daedata << it->x << " " << it->y << " " << it->z << " ";
1301  }
1302  daedata << str(boost::format("\n\
1303  </float_array>\n\
1304  <technique_common>\n\
1305  <accessor count=\"%d\" source=\"#geo0.positions-array\" stride=\"3\">\n\
1306  <param name=\"X\" type=\"float\"/>\n\
1307  <param name=\"Y\" type=\"float\"/>\n\
1308  <param name=\"Z\" type=\"float\"/>\n\
1309  </accessor>\n\
1310  </technique_common>\n\
1311  </source>\n\
1312  <vertices id=\"geo0.vertices\">\n\
1313  <input semantic=\"POSITION\" source=\"#geo0.positions\"/>\n\
1314  </vertices>\n\
1315  <triangles count=\"%d\" material=\"lambert2SG\">\n\
1316  <input offset=\"0\" semantic=\"VERTEX\" source=\"#geo0.vertices\"/>\n\
1317  <p>")%vertices[i].size()%(indices[i].size()/3));
1318  // fill with indices
1319  FOREACH(it,indices[i]) {
1320  daedata << *it << " ";
1321  }
1322  daedata << str(boost::format("</p>\n\
1323  </triangles>\n\
1324  </mesh>\n\
1325  </geometry>\n"));
1326  }
1327  daedata << str(boost::format("\
1328  </library_geometries>\n\
1329  <library_visual_scenes>\n\
1330  <visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\
1331  <node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name);
1332  for(unsigned int i=0; i < index; i++) {
1333  daedata << str(boost::format("\
1334  <instance_geometry url=\"#base2_M1KShape%i\">\n\
1335  <bind_material>\n\
1336  <technique_common>\n\
1337  <instance_material symbol=\"lambert2SG\" target=\"#blinn2%i\"/>\n\
1338  </technique_common>\n\
1339  </bind_material>\n\
1340  </instance_geometry>\n")%i%i);
1341  }
1342  daedata << str(boost::format("\
1343  </node>\n\
1344  </visual_scene>\n\
1345  </library_visual_scenes>\n\
1346  <scene>\n\
1347  <instance_visual_scene url=\"#VisualSceneNode\"/>\n\
1348  </scene>\n\
1349 </COLLADA>"));
1350 
1351 #ifdef HAVE_MKSTEMPS
1352  geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name);
1353  int fd = mkstemps(&geometry->filename[0],4);
1354 #else
1355  int fd = -1;
1356  for(int iter = 0; iter < 1000; ++iter) {
1357  geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_%d.dae")%name%rand());
1358  if( !!std::ifstream(geometry->filename.c_str())) {
1359  fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL);
1360  if( fd != -1 ) {
1361  break;
1362  }
1363  }
1364  }
1365  if( fd == -1 ) {
1366  ROS_ERROR("failed to open geometry dae file %s",geometry->filename.c_str());
1367  return geometry;
1368  }
1369 #endif
1370  //ROS_INFO("temp file: %s",geometry->filename.c_str());
1371  std::string daedatastr = daedata.str();
1372  if( (size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) {
1373  ROS_ERROR("failed to write to geometry dae file %s",geometry->filename.c_str());
1374  }
1375  close(fd);
1376  _listTempFilenames.push_back(boost::shared_ptr<UnlinkFilename>(new UnlinkFilename(geometry->filename)));
1377  geometry->filename = std::string("file://") + geometry->filename;
1378  return geometry;
1379  }
1380 
1384  void _ExtractGeometry(const domNodeRef pdomnode,std::list<GEOMPROPERTIES>& listGeomProperties, const std::list<JointAxisBinding>& listAxisBindings, const Pose& tlink)
1385  {
1386  if( !pdomnode ) {
1387  return;
1388  }
1389 
1390  ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName()));
1391 
1392  // For all child nodes of pdomnode
1393  for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
1394  // check if contains a joint
1395  bool contains=false;
1396  FOREACHC(it,listAxisBindings) {
1397  // don't check ID's check if the reference is the same!
1398  if ( (pdomnode->getNode_array()[i]) == (it->visualnode)) {
1399  contains=true;
1400  break;
1401  }
1402  }
1403  if (contains) {
1404  continue;
1405  }
1406 
1407  _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink);
1408  // Plink stayes the same for all children
1409  // replace pdomnode by child = pdomnode->getNode_array()[i]
1410  // hope for the best!
1411  // put everything in a subroutine in order to process pdomnode too!
1412  }
1413 
1414  unsigned int nGeomBefore = listGeomProperties.size(); // #of Meshes already associated to this link
1415 
1416  // get the geometry
1417  for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
1418  domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
1419  domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
1420  if (!domgeom) {
1421  continue;
1422  }
1423 
1424  // Gets materials
1425  std::map<std::string, domMaterialRef> mapmaterials;
1426  if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
1427  const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
1428  for (size_t imat = 0; imat < matarray.getCount(); ++imat) {
1429  domMaterialRef pmat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
1430  if (!!pmat) {
1431  mapmaterials[matarray[imat]->getSymbol()] = pmat;
1432  }
1433  }
1434  }
1435 
1436  // Gets the geometry
1437  _ExtractGeometry(domgeom, mapmaterials, listGeomProperties);
1438  }
1439 
1440  std::list<GEOMPROPERTIES>::iterator itgeom= listGeomProperties.begin();
1441  for (unsigned int i=0; i< nGeomBefore; i++) {
1442  itgeom++; // change only the transformations of the newly found geometries.
1443  }
1444 
1445  boost::array<double,12> tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)),
1446  _poseMult(_poseMult(_matrixFromPose(_poseInverse(_VisualRootOrigin)),
1447  _getNodeParentTransform(pdomnode)),
1448  _ExtractFullTransform(pdomnode)));
1449  Pose tnodegeom;
1450  Vector3 vscale(1,1,1);
1451  _decompose(tmnodegeom, tnodegeom, vscale);
1452 
1453  ROS_DEBUG_STREAM("tnodegeom: " << pdomnode->getName()
1454  << tnodegeom.position.x << " " << tnodegeom.position.y << " " << tnodegeom.position.z << " / "
1455  << tnodegeom.rotation.x << " " << tnodegeom.rotation.y << " " << tnodegeom.rotation.z << " "
1456  << tnodegeom.rotation.w);
1457 
1458  // std::stringstream ss; ss << "geom: ";
1459  // for(int i = 0; i < 4; ++i) {
1460  // ss << tmnodegeom[4*0+i] << " " << tmnodegeom[4*1+i] << " " << tmnodegeom[4*2+i] << " ";
1461  // }
1462  // ROS_INFO(ss.str().c_str());
1463 
1464  // Switch between different type of geometry PRIMITIVES
1465  for (; itgeom != listGeomProperties.end(); itgeom++) {
1466  itgeom->_t = tnodegeom;
1467  switch (itgeom->type) {
1468  case GeomBox:
1469  itgeom->vGeomData.x *= vscale.x;
1470  itgeom->vGeomData.y *= vscale.y;
1471  itgeom->vGeomData.z *= vscale.z;
1472  break;
1473  case GeomSphere: {
1474  itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y));
1475  break;
1476  }
1477  case GeomCylinder:
1478  itgeom->vGeomData.x *= std::max(vscale.x, vscale.y);
1479  itgeom->vGeomData.y *= vscale.z;
1480  break;
1481  case GeomTrimesh:
1482  for(size_t i = 0; i < itgeom->vertices.size(); ++i ) {
1483  itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]);
1484  }
1485  itgeom->_t = Pose(); // reset back to identity
1486  break;
1487  default:
1488  ROS_WARN_STREAM(str(boost::format("unknown geometry type: %d\n")%itgeom->type));
1489  }
1490  }
1491  }
1492 
1496  void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES& geom)
1497  {
1498  if( !!pmat && !!pmat->getInstance_effect() ) {
1499  domEffectRef peffect = daeSafeCast<domEffect>(pmat->getInstance_effect()->getUrl().getElement().cast());
1500  if( !!peffect ) {
1501  domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
1502  if( !!pphong ) {
1503  if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
1504  domFx_color c = pphong->getAmbient()->getColor()->getValue();
1505  geom.ambientColor.r = c[0];
1506  geom.ambientColor.g = c[1];
1507  geom.ambientColor.b = c[2];
1508  geom.ambientColor.a = c[3];
1509  }
1510  if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
1511  domFx_color c = pphong->getDiffuse()->getColor()->getValue();
1512  geom.diffuseColor.r = c[0];
1513  geom.diffuseColor.g = c[1];
1514  geom.diffuseColor.b = c[2];
1515  geom.diffuseColor.a = c[3];
1516  }
1517  }
1518  }
1519  }
1520  }
1521 
1527  bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1528  {
1529  if( !triRef ) {
1530  return false;
1531  }
1532  listGeomProperties.push_back(GEOMPROPERTIES());
1533  GEOMPROPERTIES& geom = listGeomProperties.back();
1534  geom.type = GeomTrimesh;
1535 
1536  // resolve the material and assign correct colors to the geometry
1537  if( !!triRef->getMaterial() ) {
1538  std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1539  if( itmat != mapmaterials.end() ) {
1540  _FillGeometryColor(itmat->second,geom);
1541  }
1542  }
1543 
1544  size_t triangleIndexStride = 0, vertexoffset = -1;
1545  domInput_local_offsetRef indexOffsetRef;
1546 
1547  for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1548  size_t offset = triRef->getInput_array()[w]->getOffset();
1549  daeString str = triRef->getInput_array()[w]->getSemantic();
1550  if (!strcmp(str,"VERTEX")) {
1551  indexOffsetRef = triRef->getInput_array()[w];
1552  vertexoffset = offset;
1553  }
1554  if (offset> triangleIndexStride) {
1555  triangleIndexStride = offset;
1556  }
1557  }
1558  triangleIndexStride++;
1559 
1560  const domList_of_uints& indexArray =triRef->getP()->getValue();
1561  geom.indices.reserve(triRef->getCount()*3);
1562  geom.vertices.reserve(triRef->getCount()*3);
1563  for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1564  domInput_localRef localRef = vertsRef->getInput_array()[i];
1565  daeString str = localRef->getSemantic();
1566  if ( strcmp(str,"POSITION") == 0 ) {
1567  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1568  if( !node ) {
1569  continue;
1570  }
1571  double fUnitScale = _GetUnitScale(node);
1572  const domFloat_arrayRef flArray = node->getFloat_array();
1573  if (!!flArray) {
1574  const domList_of_floats& listFloats = flArray->getValue();
1575  int k=vertexoffset;
1576  int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
1577  for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
1578  if(k+2*triangleIndexStride < indexArray.getCount() ) {
1579  for (int j=0; j<3; j++) {
1580  int index0 = indexArray.get(k)*vertexStride;
1581  domFloat fl0 = listFloats.get(index0);
1582  domFloat fl1 = listFloats.get(index0+1);
1583  domFloat fl2 = listFloats.get(index0+2);
1584  k+=triangleIndexStride;
1585  geom.indices.push_back(geom.vertices.size());
1586  geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1587  }
1588  }
1589  }
1590  }
1591  else {
1592  ROS_WARN_STREAM("float array not defined!\n");
1593  }
1594  break;
1595  }
1596  }
1597  if( geom.indices.size() != 3*triRef->getCount() ) {
1598  ROS_WARN_STREAM("triangles declares wrong count!\n");
1599  }
1600  geom.InitCollisionMesh();
1601  return true;
1602  }
1603 
1608  bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1609  {
1610  if( !triRef ) {
1611  return false;
1612  }
1613  listGeomProperties.push_back(GEOMPROPERTIES());
1614  GEOMPROPERTIES& geom = listGeomProperties.back();
1615  geom.type = GeomTrimesh;
1616 
1617  // resolve the material and assign correct colors to the geometry
1618  if( !!triRef->getMaterial() ) {
1619  std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1620  if( itmat != mapmaterials.end() ) {
1621  _FillGeometryColor(itmat->second,geom);
1622  }
1623  }
1624 
1625  size_t triangleIndexStride = 0, vertexoffset = -1;
1626  domInput_local_offsetRef indexOffsetRef;
1627 
1628  for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1629  size_t offset = triRef->getInput_array()[w]->getOffset();
1630  daeString str = triRef->getInput_array()[w]->getSemantic();
1631  if (!strcmp(str,"VERTEX")) {
1632  indexOffsetRef = triRef->getInput_array()[w];
1633  vertexoffset = offset;
1634  }
1635  if (offset> triangleIndexStride) {
1636  triangleIndexStride = offset;
1637  }
1638  }
1639  triangleIndexStride++;
1640  size_t primitivecount = triRef->getCount();
1641  if( primitivecount > triRef->getP_array().getCount() ) {
1642  ROS_WARN_STREAM("trifans has incorrect count\n");
1643  primitivecount = triRef->getP_array().getCount();
1644  }
1645  for(size_t ip = 0; ip < primitivecount; ++ip) {
1646  domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
1647  for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1648  domInput_localRef localRef = vertsRef->getInput_array()[i];
1649  daeString str = localRef->getSemantic();
1650  if ( strcmp(str,"POSITION") == 0 ) {
1651  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1652  if( !node ) {
1653  continue;
1654  }
1655  double fUnitScale = _GetUnitScale(node);
1656  const domFloat_arrayRef flArray = node->getFloat_array();
1657  if (!!flArray) {
1658  const domList_of_floats& listFloats = flArray->getValue();
1659  int k=vertexoffset;
1660  int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
1661  size_t usedindices = 3*(indexArray.getCount()-2);
1662  if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
1663  geom.indices.reserve(geom.indices.size()+usedindices);
1664  }
1665  if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
1666  geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
1667  }
1668  size_t startoffset = (int)geom.vertices.size();
1669  while(k < (int)indexArray.getCount() ) {
1670  int index0 = indexArray.get(k)*vertexStride;
1671  domFloat fl0 = listFloats.get(index0);
1672  domFloat fl1 = listFloats.get(index0+1);
1673  domFloat fl2 = listFloats.get(index0+2);
1674  k+=triangleIndexStride;
1675  geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1676  }
1677  for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
1678  geom.indices.push_back(startoffset);
1679  geom.indices.push_back(ivert-1);
1680  geom.indices.push_back(ivert);
1681  }
1682  }
1683  else {
1684  ROS_WARN_STREAM("float array not defined!\n");
1685  }
1686  break;
1687  }
1688  }
1689  }
1690 
1691  geom.InitCollisionMesh();
1692  return false;
1693  }
1694 
1699  bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1700  {
1701  if( !triRef ) {
1702  return false;
1703  }
1704  listGeomProperties.push_back(GEOMPROPERTIES());
1705  GEOMPROPERTIES& geom = listGeomProperties.back();
1706  geom.type = GeomTrimesh;
1707 
1708  // resolve the material and assign correct colors to the geometry
1709  if( !!triRef->getMaterial() ) {
1710  std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1711  if( itmat != mapmaterials.end() ) {
1712  _FillGeometryColor(itmat->second,geom);
1713  }
1714  }
1715  size_t triangleIndexStride = 0, vertexoffset = -1;
1716  domInput_local_offsetRef indexOffsetRef;
1717 
1718  for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1719  size_t offset = triRef->getInput_array()[w]->getOffset();
1720  daeString str = triRef->getInput_array()[w]->getSemantic();
1721  if (!strcmp(str,"VERTEX")) {
1722  indexOffsetRef = triRef->getInput_array()[w];
1723  vertexoffset = offset;
1724  }
1725  if (offset> triangleIndexStride) {
1726  triangleIndexStride = offset;
1727  }
1728  }
1729  triangleIndexStride++;
1730  size_t primitivecount = triRef->getCount();
1731  if( primitivecount > triRef->getP_array().getCount() ) {
1732  ROS_WARN_STREAM("tristrips has incorrect count\n");
1733  primitivecount = triRef->getP_array().getCount();
1734  }
1735  for(size_t ip = 0; ip < primitivecount; ++ip) {
1736  domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
1737  for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1738  domInput_localRef localRef = vertsRef->getInput_array()[i];
1739  daeString str = localRef->getSemantic();
1740  if ( strcmp(str,"POSITION") == 0 ) {
1741  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1742  if( !node ) {
1743  continue;
1744  }
1745  double fUnitScale = _GetUnitScale(node);
1746  const domFloat_arrayRef flArray = node->getFloat_array();
1747  if (!!flArray) {
1748  const domList_of_floats& listFloats = flArray->getValue();
1749  int k=vertexoffset;
1750  int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
1751  size_t usedindices = indexArray.getCount()-2;
1752  if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
1753  geom.indices.reserve(geom.indices.size()+usedindices);
1754  }
1755  if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
1756  geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
1757  }
1758 
1759  size_t startoffset = (int)geom.vertices.size();
1760  while(k < (int)indexArray.getCount() ) {
1761  int index0 = indexArray.get(k)*vertexStride;
1762  domFloat fl0 = listFloats.get(index0);
1763  domFloat fl1 = listFloats.get(index0+1);
1764  domFloat fl2 = listFloats.get(index0+2);
1765  k+=triangleIndexStride;
1766  geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1767  }
1768 
1769  bool bFlip = false;
1770  for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
1771  geom.indices.push_back(ivert-2);
1772  geom.indices.push_back(bFlip ? ivert : ivert-1);
1773  geom.indices.push_back(bFlip ? ivert-1 : ivert);
1774  bFlip = !bFlip;
1775  }
1776  }
1777  else {
1778  ROS_WARN_STREAM("float array not defined!\n");
1779  }
1780  break;
1781  }
1782  }
1783  }
1784 
1785  geom.InitCollisionMesh();
1786  return false;
1787  }
1788 
1793  bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1794  {
1795  if( !triRef ) {
1796  return false;
1797  }
1798  listGeomProperties.push_back(GEOMPROPERTIES());
1799  GEOMPROPERTIES& geom = listGeomProperties.back();
1800  geom.type = GeomTrimesh;
1801 
1802  // resolve the material and assign correct colors to the geometry
1803  if( !!triRef->getMaterial() ) {
1804  std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1805  if( itmat != mapmaterials.end() ) {
1806  _FillGeometryColor(itmat->second,geom);
1807  }
1808  }
1809 
1810  size_t triangleIndexStride = 0,vertexoffset = -1;
1811  domInput_local_offsetRef indexOffsetRef;
1812  for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1813  size_t offset = triRef->getInput_array()[w]->getOffset();
1814  daeString str = triRef->getInput_array()[w]->getSemantic();
1815  if (!strcmp(str,"VERTEX")) {
1816  indexOffsetRef = triRef->getInput_array()[w];
1817  vertexoffset = offset;
1818  }
1819  if (offset> triangleIndexStride) {
1820  triangleIndexStride = offset;
1821  }
1822  }
1823  triangleIndexStride++;
1824  const domList_of_uints& indexArray =triRef->getP()->getValue();
1825  for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1826  domInput_localRef localRef = vertsRef->getInput_array()[i];
1827  daeString str = localRef->getSemantic();
1828  if ( strcmp(str,"POSITION") == 0 ) {
1829  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1830  if( !node ) {
1831  continue;
1832  }
1833  double fUnitScale = _GetUnitScale(node);
1834  const domFloat_arrayRef flArray = node->getFloat_array();
1835  if (!!flArray) {
1836  const domList_of_floats& listFloats = flArray->getValue();
1837  size_t k=vertexoffset;
1838  int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
1839  for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
1840  size_t numverts = triRef->getVcount()->getValue()[ipoly];
1841  if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
1842  size_t startoffset = geom.vertices.size();
1843  for (size_t j=0; j<numverts; j++) {
1844  int index0 = indexArray.get(k)*vertexStride;
1845  domFloat fl0 = listFloats.get(index0);
1846  domFloat fl1 = listFloats.get(index0+1);
1847  domFloat fl2 = listFloats.get(index0+2);
1848  k+=triangleIndexStride;
1849  geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1850  }
1851  for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
1852  geom.indices.push_back(startoffset);
1853  geom.indices.push_back(ivert-1);
1854  geom.indices.push_back(ivert);
1855  }
1856  }
1857  }
1858  }
1859  else {
1860  ROS_WARN_STREAM("float array not defined!\n");
1861  }
1862  break;
1863  }
1864  }
1865  geom.InitCollisionMesh();
1866  return false;
1867  }
1868 
1872  bool _ExtractGeometry(const domGeometryRef geom, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1873  {
1874  if( !geom ) {
1875  return false;
1876  }
1877  std::vector<Vector3> vconvexhull;
1878  if (geom->getMesh()) {
1879  const domMeshRef meshRef = geom->getMesh();
1880  for (size_t tg = 0; tg<meshRef->getTriangles_array().getCount(); tg++) {
1881  _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1882  }
1883  for (size_t tg = 0; tg<meshRef->getTrifans_array().getCount(); tg++) {
1884  _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1885  }
1886  for (size_t tg = 0; tg<meshRef->getTristrips_array().getCount(); tg++) {
1887  _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1888  }
1889  for (size_t tg = 0; tg<meshRef->getPolylist_array().getCount(); tg++) {
1890  _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1891  }
1892  if( meshRef->getPolygons_array().getCount()> 0 ) {
1893  ROS_WARN_STREAM("openrave does not support collada polygons\n");
1894  }
1895 
1896  // if( alltrimesh.vertices.size() == 0 ) {
1897  // const domVerticesRef vertsRef = meshRef->getVertices();
1898  // for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
1899  // domInput_localRef localRef = vertsRef->getInput_array()[i];
1900  // daeString str = localRef->getSemantic();
1901  // if ( strcmp(str,"POSITION") == 0 ) {
1902  // const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1903  // if( !node )
1904  // continue;
1905  // double fUnitScale = _GetUnitScale(node);
1906  // const domFloat_arrayRef flArray = node->getFloat_array();
1907  // if (!!flArray) {
1908  // const domList_of_floats& listFloats = flArray->getValue();
1909  // int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1910  // vconvexhull.reserve(vconvexhull.size()+listFloats.getCount());
1911  // for (size_t vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride) {
1912  // //btVector3 verts[3];
1913  // domFloat fl0 = listFloats.get(vertIndex);
1914  // domFloat fl1 = listFloats.get(vertIndex+1);
1915  // domFloat fl2 = listFloats.get(vertIndex+2);
1916  // vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1917  // }
1918  // }
1919  // }
1920  // }
1921  //
1922  // _computeConvexHull(vconvexhull,alltrimesh);
1923  // }
1924 
1925  return true;
1926  }
1927  else if (geom->getConvex_mesh()) {
1928  {
1929  const domConvex_meshRef convexRef = geom->getConvex_mesh();
1930  daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
1931  if ( !!otherElemRef ) {
1932  domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
1933  ROS_WARN_STREAM( "otherLinked\n");
1934  }
1935  else {
1936  ROS_WARN("convexMesh polyCount = %d\n",(int)convexRef->getPolygons_array().getCount());
1937  ROS_WARN("convexMesh triCount = %d\n",(int)convexRef->getTriangles_array().getCount());
1938  }
1939  }
1940 
1941  const domConvex_meshRef convexRef = geom->getConvex_mesh();
1942  //daeString urlref = convexRef->getConvex_hull_of().getURI();
1943  daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
1944  if (urlref2) {
1945  daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
1946 
1947  // Load all the geometry libraries
1948  for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
1949  domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
1950  for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
1951  domGeometryRef lib = libgeom->getGeometry_array()[i];
1952  if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2
1953  //found convex_hull geometry
1954  domMesh *meshElement = lib->getMesh(); //linkedGeom->getMesh();
1955  if (meshElement) {
1956  const domVerticesRef vertsRef = meshElement->getVertices();
1957  for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
1958  domInput_localRef localRef = vertsRef->getInput_array()[i];
1959  daeString str = localRef->getSemantic();
1960  if ( strcmp(str,"POSITION") == 0) {
1961  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1962  if( !node ) {
1963  continue;
1964  }
1965  double fUnitScale = _GetUnitScale(node);
1966  const domFloat_arrayRef flArray = node->getFloat_array();
1967  if (!!flArray) {
1968  vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
1969  const domList_of_floats& listFloats = flArray->getValue();
1970  for (size_t k=0; k+2<flArray->getCount(); k+=3) {
1971  domFloat fl0 = listFloats.get(k);
1972  domFloat fl1 = listFloats.get(k+1);
1973  domFloat fl2 = listFloats.get(k+2);
1974  vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1975  }
1976  }
1977  }
1978  }
1979  }
1980  }
1981  }
1982  }
1983  }
1984  else {
1985  //no getConvex_hull_of but direct vertices
1986  const domVerticesRef vertsRef = convexRef->getVertices();
1987  for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
1988  domInput_localRef localRef = vertsRef->getInput_array()[i];
1989  daeString str = localRef->getSemantic();
1990  if ( strcmp(str,"POSITION") == 0 ) {
1991  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1992  if( !node ) {
1993  continue;
1994  }
1995  double fUnitScale = _GetUnitScale(node);
1996  const domFloat_arrayRef flArray = node->getFloat_array();
1997  if (!!flArray) {
1998  const domList_of_floats& listFloats = flArray->getValue();
1999  vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
2000  for (size_t k=0; k+2<flArray->getCount(); k+=3) {
2001  domFloat fl0 = listFloats.get(k);
2002  domFloat fl1 = listFloats.get(k+1);
2003  domFloat fl2 = listFloats.get(k+2);
2004  vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
2005  }
2006  }
2007  }
2008  }
2009  }
2010 
2011  if( vconvexhull.size()> 0 ) {
2012  listGeomProperties.push_back(GEOMPROPERTIES());
2013  GEOMPROPERTIES& geom = listGeomProperties.back();
2014  geom.type = GeomTrimesh;
2015 
2016  //_computeConvexHull(vconvexhull,trimesh);
2017  geom.InitCollisionMesh();
2018  }
2019  return true;
2020  }
2021 
2022  return false;
2023  }
2024 
2026  void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
2027  {
2028  // over write joint parameters by elements in instance_actuator
2029  for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
2030  domExtraRef pextra = as->getExtra_array()[ie];
2031  if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
2032  //std::string aname = pextra->getAttribute("name");
2033  domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
2034  if( !!tec ) {
2035  urdf::JointSharedPtr pjoint;
2036  daeElementRef domactuator;
2037  {
2038  daeElementRef bact = tec->getChild("bind_actuator");
2039  pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as);
2040  if (!pjoint) continue;
2041  }
2042  {
2043  daeElementRef iact = tec->getChild("instance_actuator");
2044  if(!iact) continue;
2045  std::string instance_url = iact->getAttribute("url");
2046  domactuator = daeURI(*iact, instance_url).getElement();
2047  if(!domactuator) continue;
2048  }
2049  daeElement *nom_torque = domactuator->getChild("nominal_torque");
2050  if ( !! nom_torque ) {
2051  if( !! pjoint->limits ) {
2052  pjoint->limits->effort = boost::lexical_cast<double>(nom_torque->getCharData());
2053  ROS_DEBUG("effort limit at joint (%s) is over written by %f",
2054  pjoint->name.c_str(), pjoint->limits->effort);
2055  }
2056  }
2057  }
2058  }
2059  }
2060  }
2061 
2063  void _ExtractRobotManipulators(const domArticulated_systemRef as)
2064  {
2065  ROS_DEBUG("collada manipulators not supported yet");
2066  }
2067 
2069  void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
2070  {
2071  ROS_DEBUG("collada sensors not supported yet");
2072  }
2073 
2074  inline daeElementRef _getElementFromUrl(const daeURI &uri)
2075  {
2076  return daeStandardURIResolver(*_collada).resolveElement(uri);
2077  }
2078 
2079  static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
2080  {
2081  if( !!paddr->getSIDREF() ) {
2082  return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt;
2083  }
2084  if (!!paddr->getParam()) {
2085  return searchBinding(paddr->getParam()->getValue(),parent);
2086  }
2087  return NULL;
2088  }
2089 
2093  static daeElement* searchBinding(daeString ref, daeElementRef parent)
2094  {
2095  if( !parent ) {
2096  return NULL;
2097  }
2098  daeElement* pelt = NULL;
2099  domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
2100  if( !!kscene ) {
2101  pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
2102  if( !!pelt ) {
2103  return pelt;
2104  }
2105  return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
2106  }
2107  domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
2108  if( !!articulated_system ) {
2109  if( !!articulated_system->getKinematics() ) {
2110  pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
2111  if( !!pelt ) {
2112  return pelt;
2113  }
2114  }
2115  if( !!articulated_system->getMotion() ) {
2116  return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
2117  }
2118  return NULL;
2119  }
2120  // try to get a bind array
2121  daeElementRef pbindelt;
2122  const domKinematics_bind_Array* pbindarray = NULL;
2123  const domKinematics_newparam_Array* pnewparamarray = NULL;
2124  domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
2125  if( !!ias ) {
2126  pbindarray = &ias->getBind_array();
2127  pbindelt = ias->getUrl().getElement();
2128  pnewparamarray = &ias->getNewparam_array();
2129  }
2130  if( !pbindarray || !pbindelt ) {
2131  domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
2132  if( !!ikm ) {
2133  pbindarray = &ikm->getBind_array();
2134  pbindelt = ikm->getUrl().getElement();
2135  pnewparamarray = &ikm->getNewparam_array();
2136  }
2137  }
2138  if( !!pbindarray && !!pbindelt ) {
2139  for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
2140  domKinematics_bindRef pbind = (*pbindarray)[ibind];
2141  if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
2142  // found a match
2143  if( !!pbind->getParam() ) {
2144  return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
2145  }
2146  else if( !!pbind->getSIDREF() ) {
2147  return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
2148  }
2149  }
2150  }
2151  for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
2152  domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
2153  if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
2154  if( !!newparam->getSIDREF() ) { // can only bind with SIDREF
2155  return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt;
2156  }
2157  ROS_WARN_STREAM(str(boost::format("newparam sid=%s does not have SIDREF\n")%newparam->getSid()));
2158  }
2159  }
2160  }
2161  ROS_WARN_STREAM(str(boost::format("failed to get binding '%s' for element: %s\n")%ref%parent->getElementName()));
2162  return NULL;
2163  }
2164 
2165  static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray)
2166  {
2167  for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
2168  daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
2169  if( !!pelt ) {
2170  return pelt;
2171  }
2172  }
2173  return NULL;
2174  }
2175 
2176  static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray)
2177  {
2178  for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
2179  daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
2180  if( !!pelt ) {
2181  return pelt;
2182  }
2183  }
2184  return NULL;
2185  }
2186 
2187  template <typename U> static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) {
2188  if( !!paddr->getBool() ) {
2189  return paddr->getBool()->getValue();
2190  }
2191  if( !paddr->getParam() ) {
2192  ROS_WARN_STREAM("param not specified, setting to 0\n");
2193  return false;
2194  }
2195  for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
2196  domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
2197  if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
2198  if( !!pnewparam->getBool() ) {
2199  return pnewparam->getBool()->getValue();
2200  }
2201  else if( !!pnewparam->getSIDREF() ) {
2202  domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
2203  if( !ptarget ) {
2204  ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
2205  continue;
2206  }
2207  return ptarget->getValue();
2208  }
2209  }
2210  }
2211  ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
2212  return false;
2213  }
2214  template <typename U> static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) {
2215  if( !!paddr->getFloat() ) {
2216  return paddr->getFloat()->getValue();
2217  }
2218  if( !paddr->getParam() ) {
2219  ROS_WARN_STREAM("param not specified, setting to 0\n");
2220  return 0;
2221  }
2222  for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
2223  domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
2224  if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
2225  if( !!pnewparam->getFloat() ) {
2226  return pnewparam->getFloat()->getValue();
2227  }
2228  else if( !!pnewparam->getSIDREF() ) {
2229  domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
2230  if( !ptarget ) {
2231  ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
2232  continue;
2233  }
2234  return ptarget->getValue();
2235  }
2236  }
2237  }
2238  ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
2239  return 0;
2240  }
2241 
2242  static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f)
2243  {
2244  daeElement* pfloat = pcommon->getChild("float");
2245  if( !!pfloat ) {
2246  std::stringstream sfloat(pfloat->getCharData());
2247  sfloat >> f;
2248  return !!sfloat;
2249  }
2250  daeElement* pparam = pcommon->getChild("param");
2251  if( !!pparam ) {
2252  if( pparam->hasAttribute("ref") ) {
2253  ROS_WARN_STREAM("cannot process param ref\n");
2254  }
2255  else {
2256  daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt;
2257  if( !!pelt ) {
2258  ROS_WARN_STREAM(str(boost::format("found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData()));
2259  }
2260  }
2261  }
2262  return false;
2263  }
2264 
2265  static boost::array<double,12> _matrixIdentity()
2266  {
2267  boost::array<double,12> m = {{1,0,0,0,0,1,0,0,0,0,1,0}};
2268  return m;
2269  };
2270 
2272  static boost::array<double,12> _getTransform(daeElementRef pelt)
2273  {
2274  boost::array<double,12> m = _matrixIdentity();
2275  domRotateRef protate = daeSafeCast<domRotate>(pelt);
2276  if( !!protate ) {
2277  m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (double)(protate->getValue()[3]*(M_PI/180.0)));
2278  return m;
2279  }
2280 
2281  domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
2282  if( !!ptrans ) {
2283  double scale = _GetUnitScale(pelt);
2284  m[3] = ptrans->getValue()[0]*scale;
2285  m[7] = ptrans->getValue()[1]*scale;
2286  m[11] = ptrans->getValue()[2]*scale;
2287  return m;
2288  }
2289 
2290  domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
2291  if( !!pmat ) {
2292  double scale = _GetUnitScale(pelt);
2293  for(int i = 0; i < 3; ++i) {
2294  m[4*i+0] = pmat->getValue()[4*i+0];
2295  m[4*i+1] = pmat->getValue()[4*i+1];
2296  m[4*i+2] = pmat->getValue()[4*i+2];
2297  m[4*i+3] = pmat->getValue()[4*i+3]*scale;
2298  }
2299  return m;
2300  }
2301 
2302  domScaleRef pscale = daeSafeCast<domScale>(pelt);
2303  if( !!pscale ) {
2304  m[0] = pscale->getValue()[0];
2305  m[4*1+1] = pscale->getValue()[1];
2306  m[4*2+2] = pscale->getValue()[2];
2307  return m;
2308  }
2309 
2310  domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
2311  if( pelt->typeID() == domLookat::ID() ) {
2312  ROS_ERROR_STREAM("look at transform not implemented\n");
2313  return m;
2314  }
2315 
2316  domSkewRef pskew = daeSafeCast<domSkew>(pelt);
2317  if( !!pskew ) {
2318  ROS_ERROR_STREAM("skew transform not implemented\n");
2319  }
2320 
2321  return m;
2322  }
2323 
2326  template <typename T> static boost::array<double,12> _getNodeParentTransform(const T pelt) {
2327  domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
2328  if( !pnode ) {
2329  return _matrixIdentity();
2330  }
2331  return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode));
2332  }
2333 
2335  template <typename T> static boost::array<double,12> _ExtractFullTransform(const T pelt) {
2336  boost::array<double,12> t = _matrixIdentity();
2337  for(size_t i = 0; i < pelt->getContents().getCount(); ++i) {
2338  t = _poseMult(t, _getTransform(pelt->getContents()[i]));
2339  }
2340  return t;
2341  }
2342 
2344  template <typename T> static boost::array<double,12> _ExtractFullTransformFromChildren(const T pelt) {
2345  boost::array<double,12> t = _matrixIdentity();
2346  daeTArray<daeElementRef> children; pelt->getChildren(children);
2347  for(size_t i = 0; i < children.getCount(); ++i) {
2348  t = _poseMult(t, _getTransform(children[i]));
2349  }
2350  return t;
2351  }
2352 
2353  // decompose a matrix into a scale and rigid transform (necessary for model scales)
2354  void _decompose(const boost::array<double,12>& tm, Pose& tout, Vector3& vscale)
2355  {
2356  vscale.x = 1; vscale.y = 1; vscale.z = 1;
2357  tout = _poseFromMatrix(tm);
2358  }
2359 
2360  virtual void handleError( daeString msg )
2361  {
2362  ROS_ERROR("COLLADA error: %s\n", msg);
2363  }
2364 
2365  virtual void handleWarning( daeString msg )
2366  {
2367  ROS_WARN("COLLADA warning: %s\n", msg);
2368  }
2369 
2370  inline static double _GetUnitScale(daeElement* pelt)
2371  {
2372  return ((USERDATA*)pelt->getUserData())->scale;
2373  }
2374 
2375  domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr)
2376  {
2377  for(size_t i = 0; i < arr.getCount(); ++i) {
2378  if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) {
2379  return arr[i];
2380  }
2381  }
2382  return domTechniqueRef();
2383  }
2384 
2387  for(size_t i = 0; i < arr.getCount(); ++i) {
2388  if( strcmp(arr[i]->getType(),"interface_type") == 0 ) {
2389  domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
2390  if( !!tec ) {
2391  daeElement* ptype = tec->getChild("interface");
2392  if( !!ptype ) {
2393  return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
2394  }
2395  }
2396  }
2397  }
2399  }
2400 
2401  std::string _ExtractLinkName(domLinkRef pdomlink) {
2402  std::string linkname;
2403  if( !!pdomlink ) {
2404  if( !!pdomlink->getName() ) {
2405  linkname = pdomlink->getName();
2406  }
2407  if( linkname.size() == 0 && !!pdomlink->getID() ) {
2408  linkname = pdomlink->getID();
2409  }
2410  }
2411  return linkname;
2412  }
2413 
2414  bool _checkMathML(daeElementRef pelt,const std::string& type)
2415  {
2416  if( pelt->getElementName()==type ) {
2417  return true;
2418  }
2419  // check the substring after ':'
2420  std::string name = pelt->getElementName();
2421  std::size_t pos = name.find_last_of(':');
2422  if( pos == std::string::npos ) {
2423  return false;
2424  }
2425  return name.substr(pos+1)==type;
2426  }
2427 
2428  urdf::JointSharedPtr _getJointFromRef(xsToken targetref, daeElementRef peltref) {
2429  daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
2430  domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
2431 
2432  if (!pdomjoint) {
2433  domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
2434  if (!!pdomijoint) {
2435  pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
2436  }
2437  }
2438 
2439  if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
2440  ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref));
2441  return urdf::JointSharedPtr();
2442  }
2443 
2444  urdf::JointSharedPtr pjoint;
2445  std::string name(pdomjoint->getName());
2446  if (_model->joints_.find(name) == _model->joints_.end()) {
2447  pjoint.reset();
2448  } else {
2449  pjoint = _model->joints_.find(name)->second;
2450  }
2451  if(!pjoint) {
2452  ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
2453  }
2454  return pjoint;
2455  }
2456 
2460  static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings)
2461  {
2462  domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
2463  if (!kscene) {
2464  return;
2465  }
2466  for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
2467  domArticulated_systemRef articulated_system; // if filled, contains robot-specific information, so create a robot
2468  domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
2469  if (!kbindmodel->getNode()) {
2470  ROS_WARN_STREAM("do not support kinematics models without references to nodes\n");
2471  continue;
2472  }
2473 
2474  // visual information
2475  domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt);
2476  if (!node) {
2477  ROS_WARN_STREAM(str(boost::format("bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode()));
2478  continue;
2479  }
2480 
2481  // kinematics information
2482  daeElement* pelt = searchBinding(kbindmodel,kscene);
2483  domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
2484  if (!kimodel) {
2485  if( !pelt ) {
2486  ROS_WARN_STREAM("bind_kinematics_model does not reference element\n");
2487  }
2488  else {
2489  ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName()));
2490  }
2491  continue;
2492  }
2493  bindings.listKinematicsVisualBindings.push_back(std::make_pair(node,kimodel));
2494  }
2495  // axis info
2496  for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
2497  domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
2498  daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt;
2499  if (!pjtarget) {
2500  ROS_ERROR_STREAM(str(boost::format("Target Node %s NOT found!!!\n")%bindjoint->getTarget()));
2501  continue;
2502  }
2503  daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
2504  domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
2505  if (!pjointaxis) {
2506  continue;
2507  }
2508  bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL));
2509  }
2510  }
2511 
2512  static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings)
2513  {
2514  for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
2515  domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
2516  for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
2517  domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
2518  domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
2519  domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
2520  for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
2521  LinkBinding lb;
2522  lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
2523  lb.node = daeSafeCast<domNode>(lb.irigidbody->getTarget().getElement().cast());
2524  lb.rigidbody = daeSafeCast<domRigid_body>(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt);
2525  lb.nodephysicsoffset = nodephysicsoffset;
2526  if( !!lb.rigidbody && !!lb.node ) {
2527  bindings.listLinkBindings.push_back(lb);
2528  }
2529  }
2530  }
2531  }
2532  }
2533 
2534 
2535  size_t _countChildren(daeElement* pelt) {
2536  size_t c = 1;
2537  daeTArray<daeElementRef> children;
2538  pelt->getChildren(children);
2539  for (size_t i = 0; i < children.getCount(); ++i) {
2540  c += _countChildren(children[i]);
2541  }
2542  return c;
2543  }
2544 
2545  void _processUserData(daeElement* pelt, double scale)
2546  {
2547  // getChild could be optimized since asset tag is supposed to appear as the first element
2548  domAssetRef passet = daeSafeCast<domAsset> (pelt->getChild("asset"));
2549  if (!!passet && !!passet->getUnit()) {
2550  scale = passet->getUnit()->getMeter();
2551  }
2552 
2553  _vuserdata.push_back(USERDATA(scale));
2554  pelt->setUserData(&_vuserdata.back());
2555  daeTArray<daeElementRef> children;
2556  pelt->getChildren(children);
2557  for (size_t i = 0; i < children.getCount(); ++i) {
2558  if (children[i] != passet) {
2559  _processUserData(children[i], scale);
2560  }
2561  }
2562  }
2563 
2564  USERDATA* _getUserData(daeElement* pelt)
2565  {
2566  BOOST_ASSERT(!!pelt);
2567  void* p = pelt->getUserData();
2568  BOOST_ASSERT(!!p);
2569  return (USERDATA*)p;
2570  }
2571 
2572  //
2573  // openrave math functions (from geometry.h)
2574  //
2575 
2576  static Vector3 _poseMult(const Pose& p, const Vector3& v)
2577  {
2578  double ww = 2 * p.rotation.x * p.rotation.x;
2579  double wx = 2 * p.rotation.x * p.rotation.y;
2580  double wy = 2 * p.rotation.x * p.rotation.z;
2581  double wz = 2 * p.rotation.x * p.rotation.w;
2582  double xx = 2 * p.rotation.y * p.rotation.y;
2583  double xy = 2 * p.rotation.y * p.rotation.z;
2584  double xz = 2 * p.rotation.y * p.rotation.w;
2585  double yy = 2 * p.rotation.z * p.rotation.z;
2586  double yz = 2 * p.rotation.z * p.rotation.w;
2587  Vector3 vnew;
2588  vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
2589  vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
2590  vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
2591  return vnew;
2592  }
2593 
2594  static Vector3 _poseMult(const boost::array<double,12>& m, const Vector3& v)
2595  {
2596  Vector3 vnew;
2597  vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3];
2598  vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3];
2599  vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3];
2600  return vnew;
2601  }
2602 
2603  static boost::array<double,12> _poseMult(const boost::array<double,12>& m0, const boost::array<double,12>& m1)
2604  {
2605  boost::array<double,12> mres;
2606  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];
2607  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];
2608  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];
2609  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];
2610  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];
2611  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];
2612  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];
2613  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];
2614  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];
2615  mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
2616  mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
2617  mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
2618  return mres;
2619  }
2620 
2621  static Pose _poseMult(const Pose& p0, const Pose& p1)
2622  {
2623  Pose p;
2624  p.position = _poseMult(p0,p1.position);
2625  p.rotation = _quatMult(p0.rotation,p1.rotation);
2626  return p;
2627  }
2628 
2629  static Pose _poseInverse(const Pose& p)
2630  {
2631  Pose pinv;
2632  pinv.rotation.x = -p.rotation.x;
2633  pinv.rotation.y = -p.rotation.y;
2634  pinv.rotation.z = -p.rotation.z;
2635  pinv.rotation.w = p.rotation.w;
2636  Vector3 t = _poseMult(pinv,p.position);
2637  pinv.position.x = -t.x;
2638  pinv.position.y = -t.y;
2639  pinv.position.z = -t.z;
2640  return pinv;
2641  }
2642 
2643  static Rotation _quatMult(const Rotation& quat0, const Rotation& quat1)
2644  {
2645  Rotation q;
2646  q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
2647  q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
2648  q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
2649  q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
2650  double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
2651  // don't touch the divides
2652  q.x /= fnorm;
2653  q.y /= fnorm;
2654  q.z /= fnorm;
2655  q.w /= fnorm;
2656  return q;
2657  }
2658 
2659  static boost::array<double,12> _matrixFromAxisAngle(const Vector3& axis, double angle)
2660  {
2661  return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle));
2662  }
2663 
2664  static boost::array<double,12> _matrixFromQuat(const Rotation& quat)
2665  {
2666  boost::array<double,12> m;
2667  double qq1 = 2*quat.x*quat.x;
2668  double qq2 = 2*quat.y*quat.y;
2669  double qq3 = 2*quat.z*quat.z;
2670  m[4*0+0] = 1 - qq2 - qq3;
2671  m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z);
2672  m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y);
2673  m[4*0+3] = 0;
2674  m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z);
2675  m[4*1+1] = 1 - qq1 - qq3;
2676  m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x);
2677  m[4*1+3] = 0;
2678  m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y);
2679  m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x);
2680  m[4*2+2] = 1 - qq1 - qq2;
2681  m[4*2+3] = 0;
2682  return m;
2683  }
2684 
2685  static Pose _poseFromMatrix(const boost::array<double,12>& m)
2686  {
2687  Pose t;
2688  t.rotation = _quatFromMatrix(m);
2689  t.position.x = m[3];
2690  t.position.y = m[7];
2691  t.position.z = m[11];
2692  return t;
2693  }
2694 
2695  static boost::array<double,12> _matrixFromPose(const Pose& t)
2696  {
2697  boost::array<double,12> m = _matrixFromQuat(t.rotation);
2698  m[3] = t.position.x;
2699  m[7] = t.position.y;
2700  m[11] = t.position.z;
2701  return m;
2702  }
2703 
2704  static Rotation _quatFromAxisAngle(double x, double y, double z, double angle)
2705  {
2706  Rotation q;
2707  double axislen = std::sqrt(x*x+y*y+z*z);
2708  if( axislen == 0 ) {
2709  return q;
2710  }
2711  angle *= 0.5;
2712  double sang = std::sin(angle)/axislen;
2713  q.w = std::cos(angle);
2714  q.x = x*sang;
2715  q.y = y*sang;
2716  q.z = z*sang;
2717  return q;
2718  }
2719 
2720  static Rotation _quatFromMatrix(const boost::array<double,12>& mat)
2721  {
2722  Rotation rot;
2723  double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
2724  if (tr >= 0) {
2725  rot.w = tr + 1;
2726  rot.x = (mat[4*2+1] - mat[4*1+2]);
2727  rot.y = (mat[4*0+2] - mat[4*2+0]);
2728  rot.z = (mat[4*1+0] - mat[4*0+1]);
2729  }
2730  else {
2731  // find the largest diagonal element and jump to the appropriate case
2732  if (mat[4*1+1] > mat[4*0+0]) {
2733  if (mat[4*2+2] > mat[4*1+1]) {
2734  rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
2735  rot.x = (mat[4*2+0] + mat[4*0+2]);
2736  rot.y = (mat[4*1+2] + mat[4*2+1]);
2737  rot.w = (mat[4*1+0] - mat[4*0+1]);
2738  }
2739  else {
2740  rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
2741  rot.z = (mat[4*1+2] + mat[4*2+1]);
2742  rot.x = (mat[4*0+1] + mat[4*1+0]);
2743  rot.w = (mat[4*0+2] - mat[4*2+0]);
2744  }
2745  }
2746  else if (mat[4*2+2] > mat[4*0+0]) {
2747  rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
2748  rot.x = (mat[4*2+0] + mat[4*0+2]);
2749  rot.y = (mat[4*1+2] + mat[4*2+1]);
2750  rot.w = (mat[4*1+0] - mat[4*0+1]);
2751  }
2752  else {
2753  rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
2754  rot.y = (mat[4*0+1] + mat[4*1+0]);
2755  rot.z = (mat[4*2+0] + mat[4*0+2]);
2756  rot.w = (mat[4*2+1] - mat[4*1+2]);
2757  }
2758  }
2759  double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
2760  // don't touch the divides
2761  rot.x /= fnorm;
2762  rot.y /= fnorm;
2763  rot.z /= fnorm;
2764  rot.w /= fnorm;
2765  return rot;
2766  }
2767 
2768  static double _dot3(const Vector3& v0, const Vector3& v1)
2769  {
2770  return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
2771  }
2772  static Vector3 _cross3(const Vector3& v0, const Vector3& v1)
2773  {
2774  Vector3 v;
2775  v.x = v0.y * v1.z - v0.z * v1.y;
2776  v.y = v0.z * v1.x - v0.x * v1.z;
2777  v.z = v0.x * v1.y - v0.y * v1.x;
2778  return v;
2779  }
2780  static Vector3 _sub3(const Vector3& v0, const Vector3& v1)
2781  {
2782  Vector3 v;
2783  v.x = v0.x-v1.x;
2784  v.y = v0.y-v1.y;
2785  v.z = v0.z-v1.z;
2786  return v;
2787  }
2788  static Vector3 _add3(const Vector3& v0, const Vector3& v1)
2789  {
2790  Vector3 v;
2791  v.x = v0.x+v1.x;
2792  v.y = v0.y+v1.y;
2793  v.z = v0.z+v1.z;
2794  return v;
2795  }
2796  static Vector3 _normalize3(const Vector3& v0)
2797  {
2798  Vector3 v;
2799  double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z);
2800  v.x = v0.x/norm;
2801  v.y = v0.y/norm;
2802  v.z = v0.z/norm;
2803  return v;
2804  }
2805 
2807  domCOLLADA* _dom;
2808  std::vector<USERDATA> _vuserdata; // all userdata
2809  int _nGlobalSensorId, _nGlobalManipulatorId;
2810  std::string _filename;
2811  std::string _resourcedir;
2812  urdf::ModelInterfaceSharedPtr _model;
2815 };
2816 
2817 
2818 
2819 
2820 urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_str)
2821 {
2822  urdf::ModelInterfaceSharedPtr model(new ModelInterface);
2823 
2824  ColladaModelReader reader(model);
2825  if (!reader.InitFromData(xml_str))
2826  model.reset();
2827  return model;
2828 }
2829 }
static Pose _poseMult(const Pose &p0, const Pose &p1)
boost::shared_ptr< std::string > _ExtractInterfaceType(const domExtra_Array &arr)
returns an openrave interface type from the extra array
static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings &bindings)
static boost::array< double, 12 > _ExtractFullTransformFromChildren(const T pelt)
Travel by the transformation array and calls the _getTransform method.
bool InitFromData(const std::string &pdata)
std::string _ExtractLinkName(domLinkRef pdomlink)
urdf::JointSharedPtr _getJointFromRef(xsToken targetref, daeElementRef peltref)
std::vector< USERDATA > _vuserdata
void _ExtractRobotManipulators(const domArticulated_systemRef as)
extract the robot manipulators
void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES &geom)
bool InitFromFile(const std::string &filename)
f
std::vector< int > indices
discretization value is chosen. Should be transformed by _t before rendering
static Vector3 _poseMult(const Pose &p, const Vector3 &v)
bool AddAxisInfo(const domInstance_kinematics_model_Array &arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
static boost::array< double, 12 > _poseMult(const boost::array< double, 12 > &m0, const boost::array< double, 12 > &m1)
static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U &parent)
XmlRpcServer s
static Pose _poseFromMatrix(const boost::array< double, 12 > &m)
static void GenerateSphereTriangulation(std::vector< Vector3 > realvertices, std::vector< int > realindices, int levels)
static boost::array< double, 12 > _getTransform(daeElementRef pelt)
Gets all transformations applied to the node.
#define FOREACHC
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
GeomType type
the type of geometry primitive
#define FOREACH(it, v)
domKinematics_axis_infoRef kinematics_axis_info
static boost::array< double, 12 > _matrixFromQuat(const Rotation &quat)
#define ROS_WARN(...)
virtual void handleError(daeString msg)
static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U &parent)
static Vector3 _normalize3(const Vector3 &v0)
static daeElement * searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
static boost::array< double, 12 > _matrixFromPose(const Pose &t)
bool InitCollisionMesh(double fTessellation=1.0)
static Vector3 _add3(const Vector3 &v0, const Vector3 &v1)
static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float &f)
static boost::array< double, 12 > _ExtractFullTransform(const T pelt)
Travel by the transformation array and calls the _getTransform method.
boost::shared_ptr< void > p
custom managed data
domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array &arr)
urdf::ModelInterfaceSharedPtr _model
bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
static Vector3 _sub3(const Vector3 &v0, const Vector3 &v1)
static Rotation _quatFromAxisAngle(double x, double y, double z, double angle)
UnlinkFilename(const std::string &filename)
bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
static boost::array< double, 12 > _matrixIdentity()
void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
extract the robot actuators
static Vector3 _poseMult(const boost::array< double, 12 > &m, const Vector3 &v)
static Vector3 _cross3(const Vector3 &v0, const Vector3 &v1)
urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string)
Load Model from string.
USERDATA * _getUserData(daeElement *pelt)
static Rotation _quatMult(const Rotation &quat0, const Rotation &quat1)
static double _GetUnitScale(daeElement *pelt)
void _processUserData(daeElement *pelt, double scale)
Pose _t
local transformation of the geom primitive with respect to the link&#39;s coordinate system ...
static double _dot3(const Vector3 &v0, const Vector3 &v1)
static Rotation _quatFromMatrix(const boost::array< double, 12 > &mat)
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
static boost::array< double, 12 > _matrixFromAxisAngle(const Vector3 &axis, double angle)
daeElementRef _getElementFromUrl(const daeURI &uri)
void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
Extract Sensors attached to a Robot.
static boost::array< double, 12 > _getNodeParentTransform(const T pelt)
boost::shared_ptr< DAE > _collada
bool _Extract()
the first possible robot in the scene
ColladaModelReader(urdf::ModelInterfaceSharedPtr model)
bindings for links between different spaces
bool _checkMathML(daeElementRef pelt, const std::string &type)
JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
bool _ExtractGeometry(const domGeometryRef geom, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
static daeElement * searchBindingArray(daeString ref, const domInstance_kinematics_model_Array &paramArray)
#define ROS_INFO_STREAM(args)
bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings &bindings)
append the kinematics model to the openrave kinbody
urdf::LinkSharedPtr _ExtractLink(const domLinkRef pdomlink, const domNodeRef pdomnode, const Pose &tParentWorldLink, const Pose &tParentLink, const std::vector< domJointRef > &vdomjoints, const KinematicsSceneBindings &bindings)
Extract Link info and add it to an existing body.
bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings &bindings)
extracts an articulated system. Note that an articulated system can include other articulated systems...
static daeElement * searchBindingArray(daeString ref, const domInstance_articulated_system_Array &paramArray)
void _ExtractGeometry(const domNodeRef pdomnode, std::list< GEOMPROPERTIES > &listGeomProperties, const std::list< JointAxisBinding > &listAxisBindings, const Pose &tlink)
Color ambientColor
hints for how to color the meshes
#define PRINT_POSE(pname, apose)
void _decompose(const boost::array< double, 12 > &tm, Pose &tout, Vector3 &vscale)
virtual void handleWarning(daeString msg)
inter-collada bindings for a kinematics scene
#define ROS_ERROR_STREAM(args)
static daeElement * searchBinding(daeString ref, daeElementRef parent)
bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings &bindings)
#define ROS_ERROR(...)
static std::list< boost::shared_ptr< UnlinkFilename > > _listTempFilenames
static Pose _poseInverse(const Pose &p)
size_t _countChildren(daeElement *pelt)
static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings &bindings)
go through all kinematics binds to get a kinematics/visual pair
domInstance_rigid_bodyRef irigidbody
std::list< std::pair< domNodeRef, domInstance_kinematics_modelRef > > listKinematicsVisualBindings
#define ROS_DEBUG(...)
urdf::GeometrySharedPtr _CreateGeometry(const std::string &name, const std::list< GEOMPROPERTIES > &listGeomProperties)


collada_parser
Author(s): Rosen Diankov, Kei Okada, Ioan Sucan , Jackie Kay
autogenerated on Wed Jul 15 2020 03:52:08