collada_urdf.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 /*********************************************************************
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2010, Willow Garage, Inc., University of Tokyo
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redstributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 /* Authors: Rosen Diankov, Tim Field */
37 
39 
40 #include <list>
41 #include <map>
42 #include <string>
43 #include <vector>
44 
45 #ifndef _WIN32
46 #pragma GCC diagnostic push
47 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
48 #include <dae.h>
49 #include <dae/daeDocument.h>
50 #include <dae/daeErrorHandler.h>
51 #include <dae/domAny.h>
52 #include <dom/domCOLLADA.h>
53 #include <dom/domConstants.h>
54 #include <dom/domElements.h>
55 #include <dom/domTriangles.h>
56 #include <dom/domTypes.h>
57 #pragma GCC diagnostic pop
58 #endif
59 
61 #include <urdf/model.h>
62 #include <urdf_model/pose.h>
63 #include <angles/angles.h>
64 #include <ros/assert.h>
65 
66 #include <boost/date_time/posix_time/posix_time.hpp>
67 #include <boost/date_time/posix_time/posix_time_io.hpp>
68 #include <boost/format.hpp>
69 #include <boost/array.hpp>
70 #include <boost/shared_ptr.hpp>
71 #include <boost/scoped_ptr.hpp>
72 
73 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
74 #include <assimp/scene.h>
75 #include <assimp/LogStream.hpp>
76 #include <assimp/DefaultLogger.hpp>
77 #include <assimp/IOStream.hpp>
78 #include <assimp/IOSystem.hpp>
79 #include <assimp/Importer.hpp>
80 #include <assimp/postprocess.h>
81 #else
82 #include <assimp.hpp>
83 #include <aiScene.h>
84 #include <aiPostProcess.h>
85 #include <DefaultLogger.h>
86 #include <IOStream.h>
87 #include <IOSystem.h>
88 #endif
89 
92 
93 #define FOREACH(it, v) for(decltype((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
94 #define FOREACHC FOREACH
95 
96 using namespace std;
97 
98 namespace ColladaDOM150 { }
99 
100 namespace collada_urdf {
101 
102 using namespace ColladaDOM150;
103 
105 class ResourceIOStream : public Assimp::IOStream
106 {
107 public:
109  : res_(res)
110  , pos_(res.data.get())
111  {
112  }
113 
115  {
116  }
117 
118  size_t Read(void* buffer, size_t size, size_t count)
119  {
120  size_t to_read = size * count;
121  if (pos_ + to_read > res_.data.get() + res_.size)
122  {
123  to_read = res_.size - (pos_ - res_.data.get());
124  }
125 
126  memcpy(buffer, pos_, to_read);
127  pos_ += to_read;
128 
129  return to_read;
130  }
131 
132  size_t Write( const void* buffer, size_t size, size_t count) {
133  ROS_BREAK(); return 0;
134  }
135 
136  aiReturn Seek( size_t offset, aiOrigin origin)
137  {
138  uint8_t* new_pos = 0;
139  switch (origin)
140  {
141  case aiOrigin_SET:
142  new_pos = res_.data.get() + offset;
143  break;
144  case aiOrigin_CUR:
145  new_pos = pos_ + offset; // TODO is this right? can offset really not be negative
146  break;
147  case aiOrigin_END:
148  new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
149  break;
150  default:
151  ROS_BREAK();
152  }
153 
154  if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
155  {
156  return aiReturn_FAILURE;
157  }
158 
159  pos_ = new_pos;
160  return aiReturn_SUCCESS;
161  }
162 
163  size_t Tell() const
164  {
165  return pos_ - res_.data.get();
166  }
167 
168  size_t FileSize() const
169  {
170  return res_.size;
171  }
172 
173  void Flush() {
174  }
175 
176 private:
178  uint8_t* pos_;
179 };
180 
181 namespace mathextra {
182 
183 // code from MagicSoftware by Dave Eberly
184 const double g_fEpsilon = 1e-15;
185 //===========================================================================
186 
187 
188 #define distinctRoots 0 // roots r0 < r1 < r2
189 #define singleRoot 1 // root r0
190 #define floatRoot01 2 // roots r0 = r1 < r2
191 #define floatRoot12 4 // roots r0 < r1 = r2
192 #define tripleRoot 6 // roots r0 = r1 = r2
193 
194 
195 template <typename T, typename S>
196 void Tridiagonal3 (S* mat, T* diag, T* subd)
197 {
198  T a, b, c, d, e, f, ell, q;
199 
200  a = mat[0*3+0];
201  b = mat[0*3+1];
202  c = mat[0*3+2];
203  d = mat[1*3+1];
204  e = mat[1*3+2];
205  f = mat[2*3+2];
206 
207  subd[2] = 0.0;
208  diag[0] = a;
209  if ( fabs(c) >= g_fEpsilon ) {
210  ell = (T)sqrt(b*b+c*c);
211  b /= ell;
212  c /= ell;
213  q = 2*b*e+c*(f-d);
214  diag[1] = d+c*q;
215  diag[2] = f-c*q;
216  subd[0] = ell;
217  subd[1] = e-b*q;
218  mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
219  mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
220  mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
221  }
222  else {
223  diag[1] = d;
224  diag[2] = f;
225  subd[0] = b;
226  subd[1] = e;
227  mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
228  mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
229  mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
230  }
231 }
232 
233 int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2)
234 {
235  // polynomial is L^3-c2*L^2+c1*L-c0
236 
237  int maxiter = 50;
238  double discr, temp, pval, pdval, b0, b1;
239  int i;
240 
241  // find local extrema (if any) of p'(L) = 3*L^2-2*c2*L+c1
242  discr = c2*c2-3*c1;
243  if ( discr >= 0.0 ) {
244  discr = (double)sqrt(discr);
245  temp = (c2+discr)/3;
246  pval = temp*(temp*(temp-c2)+c1)-c0;
247  if ( pval >= 0.0 ) {
248  // double root occurs before the positive local maximum
249  (*r0) = (c2-discr)/3 - 1; // initial guess for Newton's methods
250  pval = 2*g_fEpsilon;
251  for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
252  pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
253  pdval = (*r0)*(3*(*r0)-2*c2)+c1;
254  (*r0) -= pval/pdval;
255  }
256 
257  // Other two roots are solutions to quadratic equation
258  // L^2 + ((*r0)-c2)*L + [(*r0)*((*r0)-c2)+c1] = 0.
259  b1 = (*r0)-c2;
260  b0 = (*r0)*((*r0)-c2)+c1;
261  discr = b1*b1-4*b0;
262  if ( discr < -g_fEpsilon )
263  {
264  // single root r0
265  return singleRoot;
266  }
267  else
268  {
269  int result = distinctRoots;
270 
271  // roots r0 <= r1 <= r2
272  discr = sqrt(fabs(discr));
273  (*r1) = 0.5f*(-b1-discr);
274  (*r2) = 0.5f*(-b1+discr);
275 
276  if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
277  {
278  (*r0) = (*r1);
279  result |= floatRoot01;
280  }
281  if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
282  {
283  (*r1) = (*r2);
284  result |= floatRoot12;
285  }
286  return result;
287  }
288  }
289  else {
290  // double root occurs after the negative local minimum
291  (*r2) = temp + 1; // initial guess for Newton's method
292  pval = 2*g_fEpsilon;
293  for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
294  pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0;
295  pdval = (*r2)*(3*(*r2)-2*c2)+c1;
296  (*r2) -= pval/pdval;
297  }
298 
299  // Other two roots are solutions to quadratic equation
300  // L^2 + (r2-c2)*L + [r2*(r2-c2)+c1] = 0.
301  b1 = (*r2)-c2;
302  b0 = (*r2)*((*r2)-c2)+c1;
303  discr = b1*b1-4*b0;
304  if ( discr < -g_fEpsilon )
305  {
306  // single root
307  (*r0) = (*r2);
308  return singleRoot;
309  }
310  else
311  {
312  int result = distinctRoots;
313 
314  // roots r0 <= r1 <= r2
315  discr = sqrt(fabs(discr));
316  (*r0) = 0.5f*(-b1-discr);
317  (*r1) = 0.5f*(-b1+discr);
318 
319  if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
320  {
321  (*r0) = (*r1);
322  result |= floatRoot01;
323  }
324  if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
325  {
326  (*r1) = (*r2);
327  result |= floatRoot12;
328  }
329  return result;
330  }
331  }
332  }
333  else {
334  // p(L) has one double root
335  (*r0) = c0;
336  pval = 2*g_fEpsilon;
337  for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
338  pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
339  pdval = (*r0)*(3*(*r0)-2*c2)+c1;
340  (*r0) -= pval/pdval;
341  }
342  return singleRoot;
343  }
344 }
345 
346 //----------------------------------------------------------------------------
347 template <class T>
348 bool _QLAlgorithm3 (T* m_aafEntry, T* afDiag, T* afSubDiag)
349 {
350  // QL iteration with implicit shifting to reduce matrix from tridiagonal
351  // to diagonal
352 
353  for (int i0 = 0; i0 < 3; i0++)
354  {
355  const int iMaxIter = 32;
356  int iIter;
357  for (iIter = 0; iIter < iMaxIter; iIter++)
358  {
359  int i1;
360  for (i1 = i0; i1 <= 1; i1++)
361  {
362  T fSum = fabs(afDiag[i1]) +
363  fabs(afDiag[i1+1]);
364  if ( fabs(afSubDiag[i1]) + fSum == fSum )
365  break;
366  }
367  if ( i1 == i0 )
368  break;
369 
370  T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]);
371  T fTmp1 = sqrt(fTmp0*fTmp0+1.0f);
372  if ( fTmp0 < 0.0f )
373  fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
374  else
375  fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
376  T fSin = 1.0f;
377  T fCos = 1.0f;
378  T fTmp2 = 0.0f;
379  for (int i2 = i1-1; i2 >= i0; i2--)
380  {
381  T fTmp3 = fSin*afSubDiag[i2];
382  T fTmp4 = fCos*afSubDiag[i2];
383  if ( fabs(fTmp3) >= fabs(fTmp0) )
384  {
385  fCos = fTmp0/fTmp3;
386  fTmp1 = sqrt(fCos*fCos+1.0f);
387  afSubDiag[i2+1] = fTmp3*fTmp1;
388  fSin = 1.0f/fTmp1;
389  fCos *= fSin;
390  }
391  else
392  {
393  fSin = fTmp3/fTmp0;
394  fTmp1 = sqrt(fSin*fSin+1.0f);
395  afSubDiag[i2+1] = fTmp0*fTmp1;
396  fCos = 1.0f/fTmp1;
397  fSin *= fCos;
398  }
399  fTmp0 = afDiag[i2+1]-fTmp2;
400  fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos;
401  fTmp2 = fSin*fTmp1;
402  afDiag[i2+1] = fTmp0+fTmp2;
403  fTmp0 = fCos*fTmp1-fTmp4;
404 
405  for (int iRow = 0; iRow < 3; iRow++)
406  {
407  fTmp3 = m_aafEntry[iRow*3+i2+1];
408  m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] +
409  fCos*fTmp3;
410  m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] -
411  fSin*fTmp3;
412  }
413  }
414  afDiag[i0] -= fTmp2;
415  afSubDiag[i0] = fTmp0;
416  afSubDiag[i1] = 0.0f;
417  }
418 
419  if ( iIter == iMaxIter )
420  {
421  // should not get here under normal circumstances
422  return false;
423  }
424  }
425 
426  return true;
427 }
428 
429 bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag)
430 {
431  return _QLAlgorithm3<float>(m_aafEntry, afDiag, afSubDiag);
432 }
433 
434 bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag)
435 {
436  return _QLAlgorithm3<double>(m_aafEntry, afDiag, afSubDiag);
437 }
438 
439 void EigenSymmetric3(const double* fmat, double* afEigenvalue, double* fevecs)
440 {
441  double afSubDiag[3];
442 
443  memcpy(fevecs, fmat, sizeof(double)*9);
444  Tridiagonal3(fevecs, afEigenvalue,afSubDiag);
445  QLAlgorithm3(fevecs, afEigenvalue,afSubDiag);
446 
447  // make eigenvectors form a right--handed system
448  double fDet = fevecs[0*3+0] * (fevecs[1*3+1] * fevecs[2*3+2] - fevecs[1*3+2] * fevecs[2*3+1]) +
449  fevecs[0*3+1] * (fevecs[1*3+2] * fevecs[2*3+0] - fevecs[1*3+0] * fevecs[2*3+2]) +
450  fevecs[0*3+2] * (fevecs[1*3+0] * fevecs[2*3+1] - fevecs[1*3+1] * fevecs[2*3+0]);
451  if ( fDet < 0.0f )
452  {
453  fevecs[0*3+2] = -fevecs[0*3+2];
454  fevecs[1*3+2] = -fevecs[1*3+2];
455  fevecs[2*3+2] = -fevecs[2*3+2];
456  }
457 }
458 /* end of MAGIC code */
459 
460 } // end namespace geometry
461 
463 class ResourceIOSystem : public Assimp::IOSystem
464 {
465 public:
467  {
468  }
469 
471  {
472  }
473 
474  // Check whether a specific file exists
475  bool Exists(const char* file) const
476  {
477  // Ugly -- two retrievals where there should be one (Exists + Open)
478  // resource_retriever needs a way of checking for existence
479  // TODO: cache this
481  try {
482  res = retriever_.get(file);
483  }
484  catch (resource_retriever::Exception& e) {
485  return false;
486  }
487 
488  return true;
489  }
490 
491  // Get the path delimiter character we'd like to see
492  char getOsSeparator() const
493  {
494  return '/';
495  }
496 
497  // ... and finally a method to open a custom stream
498  Assimp::IOStream* Open(const char* file, const char* mode)
499  {
500  ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
501 
502  // Ugly -- two retrievals where there should be one (Exists + Open)
503  // resource_retriever needs a way of checking for existence
505  try {
506  res = retriever_.get(file);
507  }
508  catch (resource_retriever::Exception& e) {
509  return 0;
510  }
511 
512  return new ResourceIOStream(res);
513  }
514 
515  void Close(Assimp::IOStream* stream) {
516  delete stream;
517  }
518 
519 private:
521 };
522 
523 class Triangle
524 {
525 public:
526  Triangle(const urdf::Vector3 &_p1, const urdf::Vector3 &_p2, const urdf::Vector3 &_p3) :
527  p1(_p1), p2(_p2), p3(_p3)
528  {}
529  Triangle() { this->clear(); };
530  urdf::Vector3 p1, p2, p3;
531 
532  void clear() { p1.clear(); p2.clear(); p3.clear(); };
533 };
534 
536 // Portions of this code are taken verbatim from OpenRAVE (https://github.com/rdiankov/openrave, commits 87410293, ca3473f7, d844de2a) and relicensed as BSD by the original author (rdiankov)
537 class ColladaWriter : public daeErrorHandler
538 {
539 private:
540  struct SCENE
541  {
542  domVisual_sceneRef vscene;
543  domKinematics_sceneRef kscene;
544  domPhysics_sceneRef pscene;
545  domInstance_with_extraRef viscene;
546  domInstance_kinematics_sceneRef kiscene;
547  domInstance_with_extraRef piscene;
548  };
549 
550  typedef std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES;
551  struct LINKOUTPUT
552  {
553  list<pair<int,string> > listusedlinks;
554  list<pair<int,string> > listprocesseddofs;
555  daeElementRef plink;
556  domNodeRef pnode;
557  MAPLINKPOSES _maplinkposes;
558  };
559 
561  {
562  domPhysics_modelRef pmodel;
563  std::vector<std::string > vrigidbodysids;
564  };
565 
567  {
568  struct axis_output
569  {
570  //axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {}
571  axis_output() : iaxis(0) {
572  }
573  string sid, nodesid;
574  urdf::JointConstSharedPtr pjoint;
575  int iaxis;
576  string jointnodesid;
577  };
578  domKinematics_modelRef kmodel;
579  std::vector<axis_output> vaxissids;
580  std::vector<std::string > vlinksids;
581  MAPLINKPOSES _maplinkposes;
582  };
583 
584  struct axis_sids
585  {
586  axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
587  }
588  string axissid, valuesid, jointnodesid;
589  };
590 
592  {
593  domInstance_kinematics_modelRef ikm;
594  std::vector<axis_sids> vaxissids;
596  std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
597  };
598 
600  {
601  domInstance_articulated_systemRef ias;
602  std::vector<axis_sids> vaxissids;
603  std::vector<std::string > vlinksids;
604  std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
605  };
606 
608  {
609  domInstance_physics_modelRef ipm;
611  };
612 
614  {
615  std::string uri, kinematicsgeometryhash;
618  };
619 
620 public:
621  ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) {
622  daeErrorHandler::setErrorHandler(this);
623  _importer.SetIOHandler(new ResourceIOSystem());
624  }
625  virtual ~ColladaWriter() {
626  }
627 
628  daeDocument* doc() {
629  return _doc;
630  }
631 
632  bool convert()
633  {
634  try {
635  const char* documentName = "urdf_snapshot";
636  daeInt error = _collada.getDatabase()->insertDocument(documentName, &_doc ); // also creates a collada root
637  if (error != DAE_OK || _doc == NULL) {
638  throw ColladaUrdfException("Failed to create document");
639  }
640  _dom = daeSafeCast<domCOLLADA>(_doc->getDomRoot());
641  _dom->setAttribute("xmlns:math","http://www.w3.org/1998/Math/MathML");
642 
643  //create the required asset tag
644  domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
645  {
646  // facet becomes owned by locale, so no need to explicitly delete
647  boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");
648  std::stringstream ss;
649  ss.imbue(std::locale(ss.getloc(), facet));
650  ss << boost::posix_time::second_clock::local_time();
651 
652  domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
653  created->setValue(ss.str().c_str());
654  domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
655  modified->setValue(ss.str().c_str());
656 
657  domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_CONTRIBUTOR ) );
658  domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) );
659  authoringtool->setValue("URDF Collada Writer");
660 
661  domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>( asset->add( COLLADA_ELEMENT_UNIT ) );
662  units->setMeter(1);
663  units->setName("meter");
664 
665  domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>( asset->add( COLLADA_ELEMENT_UP_AXIS ) );
666  zup->setValue(UP_AXIS_Z_UP);
667  }
668 
669  _globalscene = _dom->getScene();
670  if( !_globalscene ) {
671  _globalscene = daeSafeCast<domCOLLADA::domScene>( _dom->add( COLLADA_ELEMENT_SCENE ) );
672  }
673 
674  _visualScenesLib = daeSafeCast<domLibrary_visual_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
675  _visualScenesLib->setId("vscenes");
676  _geometriesLib = daeSafeCast<domLibrary_geometries>(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
677  _geometriesLib->setId("geometries");
678  _effectsLib = daeSafeCast<domLibrary_effects>(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
679  _effectsLib->setId("effects");
680  _materialsLib = daeSafeCast<domLibrary_materials>(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
681  _materialsLib->setId("materials");
682  _kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
683  _kinematicsModelsLib->setId("kmodels");
684  _articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
685  _articulatedSystemsLib->setId("asystems");
686  _kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
687  _kinematicsScenesLib->setId("kscenes");
688  _physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
689  _physicsScenesLib->setId("pscenes");
690  _physicsModelsLib = daeSafeCast<domLibrary_physics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS));
691  _physicsModelsLib->setId("pmodels");
692  domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
693  pextra_library_sensors->setId("sensors");
694  pextra_library_sensors->setType("library_sensors");
695  _sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
696  _sensorsLib->setProfile("OpenRAVE");
697 
698  _CreateScene();
699  _WritePhysics();
700  _WriteRobot();
701  _WriteBindingsInstance_kinematics_scene();
702  return true;
703  }
704  catch (ColladaUrdfException ex) {
705  ROS_ERROR("Error converting: %s", ex.what());
706  return false;
707  }
708  }
709 
710  bool writeTo(string const& file) {
711  try {
712  daeString uri = _doc->getDocumentURI()->getURI();
713  _collada.writeTo(uri, file);
714  } catch (ColladaUrdfException ex) {
715  return false;
716  }
717  return true;
718  }
719 
720 protected:
721  virtual void handleError(daeString msg) {
722  throw ColladaUrdfException(msg);
723  }
724  virtual void handleWarning(daeString msg) {
725  std::cerr << "COLLADA DOM warning: " << msg << std::endl;
726  }
727 
729  {
730  // Create visual scene
731  _scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
732  _scene.vscene->setId("vscene");
733  _scene.vscene->setName("URDF Visual Scene");
734 
735  // Create kinematics scene
736  _scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
737  _scene.kscene->setId("kscene");
738  _scene.kscene->setName("URDF Kinematics Scene");
739 
740  // Create physic scene
741  _scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
742  _scene.pscene->setId("pscene");
743  _scene.pscene->setName("URDF Physics Scene");
744 
745  // Create instance visual scene
746  _scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
747  _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() );
748 
749  // Create instance kinematics scene
750  _scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
751  _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() );
752 
753  // Create instance physics scene
754  _scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
755  _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() );
756  }
757 
758  void _WritePhysics() {
759  domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
760  // Create gravity
761  domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
762  g->getValue().set3 (0,0,0);
763  }
764 
766  void _WriteRobot(int id = 0)
767  {
768  ROS_DEBUG_STREAM(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%id%_robot.getName()));
769  string asid = _ComputeId(str(boost::format("robot%d")%id));
770  string askid = _ComputeId(str(boost::format("%s_kinematics")%asid));
771  string asmid = _ComputeId(str(boost::format("%s_motion")%asid));
772  string iassid = _ComputeId(str(boost::format("%s_inst")%asmid));
773 
774  domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
775  ias->setSid(iassid.c_str());
776  ias->setUrl((string("#")+asmid).c_str());
777  ias->setName(_robot.getName().c_str());
778 
779  _iasout.reset(new instance_articulated_system_output());
780  _iasout->ias = ias;
781 
782  // motion info
783  domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
784  articulated_system_motion->setId(asmid.c_str());
785  domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
786  domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
787  domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
788  ias_motion->setUrl(str(boost::format("#%s")%askid).c_str());
789 
790  // kinematics info
791  domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
792  articulated_system_kinematics->setId(askid.c_str());
793  domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
794  domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
795 
796  _WriteInstance_kinematics_model(kinematics,askid,id);
797 
798  for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
799  string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof));
800  urdf::JointConstSharedPtr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
801  BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof);
802  //int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis;
803 
804  // Kinematics axis info
805  domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
806  kai->setAxis(str(boost::format("%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str());
807  kai->setSid(axis_infosid.c_str());
808  bool bactive = !pjoint->mimic;
809  double flower=0, fupper=0;
810  if( pjoint->type != urdf::Joint::CONTINUOUS ) {
811  if( !!pjoint->limits ) {
812  flower = pjoint->limits->lower;
813  fupper = pjoint->limits->upper;
814  }
815  if( !!pjoint->safety ) {
816  flower = pjoint->safety->soft_lower_limit;
817  fupper = pjoint->safety->soft_upper_limit;
818  }
819  if( flower == fupper ) {
820  bactive = false;
821  }
822  double fmult = 1.0;
823  if( pjoint->type != urdf::Joint::PRISMATIC ) {
824  fmult = 180.0/M_PI;
825  }
826  domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
827  daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult);
828  daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult);
829  }
830 
831  domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
832  daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive);
833  domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
834  daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false);
835 
836  // Motion axis info
837  domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
838  mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str());
839  if( !!pjoint->limits ) {
840  domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
841  daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity);
842  domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
843  daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort);
844  }
845  }
846 
847  // write the bindings
848  string asmsym = _ComputeId(str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid()));
849  string assym = _ComputeId(str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()));
850  FOREACH(it, _ikmout->vkinematicsbindings) {
851  domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
852  abm->setSid(asmsym.c_str());
853  daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
854  domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
855  ab->setSymbol(assym.c_str());
856  daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str());
857  _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second));
858  }
859  for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
860  const axis_sids& kas = _ikmout->vaxissids.at(idof);
861  domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
862  abm->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.axissid)).c_str());
863  daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
864  domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
865  ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
866  daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
867  string valuesid;
868  if( kas.valuesid.size() > 0 ) {
869  domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
870  abmvalue->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.valuesid)).c_str());
871  daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
872  domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
873  valuesid = _ComputeId(str(boost::format("%s_%s")%assym%kas.valuesid));
874  abvalue->setSymbol(valuesid.c_str());
875  daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
876  }
877  _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid));
878  }
879 
880  boost::shared_ptr<instance_physics_model_output> ipmout = _WriteInstance_physics_model(id,_scene.pscene,_scene.pscene->getID(), _ikmout->kmout->_maplinkposes);
881  }
882 
884  virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string& sidscope, int id)
885  {
886  ROS_DEBUG_STREAM(str(boost::format("writing instance_kinematics_model %s\n")%_robot.getName()));
887  boost::shared_ptr<kinematics_model_output> kmout = WriteKinematics_model(id);
888 
889  _ikmout.reset(new instance_kinematics_model_output());
890  _ikmout->kmout = kmout;
891  _ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
892 
893  string symscope, refscope;
894  if( sidscope.size() > 0 ) {
895  symscope = sidscope+string("_");
896  refscope = sidscope+string("/");
897  }
898  string ikmsid = _ComputeId(str(boost::format("%s_inst")%kmout->kmodel->getID()));
899  _ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str());
900  _ikmout->ikm->setSid(ikmsid.c_str());
901 
902  domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
903  kbind->setSid(_ComputeId(symscope+ikmsid).c_str());
904  daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
905  _ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node%d")%id%_maplinkindices[_robot.getRoot()])));
906 
907  _ikmout->vaxissids.reserve(kmout->vaxissids.size());
908  int i = 0;
909  FOREACH(it,kmout->vaxissids) {
910  domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
911  string ref = it->sid;
912  size_t index = ref.find("/");
913  while(index != string::npos) {
914  ref[index] = '.';
915  index = ref.find("/",index+1);
916  }
917  string sid = _ComputeId(symscope+ikmsid+"_"+ref);
918  kbind->setSid(sid.c_str());
919  daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
920  double value=0;
921  double flower=0, fupper=0;
922  if( !!it->pjoint->limits ) {
923  flower = it->pjoint->limits->lower;
924  fupper = it->pjoint->limits->upper;
925  }
926  if( flower > 0 || fupper < 0 ) {
927  value = 0.5*(flower+fupper);
928  }
929  domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
930  pvalueparam->setSid((sid+string("_value")).c_str());
931  daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
932  _ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
933  ++i;
934  }
935  }
936 
938  {
939  domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
940  string kmodelid = _ComputeKinematics_modelId(id);
941  kmodel->setId(kmodelid.c_str());
942  kmodel->setName(_robot.getName().c_str());
943 
944  domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
945 
946  // Create root node for the visual scene
947  domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
948  string bodyid = _ComputeId(str(boost::format("visual%d")%id));
949  pnoderoot->setId(bodyid.c_str());
950  pnoderoot->setSid(bodyid.c_str());
951  pnoderoot->setName(_robot.getName().c_str());
952 
953  // Declare all the joints
954  _mapjointindices.clear();
955  int index = 0;
956  FOREACHC(itj, _robot.joints_) {
957  _mapjointindices[itj->second] = index++;
958  }
959  _maplinkindices.clear();
960  index = 0;
961  FOREACHC(itj, _robot.links_) {
962  _maplinkindices[itj->second] = index++;
963  }
964  _mapmaterialindices.clear();
965  index = 0;
966  FOREACHC(itj, _robot.materials_) {
967  _mapmaterialindices[itj->second] = index++;
968  }
969 
970  double lmin, lmax;
971  vector<domJointRef> vdomjoints(_robot.joints_.size());
973  kmout->kmodel = kmodel;
974  kmout->vaxissids.resize(_robot.joints_.size());
975  kmout->vlinksids.resize(_robot.links_.size());
976 
977  FOREACHC(itjoint, _robot.joints_) {
978  urdf::JointSharedPtr pjoint = itjoint->second;
979  int index = _mapjointindices[itjoint->second];
980  domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
981  string jointid = _ComputeId(pjoint->name); //str(boost::format("joint%d")%index);
982  pdomjoint->setSid(jointid.c_str() );
983  pdomjoint->setName(pjoint->name.c_str());
984  domAxis_constraintRef axis;
985  if( !!pjoint->limits ) {
986  lmin = pjoint->limits->lower;
987  lmax = pjoint->limits->upper;
988  }
989  else {
990  lmin = lmax = 0;
991  }
992 
993  double fmult = 1.0;
994  switch(pjoint->type) {
995  case urdf::Joint::REVOLUTE:
996  case urdf::Joint::CONTINUOUS:
997  axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
998  fmult = 180.0f/M_PI;
999  lmin *= fmult;
1000  lmax *= fmult;
1001  break;
1002  case urdf::Joint::PRISMATIC:
1003  axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
1004  break;
1005  case urdf::Joint::FIXED:
1006  axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
1007  lmin = 0;
1008  lmax = 0;
1009  fmult = 0;
1010  break;
1011  default:
1012  ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
1013  break;
1014  }
1015 
1016  if( !axis ) {
1017  continue;
1018  }
1019 
1020  int ia = 0;
1021  string axisid = _ComputeId(str(boost::format("axis%d")%ia));
1022  axis->setSid(axisid.c_str());
1023  kmout->vaxissids.at(index).pjoint = pjoint;
1024  kmout->vaxissids.at(index).sid = jointid+string("/")+axisid;
1025  kmout->vaxissids.at(index).iaxis = ia;
1026  domAxisRef paxis = daeSafeCast<domAxis>(axis->add(COLLADA_ELEMENT_AXIS));
1027  paxis->getValue().setCount(3);
1028  paxis->getValue()[0] = pjoint->axis.x;
1029  paxis->getValue()[1] = pjoint->axis.y;
1030  paxis->getValue()[2] = pjoint->axis.z;
1031  if( pjoint->type != urdf::Joint::CONTINUOUS ) {
1032  domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(axis->add(COLLADA_TYPE_LIMITS));
1033  daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin;
1034  daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax;
1035  }
1036  vdomjoints.at(index) = pdomjoint;
1037  }
1038 
1039  LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID());
1040  FOREACHC(itused, childinfo.listusedlinks) {
1041  kmout->vlinksids.at(itused->first) = itused->second;
1042  }
1043  FOREACH(itprocessed,childinfo.listprocesseddofs) {
1044  kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second;
1045  }
1046  kmout->_maplinkposes = childinfo._maplinkposes;
1047 
1048  // create the formulas for all mimic joints
1049  FOREACHC(itjoint, _robot.joints_) {
1050  string jointsid = _ComputeId(itjoint->second->name);
1051  urdf::JointSharedPtr pjoint = itjoint->second;
1052  if( !pjoint->mimic ) {
1053  continue;
1054  }
1055 
1056  domFormulaRef pf = daeSafeCast<domFormula>(ktec->add(COLLADA_ELEMENT_FORMULA));
1057  string formulaid = _ComputeId(str(boost::format("%s_formula")%jointsid));
1058  pf->setSid(formulaid.c_str());
1059  domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(pf->add(COLLADA_ELEMENT_TARGET));
1060  string targetjointid = str(boost::format("%s/%s")%kmodel->getID()%jointsid);
1061  daeSafeCast<domCommon_param>(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str());
1062 
1063  domTechniqueRef pftec = daeSafeCast<domTechnique>(pf->add(COLLADA_ELEMENT_TECHNIQUE));
1064  pftec->setProfile("OpenRAVE");
1065  // save position equation
1066  {
1067  daeElementRef poselt = pftec->add("equation");
1068  poselt->setAttribute("type","position");
1069  // create a const0*joint+const1 formula
1070  // <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
1071  daeElementRef pmath_math = poselt->add("math");
1072  daeElementRef pmath_apply = pmath_math->add("apply");
1073  {
1074  daeElementRef pmath_plus = pmath_apply->add("plus");
1075  daeElementRef pmath_apply1 = pmath_apply->add("apply");
1076  {
1077  daeElementRef pmath_times = pmath_apply1->add("times");
1078  daeElementRef pmath_const0 = pmath_apply1->add("cn");
1079  pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
1080  daeElementRef pmath_symb = pmath_apply1->add("csymbol");
1081  pmath_symb->setAttribute("encoding","COLLADA");
1082  pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
1083  }
1084  daeElementRef pmath_const1 = pmath_apply->add("cn");
1085  pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
1086  }
1087  }
1088  // save first partial derivative
1089  {
1090  daeElementRef derivelt = pftec->add("equation");
1091  derivelt->setAttribute("type","first_partial");
1092  derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
1093  daeElementRef pmath_const0 = derivelt->add("cn");
1094  pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
1095  }
1096  // save second partial derivative
1097  {
1098  daeElementRef derivelt = pftec->add("equation");
1099  derivelt->setAttribute("type","second_partial");
1100  derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
1101  daeElementRef pmath_const0 = derivelt->add("cn");
1102  pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
1103  }
1104 
1105  {
1106  domFormula_techniqueRef pfcommontec = daeSafeCast<domFormula_technique>(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1107  // create a const0*joint+const1 formula
1108  // <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
1109  daeElementRef pmath_math = pfcommontec->add("math");
1110  daeElementRef pmath_apply = pmath_math->add("apply");
1111  {
1112  daeElementRef pmath_plus = pmath_apply->add("plus");
1113  daeElementRef pmath_apply1 = pmath_apply->add("apply");
1114  {
1115  daeElementRef pmath_times = pmath_apply1->add("times");
1116  daeElementRef pmath_const0 = pmath_apply1->add("cn");
1117  pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
1118  daeElementRef pmath_symb = pmath_apply1->add("csymbol");
1119  pmath_symb->setAttribute("encoding","COLLADA");
1120  pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
1121  }
1122  daeElementRef pmath_const1 = pmath_apply->add("cn");
1123  pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
1124  }
1125  }
1126  }
1127 
1128  return kmout;
1129  }
1130 
1137  virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
1138  {
1139  LINKOUTPUT out;
1140  int linkindex = _maplinkindices[plink];
1141  string linksid = _ComputeId(plink->name);
1142  domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
1143  pdomlink->setName(plink->name.c_str());
1144  pdomlink->setSid(linksid.c_str());
1145 
1146  domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
1147  string nodeid = _ComputeId(str(boost::format("v%s_node%d")%strModelUri%linkindex));
1148  pnode->setId( nodeid.c_str() );
1149  string nodesid = _ComputeId(str(boost::format("node%d")%linkindex));
1150  pnode->setSid(nodesid.c_str());
1151  pnode->setName(plink->name.c_str());
1152 
1153  urdf::GeometrySharedPtr geometry;
1154  urdf::MaterialSharedPtr material;
1155  urdf::Pose geometry_origin;
1156  if( !!plink->visual ) {
1157  geometry = plink->visual->geometry;
1158  material = plink->visual->material;
1159  geometry_origin = plink->visual->origin;
1160  }
1161  else if( !!plink->collision ) {
1162  geometry = plink->collision->geometry;
1163  geometry_origin = plink->collision->origin;
1164  }
1165 
1166  urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin);
1167 
1168  if( !!geometry ) {
1169  bool write_visual = false;
1170  if ( !!plink->visual ) {
1171  if (plink->visual_array.size() > 1) {
1172  int igeom = 0;
1173  for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin();
1174  it != plink->visual_array.end(); it++) {
1175  // geom
1176  string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
1177  igeom++;
1178  domGeometryRef pdomgeom;
1179  if ( it != plink->visual_array.begin() ) {
1180  urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin);
1181  pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans);
1182  }
1183  else {
1184  pdomgeom = _WriteGeometry((*it)->geometry, geomid);
1185  }
1186  domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1187  pinstgeom->setUrl((string("#") + geomid).c_str());
1188  // material
1189  _WriteMaterial(pdomgeom->getID(), (*it)->material);
1190  domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
1191  domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1192  domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
1193  pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
1194  pinstmat->setSymbol("mat0");
1195  write_visual = true;
1196  }
1197  }
1198  }
1199  if (!write_visual) {
1200  // just 1 visual
1201  int igeom = 0;
1202  string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
1203  domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
1204  domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1205  pinstgeom->setUrl((string("#")+geomid).c_str());
1206 
1207  // material
1208  _WriteMaterial(pdomgeom->getID(), material);
1209  domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
1210  domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1211  domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
1212  pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
1213  pinstmat->setSymbol("mat0");
1214  }
1215  }
1216 
1217  _WriteTransformation(pnode, geometry_origin);
1218 
1219  // process all children
1220  FOREACHC(itjoint, plink->child_joints) {
1221  urdf::JointSharedPtr pjoint = *itjoint;
1222  int index = _mapjointindices[pjoint];
1223 
1224  // <attachment_full joint="k1/joint0">
1225  domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
1226  string jointid = str(boost::format("%s/%s")%strModelUri%_ComputeId(pjoint->name));
1227  attachment_full->setJoint(jointid.c_str());
1228 
1229  LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri);
1230  FOREACH(itused,childinfo.listusedlinks) {
1231  out.listusedlinks.push_back(make_pair(itused->first,linksid+string("/")+itused->second));
1232  }
1233  FOREACH(itprocessed,childinfo.listprocesseddofs) {
1234  out.listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+string("/")+itprocessed->second));
1235  }
1236  FOREACH(itlinkpos,childinfo._maplinkposes) {
1237  out._maplinkposes[itlinkpos->first] = _poseMult(pjoint->parent_to_joint_origin_transform,itlinkpos->second);
1238  }
1239  // rotate/translate elements
1240  string jointnodesid = _ComputeId(str(boost::format("node_%s_axis0")%pjoint->name));
1241  switch(pjoint->type) {
1242  case urdf::Joint::REVOLUTE:
1243  case urdf::Joint::CONTINUOUS:
1244  case urdf::Joint::FIXED: {
1245  domRotateRef protate = daeSafeCast<domRotate>(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0));
1246  protate->setSid(jointnodesid.c_str());
1247  protate->getValue().setCount(4);
1248  protate->getValue()[0] = pjoint->axis.x;
1249  protate->getValue()[1] = pjoint->axis.y;
1250  protate->getValue()[2] = pjoint->axis.z;
1251  protate->getValue()[3] = 0;
1252  break;
1253  }
1254  case urdf::Joint::PRISMATIC: {
1255  domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
1256  ptrans->setSid(jointnodesid.c_str());
1257  ptrans->getValue().setCount(3);
1258  ptrans->getValue()[0] = 0;
1259  ptrans->getValue()[1] = 0;
1260  ptrans->getValue()[2] = 0;
1261  break;
1262  }
1263  default:
1264  ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
1265  break;
1266  }
1267 
1268  _WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform);
1269  _WriteTransformation(childinfo.pnode, pjoint->parent_to_joint_origin_transform);
1270  _WriteTransformation(childinfo.pnode, geometry_origin_inv); // have to do multiply by inverse since geometry transformation is not part of hierarchy
1271  out.listprocesseddofs.push_back(make_pair(index,string(childinfo.pnode->getSid())+string("/")+jointnodesid));
1272  // </attachment_full>
1273  }
1274 
1275  out._maplinkposes[plink] = urdf::Pose();
1276  out.listusedlinks.push_back(make_pair(linkindex,linksid));
1277  out.plink = pdomlink;
1278  out.pnode = pnode;
1279  return out;
1280  }
1281 
1282  domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL)
1283  {
1284  domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
1285  cgeometry->setId(geometry_id.c_str());
1286  switch (geometry->type) {
1287  case urdf::Geometry::MESH: {
1288  urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
1289  cgeometry->setName(urdf_mesh->filename.c_str());
1290  _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale, org_trans);
1291  break;
1292  }
1293  case urdf::Geometry::SPHERE: {
1294  shapes::Sphere sphere(static_cast<urdf::Sphere*>(geometry.get())->radius);
1295  boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(sphere));
1296  _loadVertices(mesh.get(), cgeometry);
1297  break;
1298  }
1299  case urdf::Geometry::BOX: {
1300  shapes::Box box(static_cast<urdf::Box*>(geometry.get())->dim.x,
1301  static_cast<urdf::Box*>(geometry.get())->dim.y,
1302  static_cast<urdf::Box*>(geometry.get())->dim.z);
1303  boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(box));
1304  _loadVertices(mesh.get(), cgeometry);
1305  break;
1306  }
1307  case urdf::Geometry::CYLINDER: {
1308  shapes::Cylinder cyl(static_cast<urdf::Cylinder*>(geometry.get())->radius,
1309  static_cast<urdf::Cylinder*>(geometry.get())->length);
1310  boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(cyl));
1311  _loadVertices(mesh.get(), cgeometry);
1312  break;
1313  }
1314  default: {
1315  throw ColladaUrdfException(str(boost::format("undefined geometry type %d, name %s")%(int)geometry->type%geometry_id));
1316  }
1317  }
1318  return cgeometry;
1319  }
1320 
1321  void _WriteMaterial(const string& geometry_id, urdf::MaterialSharedPtr material)
1322  {
1323  string effid = geometry_id+string("_eff");
1324  string matid = geometry_id+string("_mat");
1325  domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
1326  pdommat->setId(matid.c_str());
1327  domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
1328  pdominsteff->setUrl((string("#")+effid).c_str());
1329 
1330  urdf::Color ambient, diffuse;
1331  ambient.init("0.1 0.1 0.1 0");
1332  diffuse.init("1 1 1 0");
1333 
1334  if( !!material ) {
1335  ambient.r = diffuse.r = material->color.r;
1336  ambient.g = diffuse.g = material->color.g;
1337  ambient.b = diffuse.b = material->color.b;
1338  ambient.a = diffuse.a = material->color.a;
1339  }
1340 
1341  domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse);
1342 
1343  // <material id="g1.link0.geom0.eff">
1344  domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
1345  string material_id = geometry_id + string("_mat");
1346  dommaterial->setId(material_id.c_str());
1347  {
1348  // <instance_effect url="#g1.link0.geom0.eff"/>
1349  domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
1350  string effect_id(effect->getId());
1351  instance_effect->setUrl((string("#") + effect_id).c_str());
1352  }
1353  // </material>
1354 
1355  domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse);
1356  }
1357 
1358  boost::shared_ptr<instance_physics_model_output> _WriteInstance_physics_model(int id, daeElementRef parent, const string& sidscope, const MAPLINKPOSES& maplinkposes)
1359  {
1360  boost::shared_ptr<physics_model_output> pmout = WritePhysics_model(id, maplinkposes);
1362  ipmout->pmout = pmout;
1363  ipmout->ipm = daeSafeCast<domInstance_physics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL));
1364  string bodyid = _ComputeId(str(boost::format("visual%d")%id));
1365  ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,string("#")+bodyid));
1366  string symscope, refscope;
1367  if( sidscope.size() > 0 ) {
1368  symscope = sidscope+string("_");
1369  refscope = sidscope+string("/");
1370  }
1371  string ipmsid = str(boost::format("%s_inst")%pmout->pmodel->getID());
1372  ipmout->ipm->setUrl(str(boost::format("#%s")%pmout->pmodel->getID()).c_str());
1373  ipmout->ipm->setSid(ipmsid.c_str());
1374 
1375  string kmodelid = _ComputeKinematics_modelId(id);
1376  for(size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) {
1377  domInstance_rigid_bodyRef pirb = daeSafeCast<domInstance_rigid_body>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY));
1378  pirb->setBody(pmout->vrigidbodysids[i].c_str());
1379  pirb->setTarget(xsAnyURI(*pirb,str(boost::format("#v%s_node%d")%kmodelid%i)));
1380  }
1381 
1382  return ipmout;
1383  }
1384 
1385  boost::shared_ptr<physics_model_output> WritePhysics_model(int id, const MAPLINKPOSES& maplinkposes)
1386  {
1388  pmout->pmodel = daeSafeCast<domPhysics_model>(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL));
1389  string pmodelid = str(boost::format("pmodel%d")%id);
1390  pmout->pmodel->setId(pmodelid.c_str());
1391  pmout->pmodel->setName(_robot.getName().c_str());
1392  FOREACHC(itlink,_robot.links_) {
1393  domRigid_bodyRef rigid_body = daeSafeCast<domRigid_body>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY));
1394  string rigidsid = str(boost::format("rigid%d")%_maplinkindices[itlink->second]);
1395  pmout->vrigidbodysids.push_back(rigidsid);
1396  rigid_body->setSid(rigidsid.c_str());
1397  rigid_body->setName(itlink->second->name.c_str());
1398  domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1399  urdf::InertialSharedPtr inertial = itlink->second->inertial;
1400  if( !!inertial ) {
1401  daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true));
1402  domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
1403  mass->setValue(inertial->mass);
1404  double fCovariance[9] = { inertial->ixx, inertial->ixy, inertial->ixz, inertial->ixy, inertial->iyy, inertial->iyz, inertial->ixz, inertial->iyz, inertial->izz};
1405  double eigenvalues[3], eigenvectors[9];
1406  mathextra::EigenSymmetric3(fCovariance,eigenvalues,eigenvectors);
1407  boost::array<double,12> minertiaframe;
1408  for(int j = 0; j < 3; ++j) {
1409  minertiaframe[4*0+j] = eigenvectors[3*j];
1410  minertiaframe[4*1+j] = eigenvectors[3*j+1];
1411  minertiaframe[4*2+j] = eigenvectors[3*j+2];
1412  }
1413  urdf::Pose tinertiaframe;
1414  tinertiaframe.rotation = _quatFromMatrix(minertiaframe);
1415  tinertiaframe = _poseMult(inertial->origin,tinertiaframe);
1416 
1417  domTargetable_float3Ref pinertia = daeSafeCast<domTargetable_float3>(ptec->add(COLLADA_ELEMENT_INERTIA));
1418  pinertia->getValue().setCount(3);
1419  pinertia->getValue()[0] = eigenvalues[0];
1420  pinertia->getValue()[1] = eigenvalues[1];
1421  pinertia->getValue()[2] = eigenvalues[2];
1422  urdf::Pose posemassframe = _poseMult(maplinkposes.find(itlink->second)->second,tinertiaframe);
1423  _WriteTransformation(ptec->add(COLLADA_ELEMENT_MASS_FRAME), posemassframe);
1424 
1425  // // create a shape for every geometry
1426  // int igeom = 0;
1427  // FOREACHC(itgeom, (*itlink)->GetGeometries()) {
1428  // domRigid_body::domTechnique_common::domShapeRef pdomshape = daeSafeCast<domRigid_body::domTechnique_common::domShape>(ptec->add(COLLADA_ELEMENT_SHAPE));
1429  // // there is a weird bug here where _WriteTranformation will fail to create rotate/translate elements in instance_geometry is created first... (is this part of the spec?)
1430  // _WriteTransformation(pdomshape,tbaseinv*(*itlink)->GetTransform()*itgeom->GetTransform());
1431  // domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pdomshape->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1432  // pinstgeom->setUrl(xsAnyURI(*pinstgeom,string("#")+_GetGeometryId(*itlink,igeom)));
1433  // ++igeom;
1434  // }
1435  }
1436  }
1437  return pmout;
1438  }
1439 
1440  void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom) {
1441 
1442  // convert the mesh into an STL binary (in memory)
1443  std::vector<char> buffer;
1444  shapes::writeSTLBinary(mesh, buffer);
1445 
1446  // Create an instance of the Importer class
1447  Assimp::Importer importer;
1448 
1449  // And have it read the given file with some postprocessing
1450  const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<const void*>(&buffer[0]), buffer.size(),
1451  aiProcess_Triangulate |
1452  aiProcess_JoinIdenticalVertices |
1453  aiProcess_SortByPType |
1454  aiProcess_OptimizeGraph |
1455  aiProcess_OptimizeMeshes, "stl");
1456 
1457  // Note: we do this mesh -> STL -> assimp mesh because the aiScene::aiScene symbol is hidden by default
1458 
1459  domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
1460  domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
1461  domAccessorRef pacc;
1462  domFloat_arrayRef parray;
1463  {
1464  pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
1465 
1466  parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
1467  parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
1468  parray->setDigits(6); // 6 decimal places
1469 
1470  domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1471  pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
1472  pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID())));
1473  pacc->setStride(3);
1474 
1475  domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1476  px->setName("X"); px->setType("float");
1477  domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1478  py->setName("Y"); py->setType("float");
1479  domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1480  pz->setName("Z"); pz->setType("float");
1481  }
1482  domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
1483  {
1484  pverts->setId("vertices");
1485  domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
1486  pvertinput->setSemantic("POSITION");
1487  pvertinput->setSource(domUrifragment(*pvertsource, std::string("#")+std::string(pvertsource->getID())));
1488  }
1489  _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1));
1490  pacc->setCount(parray->getCount());
1491  }
1492 
1493  void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale, urdf::Pose *org_trans)
1494  {
1495  const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate); //|aiProcess_GenNormals|aiProcess_GenUVCoords|aiProcess_FlipUVs);
1496  if( !scene ) {
1497  ROS_WARN("failed to load resource %s",filename.c_str());
1498  return;
1499  }
1500  if( !scene->mRootNode ) {
1501  ROS_WARN("resource %s has no data",filename.c_str());
1502  return;
1503  }
1504  if (!scene->HasMeshes()) {
1505  ROS_WARN_STREAM(str(boost::format("No meshes found in file %s")%filename));
1506  return;
1507  }
1508  domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
1509  domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
1510  domAccessorRef pacc;
1511  domFloat_arrayRef parray;
1512  {
1513  pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
1514 
1515  parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
1516  parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
1517  parray->setDigits(6); // 6 decimal places
1518 
1519  domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1520  pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
1521  pacc->setSource(xsAnyURI(*parray, string("#")+string(parray->getID())));
1522  pacc->setStride(3);
1523 
1524  domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1525  px->setName("X"); px->setType("float");
1526  domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1527  py->setName("Y"); py->setType("float");
1528  domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1529  pz->setName("Z"); pz->setType("float");
1530  }
1531  domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
1532  {
1533  pverts->setId("vertices");
1534  domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
1535  pvertinput->setSemantic("POSITION");
1536  pvertinput->setSource(domUrifragment(*pvertsource, string("#")+string(pvertsource->getID())));
1537  }
1538  _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale,org_trans);
1539  pacc->setCount(parray->getCount());
1540  }
1541 
1542  void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale, urdf::Pose *org_trans = NULL)
1543  {
1544  if( !node ) {
1545  return;
1546  }
1547  aiMatrix4x4 transform = node->mTransformation;
1548  aiNode *pnode = node->mParent;
1549  while (pnode) {
1550  // Don't convert to y-up orientation, which is what the root node in
1551  // Assimp does
1552  if (pnode->mParent != NULL) {
1553  transform = pnode->mTransformation * transform;
1554  }
1555  pnode = pnode->mParent;
1556  }
1557 
1558  {
1559  uint32_t vertexOffset = parray->getCount();
1560  uint32_t nTotalVertices=0;
1561  for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1562  aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1563  nTotalVertices += input_mesh->mNumVertices;
1564  }
1565 
1566  parray->getValue().grow(parray->getCount()+nTotalVertices*3);
1567  parray->setCount(parray->getCount()+nTotalVertices);
1568 
1569  for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1570  aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1571  for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) {
1572  aiVector3D p = input_mesh->mVertices[j];
1573  p *= transform;
1574  if (org_trans) {
1575  urdf::Vector3 vv;
1576  vv.x = p.x*scale.x;
1577  vv.y = p.y*scale.y;
1578  vv.z = p.z*scale.z;
1579  urdf::Vector3 nv = _poseMult(*org_trans, vv);
1580  parray->getValue().append(nv.x);
1581  parray->getValue().append(nv.y);
1582  parray->getValue().append(nv.z);
1583  }
1584  else {
1585  parray->getValue().append(p.x*scale.x);
1586  parray->getValue().append(p.y*scale.y);
1587  parray->getValue().append(p.z*scale.z);
1588  }
1589  }
1590  }
1591 
1592  // in order to save space, separate triangles from poly lists
1593 
1594  vector<int> triangleindices, otherindices;
1595  int nNumOtherPrimitives = 0;
1596  for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1597  aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1598  uint32_t indexCount = 0, otherIndexCount = 0;
1599  for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
1600  aiFace& face = input_mesh->mFaces[j];
1601  if( face.mNumIndices == 3 ) {
1602  indexCount += face.mNumIndices;
1603  }
1604  else {
1605  otherIndexCount += face.mNumIndices;
1606  }
1607  }
1608  triangleindices.reserve(triangleindices.size()+indexCount);
1609  otherindices.reserve(otherindices.size()+otherIndexCount);
1610  for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
1611  aiFace& face = input_mesh->mFaces[j];
1612  if( face.mNumIndices == 3 ) {
1613  triangleindices.push_back(vertexOffset+face.mIndices[0]);
1614  triangleindices.push_back(vertexOffset+face.mIndices[1]);
1615  triangleindices.push_back(vertexOffset+face.mIndices[2]);
1616  }
1617  else {
1618  for (uint32_t k = 0; k < face.mNumIndices; ++k) {
1619  otherindices.push_back(face.mIndices[k]+vertexOffset);
1620  }
1621  nNumOtherPrimitives++;
1622  }
1623  }
1624  vertexOffset += input_mesh->mNumVertices;
1625  }
1626 
1627  if( triangleindices.size() > 0 ) {
1628  domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
1629  ptris->setCount(triangleindices.size()/3);
1630  ptris->setMaterial("mat0");
1631  domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
1632  pvertoffset->setSemantic("VERTEX");
1633  pvertoffset->setOffset(0);
1634  pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
1635  domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
1636  pindices->getValue().setCount(triangleindices.size());
1637  for(size_t ind = 0; ind < triangleindices.size(); ++ind) {
1638  pindices->getValue()[ind] = triangleindices[ind];
1639  }
1640  }
1641 
1642  if( nNumOtherPrimitives > 0 ) {
1643  domPolylistRef ptris = daeSafeCast<domPolylist>(pdommesh->add(COLLADA_ELEMENT_POLYLIST));
1644  ptris->setCount(nNumOtherPrimitives);
1645  ptris->setMaterial("mat0");
1646  domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
1647  pvertoffset->setSemantic("VERTEX");
1648  pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
1649  domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
1650  pindices->getValue().setCount(otherindices.size());
1651  for(size_t ind = 0; ind < otherindices.size(); ++ind) {
1652  pindices->getValue()[ind] = otherindices[ind];
1653  }
1654 
1655  domPolylist::domVcountRef pcount = daeSafeCast<domPolylist::domVcount>(ptris->add(COLLADA_ELEMENT_VCOUNT));
1656  pcount->getValue().setCount(nNumOtherPrimitives);
1657  uint32_t offset = 0;
1658  for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1659  aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1660  for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
1661  aiFace& face = input_mesh->mFaces[j];
1662  if( face.mNumIndices > 3 ) {
1663  pcount->getValue()[offset++] = face.mNumIndices;
1664  }
1665  }
1666  }
1667  }
1668  }
1669 
1670  for (uint32_t i=0; i < node->mNumChildren; ++i) {
1671  _buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale,org_trans);
1672  }
1673  }
1674 
1675 
1676  domEffectRef _WriteEffect(std::string const& effect_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse)
1677  {
1678  // <effect id="g1.link0.geom0.eff">
1679  domEffectRef effect = daeSafeCast<domEffect>(_effectsLib->add(COLLADA_ELEMENT_EFFECT));
1680  effect->setId(effect_id.c_str());
1681  {
1682  // <profile_COMMON>
1683  domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->add(COLLADA_ELEMENT_PROFILE_COMMON));
1684  {
1685  // <technique sid="">
1686  domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->add(COLLADA_ELEMENT_TECHNIQUE));
1687  {
1688  // <phong>
1689  domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->add(COLLADA_ELEMENT_PHONG));
1690  {
1691  // <ambient>
1692  domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_AMBIENT));
1693  {
1694  // <color>r g b a
1695  domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->add(COLLADA_ELEMENT_COLOR));
1696  ambient_color->getValue().setCount(4);
1697  ambient_color->getValue()[0] = color_ambient.r;
1698  ambient_color->getValue()[1] = color_ambient.g;
1699  ambient_color->getValue()[2] = color_ambient.b;
1700  ambient_color->getValue()[3] = color_ambient.a;
1701  // </color>
1702  }
1703  // </ambient>
1704 
1705  // <diffuse>
1706  domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_DIFFUSE));
1707  {
1708  // <color>r g b a
1709  domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->add(COLLADA_ELEMENT_COLOR));
1710  diffuse_color->getValue().setCount(4);
1711  diffuse_color->getValue()[0] = color_diffuse.r;
1712  diffuse_color->getValue()[1] = color_diffuse.g;
1713  diffuse_color->getValue()[2] = color_diffuse.b;
1714  diffuse_color->getValue()[3] = color_diffuse.a;
1715  // </color>
1716  }
1717  // </diffuse>
1718  }
1719  // </phong>
1720  }
1721  // </technique>
1722  }
1723  // </profile_COMMON>
1724  }
1725  // </effect>
1726 
1727  return effect;
1728  }
1729 
1733  void _WriteTransformation(daeElementRef pelt, const urdf::Pose& t)
1734  {
1735  domRotateRef prot = daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0));
1736  domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0));
1737  ptrans->getValue().setCount(3);
1738  ptrans->getValue()[0] = t.position.x;
1739  ptrans->getValue()[1] = t.position.y;
1740  ptrans->getValue()[2] = t.position.z;
1741 
1742  prot->getValue().setCount(4);
1743  // extract axis from quaternion
1744  double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z;
1745  if( std::fabs(sinang) < 1e-10 ) {
1746  prot->getValue()[0] = 1;
1747  prot->getValue()[1] = 0;
1748  prot->getValue()[2] = 0;
1749  prot->getValue()[3] = 0;
1750  }
1751  else {
1752  urdf::Rotation quat;
1753  if( t.rotation.w < 0 ) {
1754  quat.x = -t.rotation.x;
1755  quat.y = -t.rotation.y;
1756  quat.z = -t.rotation.z;
1757  quat.w = -t.rotation.w;
1758  }
1759  else {
1760  quat = t.rotation;
1761  }
1762  sinang = std::sqrt(sinang);
1763  prot->getValue()[0] = quat.x/sinang;
1764  prot->getValue()[1] = quat.y/sinang;
1765  prot->getValue()[2] = quat.z/sinang;
1766  prot->getValue()[3] = angles::to_degrees(2.0*std::atan2(sinang,quat.w));
1767  }
1768  }
1769 
1770  // binding in instance_kinematics_scene
1772  {
1773  FOREACHC(it, _iasout->vkinematicsbindings) {
1774  domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
1775  pmodelbind->setNode(it->second.c_str());
1776  daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
1777  }
1778  FOREACHC(it, _iasout->vaxissids) {
1779  domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
1780  pjointbind->setTarget(it->jointnodesid.c_str());
1781  daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
1782  daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
1783  }
1784  }
1785 
1786 private:
1787  static urdf::Vector3 _poseMult(const urdf::Pose& p, const urdf::Vector3& v)
1788  {
1789  double ww = 2 * p.rotation.x * p.rotation.x;
1790  double wx = 2 * p.rotation.x * p.rotation.y;
1791  double wy = 2 * p.rotation.x * p.rotation.z;
1792  double wz = 2 * p.rotation.x * p.rotation.w;
1793  double xx = 2 * p.rotation.y * p.rotation.y;
1794  double xy = 2 * p.rotation.y * p.rotation.z;
1795  double xz = 2 * p.rotation.y * p.rotation.w;
1796  double yy = 2 * p.rotation.z * p.rotation.z;
1797  double yz = 2 * p.rotation.z * p.rotation.w;
1798  urdf::Vector3 vnew;
1799  vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
1800  vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
1801  vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
1802  return vnew;
1803  }
1804 
1805  static urdf::Pose _poseInverse(const urdf::Pose& p)
1806  {
1807  urdf::Pose pinv;
1808  pinv.rotation.x = -p.rotation.x;
1809  pinv.rotation.y = -p.rotation.y;
1810  pinv.rotation.z = -p.rotation.z;
1811  pinv.rotation.w = p.rotation.w;
1812  urdf::Vector3 t = _poseMult(pinv,p.position);
1813  pinv.position.x = -t.x;
1814  pinv.position.y = -t.y;
1815  pinv.position.z = -t.z;
1816  return pinv;
1817  }
1818 
1819  static urdf::Rotation _quatMult(const urdf::Rotation& quat0, const urdf::Rotation& quat1)
1820  {
1821  urdf::Rotation q;
1822  q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
1823  q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
1824  q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
1825  q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
1826  double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
1827  // don't touch the divides
1828  q.x /= fnorm;
1829  q.y /= fnorm;
1830  q.z /= fnorm;
1831  q.w /= fnorm;
1832  return q;
1833  }
1834 
1835  static urdf::Pose _poseMult(const urdf::Pose& p0, const urdf::Pose& p1)
1836  {
1837  urdf::Pose p;
1838  p.position = _poseMult(p0,p1.position);
1839  p.rotation = _quatMult(p0.rotation,p1.rotation);
1840  return p;
1841  }
1842 
1843  static urdf::Rotation _quatFromMatrix(const boost::array<double,12>& mat)
1844  {
1845  urdf::Rotation rot;
1846  double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
1847  if (tr >= 0) {
1848  rot.w = tr + 1;
1849  rot.x = (mat[4*2+1] - mat[4*1+2]);
1850  rot.y = (mat[4*0+2] - mat[4*2+0]);
1851  rot.z = (mat[4*1+0] - mat[4*0+1]);
1852  }
1853  else {
1854  // find the largest diagonal element and jump to the appropriate case
1855  if (mat[4*1+1] > mat[4*0+0]) {
1856  if (mat[4*2+2] > mat[4*1+1]) {
1857  rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
1858  rot.x = (mat[4*2+0] + mat[4*0+2]);
1859  rot.y = (mat[4*1+2] + mat[4*2+1]);
1860  rot.w = (mat[4*1+0] - mat[4*0+1]);
1861  }
1862  else {
1863  rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
1864  rot.z = (mat[4*1+2] + mat[4*2+1]);
1865  rot.x = (mat[4*0+1] + mat[4*1+0]);
1866  rot.w = (mat[4*0+2] - mat[4*2+0]);
1867  }
1868  }
1869  else if (mat[4*2+2] > mat[4*0+0]) {
1870  rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
1871  rot.x = (mat[4*2+0] + mat[4*0+2]);
1872  rot.y = (mat[4*1+2] + mat[4*2+1]);
1873  rot.w = (mat[4*1+0] - mat[4*0+1]);
1874  }
1875  else {
1876  rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
1877  rot.y = (mat[4*0+1] + mat[4*1+0]);
1878  rot.z = (mat[4*2+0] + mat[4*0+2]);
1879  rot.w = (mat[4*2+1] - mat[4*1+2]);
1880  }
1881  }
1882  double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
1883  // don't touch the divides
1884  rot.x /= fnorm;
1885  rot.y /= fnorm;
1886  rot.z /= fnorm;
1887  rot.w /= fnorm;
1888  return rot;
1889  }
1890 
1891  static std::string _ComputeKinematics_modelId(int id)
1892  {
1893  return _ComputeId(str(boost::format("kmodel%d")%id));
1894  }
1895 
1897  static std::string _ComputeId(const std::string& name)
1898  {
1899  std::string newname = name;
1900  for(size_t i = 0; i < newname.size(); ++i) {
1901  if( newname[i] == '/' || newname[i] == ' ' || newname[i] == '.' ) {
1902  newname[i] = '_';
1903  }
1904  }
1905  return newname;
1906  }
1907 
1909 
1912  domCOLLADA* _dom;
1913  daeDocument *_doc;
1914  domCOLLADA::domSceneRef _globalscene;
1915 
1916  domLibrary_visual_scenesRef _visualScenesLib;
1917  domLibrary_kinematics_scenesRef _kinematicsScenesLib;
1918  domLibrary_kinematics_modelsRef _kinematicsModelsLib;
1919  domLibrary_articulated_systemsRef _articulatedSystemsLib;
1920  domLibrary_physics_scenesRef _physicsScenesLib;
1921  domLibrary_physics_modelsRef _physicsModelsLib;
1922  domLibrary_materialsRef _materialsLib;
1923  domLibrary_effectsRef _effectsLib;
1924  domLibrary_geometriesRef _geometriesLib;
1925  domTechniqueRef _sensorsLib;
1927 
1930  std::map< urdf::JointConstSharedPtr, int > _mapjointindices;
1931  std::map< urdf::LinkConstSharedPtr, int > _maplinkindices;
1932  std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices;
1933  Assimp::Importer _importer;
1934 };
1935 
1936 
1937 ColladaUrdfException::ColladaUrdfException(std::string const& what)
1938  : std::runtime_error(what)
1939 {
1940 }
1941 
1942 bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) {
1943  ColladaWriter writer(robot_model, 0);
1944  if ( ! writer.convert() ) {
1945  std::cerr << std::endl << "Error converting document" << std::endl;
1946  return -1;
1947  }
1948  return writer.writeTo(file);
1949 }
1950 
1951 }
d
domInstance_with_extraRef piscene
static urdf::Vector3 _poseMult(const urdf::Pose &p, const urdf::Vector3 &v)
std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices
axis_sids(const string &axissid, const string &valuesid, const string &jointnodesid)
std::map< urdf::JointConstSharedPtr, int > _mapjointindices
bool QLAlgorithm3(double *m_aafEntry, double *afDiag, double *afSubDiag)
boost::shared_ptr< instance_articulated_system_output > _iasout
ColladaWriter(const urdf::Model &robot, int writeoptions)
domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string &geometry_id, urdf::Pose *org_trans=NULL)
static urdf::Pose _poseMult(const urdf::Pose &p0, const urdf::Pose &p1)
domLibrary_effectsRef _effectsLib
domLibrary_kinematics_modelsRef _kinematicsModelsLib
f
static std::string _ComputeId(const std::string &name)
computes a collada-compliant sid from the urdf name
static urdf::Rotation _quatMult(const urdf::Rotation &quat0, const urdf::Rotation &quat1)
virtual boost::shared_ptr< kinematics_model_output > WriteKinematics_model(int id)
#define floatRoot12
Assimp::IOStream * Open(const char *file, const char *mode)
domInstance_kinematics_sceneRef kiscene
domCOLLADA::domSceneRef _globalscene
std::vector< std::string > vrigidbodysids
same ordering as the physics indices
list< pair< int, string > > listprocesseddofs
boost::shared_ptr< kinematics_model_output > kmout
Implements writing urdf::Model objects to a COLLADA DOM.
boost::shared_ptr< kinematics_model_output > kmout
boost::shared_ptr< instance_physics_model_output > _WriteInstance_physics_model(int id, daeElementRef parent, const string &sidscope, const MAPLINKPOSES &maplinkposes)
domLibrary_articulated_systemsRef _articulatedSystemsLib
void _loadMesh(std::string const &filename, domGeometryRef pdomgeom, const urdf::Vector3 &scale, urdf::Pose *org_trans)
ResourceIOStream(const resource_retriever::MemoryResource &res)
domInstance_with_extraRef viscene
#define M_PI
std::vector< std::pair< std::string, std::string > > vkinematicsbindings
#define ROS_WARN(...)
ResourceIOStream is copied from rviz (BSD, Willow Garage)
domLibrary_kinematics_scenesRef _kinematicsScenesLib
domTechniqueRef _sensorsLib
custom sensors library
boost::shared_ptr< instance_kinematics_model_output > _ikmout
def get(url)
#define FOREACHC
std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES
domLibrary_visual_scenesRef _visualScenesLib
resource_retriever::MemoryResource res_
static std::string _ComputeKinematics_modelId(int id)
unsigned int index
domLibrary_materialsRef _materialsLib
Triangle(const urdf::Vector3 &_p1, const urdf::Vector3 &_p2, const urdf::Vector3 &_p3)
list< pair< int, string > > listusedlinks
boost::shared_ptr< physics_model_output > pmout
ResourceIOSystem is copied from rviz (BSD, Willow Garage)
domLibrary_physics_modelsRef _physicsModelsLib
domLibrary_geometriesRef _geometriesLib
boost::shared_ptr< physics_model_output > pmout
virtual void handleError(daeString msg)
static double to_degrees(double radians)
domLibrary_physics_scenesRef _physicsScenesLib
bool WriteUrdfModelToColladaFile(urdf::Model const &robot_model, std::string const &file)
#define FOREACH(it, v)
#define floatRoot01
domEffectRef _WriteEffect(std::string const &effect_id, urdf::Color const &color_ambient, urdf::Color const &color_diffuse)
#define ROS_WARN_STREAM(args)
const urdf::Model & _robot
#define ROS_DEBUG_STREAM(args)
bool writeTo(string const &file)
#define singleRoot
void Tridiagonal3(S *mat, T *diag, T *subd)
boost::shared_ptr< physics_model_output > WritePhysics_model(int id, const MAPLINKPOSES &maplinkposes)
void _WriteRobot(int id=0)
Write kinematic body in a given scene.
size_t Read(void *buffer, size_t size, size_t count)
void _WriteTransformation(daeElementRef pelt, const urdf::Pose &t)
Write transformation.
std::map< urdf::LinkConstSharedPtr, int > _maplinkindices
void writeSTLBinary(const Mesh *mesh, std::vector< char > &buffer)
virtual void handleWarning(daeString msg)
static urdf::Pose _poseInverse(const urdf::Pose &p)
static urdf::Rotation _quatFromMatrix(const boost::array< double, 12 > &mat)
bool _QLAlgorithm3(T *m_aafEntry, T *afDiag, T *afSubDiag)
virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string &sidscope, int id)
Write kinematic body in a given scene.
#define ROS_ASSERT(cond)
std::vector< std::pair< std::string, std::string > > vkinematicsbindings
void EigenSymmetric3(const double *fmat, double *afEigenvalue, double *fevecs)
void _WriteMaterial(const string &geometry_id, urdf::MaterialSharedPtr material)
void _buildAiMesh(const aiScene *scene, aiNode *node, domMeshRef pdommesh, domFloat_arrayRef parray, const string &geomid, const urdf::Vector3 &scale, urdf::Pose *org_trans=NULL)
Mesh * createMeshFromShape(const Shape *shape)
#define ROS_BREAK()
resource_retriever::Retriever retriever_
void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom)
size_t Write(const void *buffer, size_t size, size_t count)
#define ROS_ERROR(...)
aiReturn Seek(size_t offset, aiOrigin origin)
#define distinctRoots
bool Exists(const char *file) const
int CubicRoots(double c0, double c1, double c2, double *r0, double *r1, double *r2)
void Close(Assimp::IOStream *stream)
virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string &strModelUri)
Write link of a kinematic body.


collada_urdf
Author(s): Tim Field, Rosen Diankov, Ioan Sucan , Jackie Kay
autogenerated on Wed Jul 15 2020 03:52:10