cuikurdf2world.cpp
Go to the documentation of this file.
1 extern "C" {
2  #include "cuiksystem.h"
3  #include "parameters.h"
4 
5  #include "defines.h"
6  #include "error.h"
7  #include "filename.h"
8  #include "htransform.h"
9  #include "polyhedron.h"
10  #include "color.h"
11  #include "link.h"
12  #include "world.h"
13  #include "joint.h"
14 }
15 
16 #include <stdlib.h>
17 #include <math.h>
18 #include <tinyxml2.h>
19 #include <strings.h>
20 #include <string>
21 #include <unordered_map>
22 
23 using namespace tinyxml2;
24 
75 double correctAngle(double a);
76 
88 void GetTransform(THTransform *t,bool err,XMLElement *element);
89 
103 XMLElement *FindFirstElement(char *label,XMLNode **child,XMLNode *node);
104 
116 XMLElement *FindNextElement(char *label,XMLNode **child);
117 
118 
119 /***********************************************************************************/
120 
121 double correctAngle(double a)
122 {
123  double b=a;
124 
125  PI2PI(b);
126 
127  if (fabs(b+M_PI)<0.001)
128  b=-M_PI;
129  else
130  {
131  if (fabs(b-M_PI)<0.001)
132  b=+M_PI;
133  else
134  {
135  if (fabs(b+M_PI_2)<0.001)
136  b=-M_PI_2;
137  else
138  {
139  if (fabs(b-M_PI_2)<0.001)
140  b=+M_PI_2;
141  }
142  }
143  }
144  return(b);
145 }
146 
147 void GetTransform(THTransform *t,bool err,XMLElement *element)
148 {
149  double x,y,z,a,b,c;
150  THTransform r;
151  char *tp;
152 
153  /* Get the translation parametres */
154  tp=(char *)element->Attribute("xyz");
155  if (tp)
156  {
157  sscanf(tp,"%lf %lf %lf",&x,&y,&z);
158  HTransformTxyz(x,y,z,t);
159  }
160  else
161  {
162  if (err)
163  Error("Origin without xyz attribute");
164  else
165  HTransformIdentity(t);
166  }
167 
168  /* get the rotation parameters */
169  tp=(char *)element->Attribute("rpy");
170  if (tp)
171  {
172  sscanf(tp,"%lf %lf %lf",&a,&b,&c);
173  a=correctAngle(a);b=correctAngle(b);c=correctAngle(c);
174  HTransformYawPitchRoll(c,b,a,&r);
175  HTransformProduct(t,&r,t);
176  }
177  else
178  {
179  if (err)
180  Error("Origin without rpy attribute");
181  }
182 }
183 
184 XMLElement *FindFirstElement(char *label,XMLNode **child,XMLNode *node)
185 {
186  XMLElement *element;
187  bool found=false;
188 
189  *child=node->FirstChild();
190  while((!found)&&(*child))
191  {
192  element=(*child)->ToElement();
193  found=((element)&&(strcasecmp(element->Value(),label)==0));
194  if (!found)
195  *child=(*child)->NextSibling();
196  }
197  if (found)
198  return(element);
199  else
200  return(NULL);
201 }
202 
203 XMLElement *FindNextElement(char *label,XMLNode **child)
204 {
205  XMLElement *element;
206  bool found=false;
207 
208  *child=(*child)->NextSibling();
209  while((!found)&&(*child))
210  {
211  element=(*child)->ToElement();
212  found=((element)&&(strcasecmp(element->Value(),label)==0));
213  if (!found)
214  *child=(*child)->NextSibling();
215  }
216  if (found)
217  return(element);
218  else
219  return(NULL);
220 }
221 
222 /***********************************************************************************/
223 
245 int main(int argc, char **arg)
246 {
247  if (argc>2)
248  {
249  Tfilename furdf;
250  XMLDocument urdf;
251  char *fileName;
252  unsigned int lPath,lName;
253 
254  lPath=strlen(arg[1]);
255  if (arg[1][lPath]=='/')
256  {lPath--;arg[1][lPath]=0;}
257  lName=strlen(arg[2]);
258 
259  NEW(fileName,lPath+lName+2,char);
260  sprintf(fileName,"%s/%s",arg[1],arg[2]);
261  fileName[lPath+lName+1]=0;
262 
263  /*Read the problem from file*/
264  CreateFileName(NULL,fileName,NULL,(char *)URDF_EXT,&furdf);
265  fprintf(stderr,"Parsing URDF : %s\n",GetFileFullName(&furdf));
266 
267  if (urdf.LoadFile((const char*)GetFileFullName(&furdf))==XML_SUCCESS)
268  {
269  XMLNode *robotNode;
270  XMLElement *robot;
271 
272  robot=FindFirstElement((char *)"robot",&robotNode,(XMLNode *)&urdf);
273  if (!robot)
274  Error("URDF without a robot definition?");
275  else
276  {
277  unsigned int i,l1,l2;
278  char *linkName,*jointName,*jointType,*jointParent,*jointChild;
279  XMLNode *linkNode,*jointNode,*partNode,*geomNode,*materialNode,*bodyNode,*colorNode,*parentNode,*childNode,*inertiaNode;
280  XMLElement *link,*joint,*part,*origin,*geom,*material,*body,*color,*parent,*child,*axis,*limit,*dynamicInfo,*mass,*inertia;
281  THTransform t,ts,tt;
282  Tcolor c;
283  Tpolyhedron p;
284  Tlink l;
285  Tjoint j;
286  Tworld w;
287  double *z,**point,**rangePoint,*a,*ap,**iMatrix;
288  double ll,ul; /* joint limits */
289  Tinterval range;
290  bool haveBody;
291  THTransform iFrame;
292  double mss;
293  double friction,damping,velocity,effort;
294  /* materials (colors) are declared once and re-used */
295  std::unordered_map<std::string,Tcolor> materials;
296 
297  NEW(point,4,double*);
298  for(i=0;i<4;i++)
299  NEW(point[i],3,double);
300 
301  NEW(rangePoint,4,double*);
302  for(i=0;i<2;i++)
303  NEW(rangePoint[i],3,double);
304 
305  NEW(z,3,double);
306  NEW(a,3,double);
307  NEW(ap,3,double);
308 
309  NEW(iMatrix,3,double*);
310  for(i=0;i<3;i++)
311  NEW(iMatrix[i],3,double);
312 
313  z[0]=0;z[1]=0;z[2]=0;
314 
315  /* Init the world to define */
316  InitWorld(&w);
317 
318  /* First process the links */
319  printf("\n Processing links\n");
320  link=FindFirstElement((char *)"link",&linkNode,robotNode);
321  while(link)
322  {
323  linkName=(char *)link->Attribute("name");
324  printf(" Processing link: %s\n",linkName);
325 
326  dynamicInfo=FindFirstElement((char *)"inertial",&partNode,linkNode);
327  if (dynamicInfo)
328  {
329  origin=FindFirstElement((char *)"origin",&inertiaNode,dynamicInfo);
330  if (origin)
331  GetTransform(&iFrame,false,origin);
332  else
333  HTransformIdentity(&iFrame);
334 
335  mass=FindFirstElement((char *)"mass",&inertiaNode,dynamicInfo);
336  if (mass)
337  mss=atof(mass->Attribute("value"));
338  else
339  mss=0.0;
340 
341  inertia=FindFirstElement((char *)"inertia",&inertiaNode,dynamicInfo);
342  if (inertia)
343  {
344  double ixx,ixy,ixz,iyy,iyz,izz;
345 
346  ixx=atof(inertia->Attribute("ixx"));
347  ixy=atof(inertia->Attribute("ixy"));
348  ixz=atof(inertia->Attribute("ixz"));
349  iyy=atof(inertia->Attribute("iyy"));
350  iyz=atof(inertia->Attribute("iyz"));
351  izz=atof(inertia->Attribute("izz"));
352 
353  iMatrix[0][0]=ixx;iMatrix[0][1]=ixy;iMatrix[0][2]=ixz;
354  iMatrix[1][0]=ixy;iMatrix[1][1]=iyy;iMatrix[1][2]=iyz;
355  iMatrix[2][0]=ixz;iMatrix[2][1]=iyz;iMatrix[2][2]=izz;
356  }
357  else
358  {
359  iMatrix[0][0]=0.0;iMatrix[0][1]=0.0;iMatrix[0][2]=0.0;
360  iMatrix[1][0]=0.0;iMatrix[1][1]=0.0;iMatrix[1][2]=0.0;
361  iMatrix[2][0]=0.0;iMatrix[2][1]=0.0;iMatrix[2][2]=0.0;
362  }
363 
364  InitLink(linkName,mss,iMatrix,&iFrame,&l);
365  }
366  else
367  InitLink(linkName,0.0,NULL,NULL,&l);
368 
369  for(i=0;i<2;i++)
370  {
371  if (i==0)
372  part=FindFirstElement((char *)"visual",&partNode,linkNode);
373  else
374  part=FindFirstElement((char *)"collision",&partNode,linkNode);
375  if (part)
376  {
377  /* origin of the visual/collision: to be applied to all the bodies */
378  origin=FindFirstElement((char *)"origin",&geomNode,partNode);
379  if (origin)
380  GetTransform(&t,true,origin);
381  else
382  HTransformIdentity(&t);
383 
384  /* Color: to be applied to all the bodies */
385  material=FindFirstElement((char *)"material",&materialNode,partNode);
386  if (material)
387  {
388  char *sc;
389  double r,g,b;
390  std::string materialName;
391 
392  materialName=material->Attribute("name");
393 
394  /* check if the material includes the color declaration */
395  color=FindFirstElement((char *)"color",&colorNode,materialNode);
396  if (color)
397  {
398  /* a new color (re)declaration */
399  sc=(char *)color->Attribute("rgba");
400  sscanf(sc,"%lf %lf %lf",&r,&g,&b);
401  NewColor(r,g,b,&c);
402  materials[materialName]=c; /* store the color for this material
403  just in case it is used latter on */
404  }
405  else
406  {
407  std::unordered_map<std::string,Tcolor>::const_iterator mat;
408 
409  /* check if we have a material previously stored */
410  mat=materials.find(materialName);
411  if (mat==materials.end())
412  NewColor(0.5,0.5,0.5,&c); /* unknown material/color -> used a default color */
413  else
414  c=mat->second; /* use the color previously defined */
415  }
416  }
417  else
418  NewColor(0.5,0.5,0.5,&c); /* default color */
419 
420  /* Define a function with the code below and search also for spheres, cylinders and boxes */
421  /* The color (c) and transform (t) should be parameters of this function */
422  geom=FindFirstElement((char *)"geometry",&geomNode,partNode);
423  while (geom)
424  {
425  bodyNode=geomNode->FirstChild();
426  while(bodyNode)
427  {
428  haveBody=false;
429  body=bodyNode->ToElement();
430  if (body)
431  {
432  haveBody=true;
433 
434  HTransformIdentity(&ts);
435 
436  if (strcasecmp(body->Value(),(char*)"mesh")==0)
437  {
438  /* We have a mesh */
439 
440  unsigned int k;
441  char *s;
442  double sx,sy,sz;
443  char *meshName,*fileMeshName;
444  Tfilename fmesh;
445 
446  meshName=(char *)body->Attribute("filename");
447 
448  HTransformIdentity(&ts);
449  s=(char *)body->Attribute("scale");
450  if (s)
451  {
452  sscanf(s,"%lf %lf %lf",&sx,&sy,&sz);
453 
454  HTransformSetElement(0,0,sx,&ts);
455  HTransformSetElement(1,1,sy,&ts);
456  HTransformSetElement(2,2,sz,&ts);
457  }
458 
459  /* Define the link with the collected information */
460  lName=strlen(meshName);
461  /* skip the 'package://' part of the mesh name */
462  k=0;
463  while ((i<lName)&&((meshName[k]!=':')||(meshName[k+1]!='/')||(meshName[k+2]!='/')))
464  k++;
465  if (k<lName) k+=3;
466  lName-=k;
467  /* and replace it by the package path as given by the user */
468 
469  NEW(fileMeshName,lPath+lName+2,char);
470  sprintf(fileMeshName,"%s/%s",arg[1],&(meshName[k]));
471  fileMeshName[lPath+lName+1]=0;
472 
473  CreateFileName(NULL,fileMeshName,NULL,NULL,&fmesh);
474  printf(" Reading mesh: %s\n",GetFileFullName(&fmesh));
475  InitPolyhedronFromFile(&fmesh,&c,10,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
476  DeleteFileName(&fmesh);
477  free(fileMeshName);
478  }
479  else
480  {
481  if (strcasecmp(body->Value(),(char*)"sphere")==0)
482  {
483  char *sr;
484  double r,z[3]={0,0,0};
485 
486  sr=(char *)body->Attribute("radius");
487  sscanf(sr,"%lf",&r);
488 
489  NewSphere(r,z,&c,10,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
490  }
491  else
492  {
493  if (strcasecmp(body->Value(),(char*)"cylinder")==0)
494  {
495  char *sr,*sl;
496  double r,l;
497  double p1[3]={0,0,0},p2[3]={0,0,0};
498 
499  sr=(char *)body->Attribute("radius");
500  sscanf(sr,"%lf",&r);
501  sl=(char *)body->Attribute("length");
502  sscanf(sl,"%lf",&l);
503 
504  /* z-aligned cylinder (zero centered). Note that our
505  cylinders are open.... might need to close them !!*/
506  p1[2]=-l/2;
507  p2[2]=l/2;
508 
509  NewCylinder(r,p1,p2,&c,10,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
510  }
511  else
512  {
513  if (strcasecmp(body->Value(),(char*)"box")==0)
514  {
515  char *ss;
516  double x,y,z;
517 
518  ss=(char *)body->Attribute("size");
519  sscanf(ss,"%lf %lf %lf",&x,&y,&z);
520 
521  NewBox(-x/2,-y/2,-z/2,x/2,y/2,z/2,&c,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
522  }
523  else
524  /* unknow geometric object */
525  haveBody=false;
526  }
527  }
528  }
529  }
530  if (haveBody)
531  {
532  HTransformProduct(&t,&ts,&tt);
533  TransformPolyhedron(&tt,&p);
534 
535  AddBody2Link(&p,&l);
536  DeletePolyhedron(&p);
537  }
538 
539  bodyNode=bodyNode->NextSibling();
540  }
541  geom=FindNextElement((char *)"geometry",&partNode);
542  }
543  }
544  }
545 
546  AddLink2World(&l,FALSE,&w);
547  DeleteLink(&l);
548 
549  link=FindNextElement((char *)"link",&linkNode);
550  }
551  /* Now process the joints */
552  printf("\n Processing joints\n");
553  joint=FindFirstElement((char *)"joint",&jointNode,robotNode);
554  while(joint)
555  {
556  jointName=(char *)joint->Attribute("name");
557  jointType=(char *)joint->Attribute("type");
558 
559  parent=FindFirstElement((char *)"parent",&parentNode,jointNode);
560  jointParent=(char *)parent->Attribute("link");
561 
562  child=FindFirstElement((char *)"child",&childNode,jointNode);
563  jointChild=(char *)child->Attribute("link");
564 
565  l1=GetWorldLinkID(jointParent,&w);
566  if (l1==NO_UINT)
567  Error("Unkown parent link when defining a joint");
568 
569  l2=GetWorldLinkID(jointChild,&w);
570  if (l2==NO_UINT)
571  Error("Unkown child when defining joint");
572 
573  printf(" Processing joint: %s [%s] %s <-> %s\n",jointName,jointType,jointParent,jointChild);
574 
575  /* origin if provided */
576  origin=FindFirstElement((char *)"origin",&geomNode,jointNode);
577  if (origin)
578  GetTransform(&t,true,origin);
579  else
580  HTransformIdentity(&t);
581 
582  /* axis if provided */
583  axis=FindFirstElement((char *)"axis",&geomNode,jointNode);
584  if (axis)
585  {
586  char *ap;
587 
588  ap=(char *)axis->Attribute("xyz");
589  sscanf(ap,"%lf %lf %lf",&(a[0]),&(a[1]),&(a[2]));
590  }
591  else
592  { a[0]=1.0;a[1]=0.0;a[2]=0.0; }
593 
594  HTransformApply(z,point[0],&t);
595  HTransformApply(a,point[1],&t);
596  point[2][0]=point[2][1]=point[2][2]=0.0;
597  point[3][0]=a[0];point[3][1]=a[1];point[3][2]=a[2];
598 
599  DefineNormalVector(ap,a);
600  HTransformApply(ap,rangePoint[0],&t);
601  rangePoint[1][0]=ap[0];rangePoint[1][1]=ap[1];rangePoint[1][2]=ap[2];
602 
603  /* dynamics if provided */
604  friction=0.0;
605  damping=0.0;
606 
607  dynamicInfo=FindFirstElement((char *)"dynamics",&geomNode,jointNode);
608  if (dynamicInfo)
609  {
610  char *fr,*da;
611 
612  fr=(char *)dynamicInfo->Attribute("friction");
613  if (fr)
614  friction=atof(fr);
615 
616  da=(char *)dynamicInfo->Attribute("damping");
617  if (da)
618  damping=atof(da);
619  }
620 
621  /* Limits if provided */
622  velocity=INF;
623  effort=INF;
624 
625  limit=FindFirstElement((char *)"limit",&geomNode,jointNode);
626  if (limit)
627  {
628  char *ve,*ef;
629 
630  ll=atof((char *)limit->Attribute("lower"));
631  ul=atof((char *)limit->Attribute("upper"));
632  NewInterval(ll,ul,&range);
633 
634  ve=(char *)limit->Attribute("velocity");
635  if (ve)
636  velocity=atof(ve);
637 
638  ef=(char *)limit->Attribute("effort");
639  if (ef)
640  effort=atof(ef);
641  }
642  else
643  NewInterval(-INF,INF,&range);
644 
645  /* Now define the joint */
646  if (strcasecmp(jointType,(char *)"fixed")==0)
647  NewFixJoint(GetWorldNJoints(&w),
648  l1,GetWorldLink(l1,&w),
649  l2,GetWorldLink(l2,&w),
650  &t,&j);
651  else
652  {
653  if (strcasecmp(jointType,(char *)"revolute")==0)
654  NewRevoluteJoint(GetWorldNJoints(&w),REP_JOINTS,
655  l1,GetWorldLink(l1,&w),
656  l2,GetWorldLink(l2,&w),
657  (double **)point,TRUE,TRUE,&range,(double **)rangePoint,
658  FALSE,FALSE,NULL,
659  velocity,INF,effort,1.0,friction,damping,&j);
660  else
661  {
662  if (strcasecmp(jointType,(char *)"continuous")==0)
663  NewRevoluteJoint(GetWorldNJoints(&w),REP_JOINTS,
664  l1,GetWorldLink(l1,&w),
665  l2,GetWorldLink(l2,&w),
666  (double **)point,FALSE,TRUE,&range,(double **)rangePoint,
667  FALSE,FALSE,NULL,
668  velocity,INF,effort,1.0,friction,damping,&j);
669  else
670  {
671  if (strcasecmp(jointType,(char *)"prismatic")==0)
672  NewPrismaticJoint(GetWorldNJoints(&w),
673  l1,GetWorldLink(l1,&w),
674  l2,GetWorldLink(l2,&w),
675  (double **)point,&range,
676  FALSE,FALSE,
677  velocity,INF,effort,1.0,friction,damping,&j);
678  else
679  {
680  if (strcasecmp(jointType,(char *)"floating")==0)
681  NewFreeJoint(GetWorldNJoints(&w),
682  l1,GetWorldLink(l1,&w),
683  l2,GetWorldLink(l2,&w),
684  velocity,INF,effort,1.0,friction,damping,&j);
685  else
686  Error("Undefined joint type");
687  }
688  }
689  }
690  }
691 
692  AddJoint2World(&j,&w);
693  DeleteJoint(&j);
694 
695  DeleteInterval(&range);
696 
697  joint=FindNextElement((char *)"joint",&jointNode);
698  }
699  printf("\n");
700 
701  /* and print the world */
702  NoCheckAllCollisions(0,0,&w);
703  PrintWorld((argc>3?arg[3]:fileName),argc,arg,&w);
704 
705  /* Generate the ".joint" file? */
706  /* Generate the ".param" file? */
707 
708  DeleteWorld(&w);
709 
710  for(i=0;i<3;i++)
711  free(iMatrix[i]);
712  free(iMatrix);
713 
714  for(i=0;i<4;i++)
715  free(point[i]);
716  free(point);
717 
718  for(i=0;i<2;i++)
719  free(rangePoint[i]);
720  free(rangePoint);
721 
722  free(z);
723  free(a);
724  free(ap);
725  }
726  free(fileName);
727  }
728  else
729  Error("Unable to parse the XML in the URDF file");
730 
731  DeleteFileName(&furdf);
732  }
733  else
734  {
735  fprintf(stderr," Wrong number of parameters.\n");
736  fprintf(stderr," Use:\n");
737  fprintf(stderr," cuikurdf2world <package path> <problem filename>.urdf [<output>.world]\n");
738  fprintf(stderr," where <package path> is folder with the ROS package files (relative to the current directory).\n");
739  fprintf(stderr," <problem filename> is the URDF file to read (relative to the package)\n");
740  fprintf(stderr," <ouput> is the optional name for the world file to create (relative to the current directory).\n");
741  fprintf(stderr," (the '.urdf' extension is not required)\n");
742  }
743  return(EXIT_SUCCESS);
744 }
745