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 <tr1/unordered_map>
22 
23 using namespace tinyxml2;
24 
75 double correctAngle(double a);
76 
77 double correctAngle(double a)
78 {
79  double b=a;
80 
81  PI2PI(b);
82 
83  if (fabs(b+M_PI)<0.001)
84  b=-M_PI;
85  else
86  {
87  if (fabs(b-M_PI)<0.001)
88  b=+M_PI;
89  else
90  {
91  if (fabs(b+M_PI_2)<0.001)
92  b=-M_PI_2;
93  else
94  {
95  if (fabs(b-M_PI_2)<0.001)
96  b=+M_PI_2;
97  }
98  }
99  }
100  return(b);
101 }
102 
116 XMLElement *FindFirstElement(char *label,XMLNode **child,XMLNode *node);
117 
129 XMLElement *FindNextElement(char *label,XMLNode **child);
130 
131 XMLElement *FindFirstElement(char *label,XMLNode **child,XMLNode *node)
132 {
133  XMLElement *element;
134  bool found=false;
135 
136  *child=node->FirstChild();
137  while((!found)&&(*child))
138  {
139  element=(*child)->ToElement();
140  found=((element)&&(strcasecmp(element->Value(),label)==0));
141  if (!found)
142  *child=(*child)->NextSibling();
143  }
144  if (found)
145  return(element);
146  else
147  return(NULL);
148 }
149 
150 XMLElement *FindNextElement(char *label,XMLNode **child)
151 {
152  XMLElement *element;
153  bool found=false;
154 
155  *child=(*child)->NextSibling();
156  while((!found)&&(*child))
157  {
158  element=(*child)->ToElement();
159  found=((element)&&(strcasecmp(element->Value(),label)==0));
160  if (!found)
161  *child=(*child)->NextSibling();
162  }
163  if (found)
164  return(element);
165  else
166  return(NULL);
167 }
168 
190 int main(int argc, char **arg)
191 {
192  if (argc>2)
193  {
194  Tfilename furdf;
195  XMLDocument urdf;
196  char *fileName;
197  unsigned int lPath,lName;
198 
199  lPath=strlen(arg[1]);
200  if (arg[1][lPath]=='/')
201  {lPath--;arg[1][lPath]=0;}
202  lName=strlen(arg[2]);
203 
204  NEW(fileName,lPath+lName+2,char);
205  sprintf(fileName,"%s/%s",arg[1],arg[2]);
206  fileName[lPath+lName+1]=0;
207 
208  /*Read the problem from file*/
209  CreateFileName(NULL,fileName,NULL,(char *)URDF_EXT,&furdf);
210  fprintf(stderr,"Parsing URDF : %s\n",GetFileFullName(&furdf));
211 
212  if (urdf.LoadFile((const char*)GetFileFullName(&furdf))==XML_NO_ERROR)
213  {
214  XMLNode *robotNode;
215  XMLElement *robot;
216 
217  robot=FindFirstElement((char *)"robot",&robotNode,(XMLNode *)&urdf);
218  if (!robot)
219  Error("URDF without a robot definition?");
220  else
221  {
222  unsigned int i,l1,l2;
223  char *linkName,*jointName,*jointType,*jointParent,*jointChild;
224  XMLNode *linkNode,*jointNode,*partNode,*geomNode,*materialNode,*bodyNode,*colorNode,*parentNode,*childNode;
225  XMLElement *link,*joint,*part,*origin,*geom,*material,*body,*color,*parent,*child,*axis,*limit;
226  THTransform t,ts,tt;
227  Tcolor c;
228  Tpolyhedron p;
229  Tlink l;
230  Tjoint j;
231  Tworld w;
232  double *z,**point,**rangePoint,*a,*ap;
233  double ll,ul; /* joint limits */
234  Tinterval range;
235  bool haveBody;
236  /* materials (colors) are declared once and re-used */
237  std::tr1::unordered_map<std::string,Tcolor> materials;
238 
239  NEW(point,4,double*);
240  for(i=0;i<4;i++)
241  NEW(point[i],3,double);
242 
243  NEW(rangePoint,4,double*);
244  for(i=0;i<2;i++)
245  NEW(rangePoint[i],3,double);
246 
247  NEW(z,3,double);
248  NEW(a,3,double);
249  NEW(ap,3,double);
250 
251  z[0]=0;z[1]=0;z[2]=0;
252 
253  /* Init the world to define */
254  InitWorld(&w);
255 
256  /* First process the links */
257  printf("\n Processing links\n");
258  link=FindFirstElement((char *)"link",&linkNode,robotNode);
259  while(link)
260  {
261  linkName=(char *)link->Attribute("name");
262  printf(" Processing link: %s\n",linkName);
263 
264  InitLink(linkName,&l);
265 
266  for(i=0;i<2;i++)
267  {
268  if (i==0)
269  part=FindFirstElement((char *)"visual",&partNode,linkNode);
270  else
271  part=FindFirstElement((char *)"collision",&partNode,linkNode);
272  if (part)
273  {
274  origin=FindFirstElement((char *)"origin",&geomNode,partNode);
275  if (origin)
276  {
277  double x,y,z,a,b,c;
278  THTransform r;
279  char *tp;
280 
281  /* Get the translation parametres */
282  tp=(char *)origin->Attribute("xyz");
283  if (!tp)
284  Error("Origin without xyz attribute");
285  sscanf(tp,"%lf %lf %lf",&x,&y,&z);
286  HTransformTxyz(x,y,z,&t);
287 
288  tp=(char *)origin->Attribute("rpy");
289  if (!tp)
290  Error("Origin without rpy attribute");
291  sscanf(tp,"%lf %lf %lf",&a,&b,&c);
292  a=correctAngle(a);b=correctAngle(b);c=correctAngle(c);
293  HTransformYawPitchRoll(c,b,a,&r);
294 
295  HTransformProduct(&t,&r,&t);
296  }
297  else
298  HTransformIdentity(&t);
299 
300  /* In principle only the visual part has material. Assume just one color */
301  material=FindFirstElement((char *)"material",&materialNode,partNode);
302  if (material)
303  {
304  char *sc;
305  double r,g,b;
306  std::string materialName;
307 
308  materialName=material->Attribute("name");
309 
310  /* check if the material includes the color declaration */
311  color=FindFirstElement((char *)"color",&colorNode,materialNode);
312  if (color)
313  {
314  /* a new color (re)declaration */
315  sc=(char *)color->Attribute("rgba");
316  sscanf(sc,"%lf %lf %lf",&r,&g,&b);
317  NewColor(r,g,b,&c);
318  materials[materialName]=c; /* store the color for this material
319  just in case it is used latter on */
320  }
321  else
322  {
323  std::tr1::unordered_map<std::string,Tcolor>::const_iterator mat;
324 
325  /* check if we have a material previously stored */
326  mat=materials.find(materialName);
327  if (mat==materials.end())
328  NewColor(0.5,0.5,0.5,&c); /* unknown material/color -> used a default color */
329  else
330  c=mat->second; /* use the color previously defined */
331  }
332  }
333  else
334  NewColor(0.5,0.5,0.5,&c);
335 
336  /* Define a function with the code below and search also for spheres, cylinders and boxes */
337  /* The color (c) and transform (t) should be parameters of this function */
338  geom=FindFirstElement((char *)"geometry",&geomNode,partNode);
339  while (geom)
340  {
341  bodyNode=geomNode->FirstChild();
342  while(bodyNode)
343  {
344  haveBody=false;
345  body=bodyNode->ToElement();
346  if (body)
347  {
348  haveBody=true;
349 
350  HTransformIdentity(&ts);
351 
352  if (strcasecmp(body->Value(),(char*)"mesh")==0)
353  {
354  unsigned int k;
355  char *s;
356  double sx,sy,sz;
357  char *meshName,*fileMeshName;
358  Tfilename fmesh;
359 
360  meshName=(char *)body->Attribute("filename");
361 
362  HTransformIdentity(&ts);
363  s=(char *)body->Attribute("scale");
364  if (s)
365  {
366  sscanf(s,"%lf %lf %lf",&sx,&sy,&sz);
367 
368  HTransformSetElement(0,0,sx,&ts);
369  HTransformSetElement(1,1,sy,&ts);
370  HTransformSetElement(2,2,sz,&ts);
371  }
372 
373  /* Define the link with the collected information */
374  lName=strlen(meshName);
375  /* skip the 'package://' part of the mesh name */
376  k=0;
377  while ((i<lName)&&((meshName[k]!=':')||(meshName[k+1]!='/')||(meshName[k+2]!='/')))
378  k++;
379  if (k<lName) k+=3;
380  lName-=k;
381  /* and replace it by the package path as given by the user */
382 
383  NEW(fileMeshName,lPath+lName+2,char);
384  sprintf(fileMeshName,"%s/%s",arg[1],&(meshName[k]));
385  fileMeshName[lPath+lName+1]=0;
386 
387  CreateFileName(NULL,fileMeshName,NULL,NULL,&fmesh);
388  printf(" Reading mesh: %s\n",GetFileFullName(&fmesh));
389  InitPolyhedronFromFile(&fmesh,&c,10,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
390  DeleteFileName(&fmesh);
391  free(fileMeshName);
392  }
393  else
394  {
395  if (strcasecmp(body->Value(),(char*)"sphere")==0)
396  {
397  char *sr;
398  double r,z[3]={0,0,0};
399 
400  sr=(char *)body->Attribute("radius");
401  sscanf(sr,"%lf",&r);
402 
403  NewSphere(r,z,&c,10,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
404  }
405  else
406  {
407  if (strcasecmp(body->Value(),(char*)"cylinder")==0)
408  {
409  char *sr,*sl;
410  double r,l;
411  double p1[3]={0,0,0},p2[3]={0,0,0};
412 
413  sr=(char *)body->Attribute("radius");
414  sscanf(sr,"%lf",&r);
415  sl=(char *)body->Attribute("length");
416  sscanf(sl,"%lf",&l);
417 
418  /* z-aligned cylinder (zero centered). Note that our
419  cylinders are open.... might need to close them !!*/
420  p1[2]=-l/2;
421  p2[2]=l/2;
422 
423  NewCylinder(r,p1,p2,&c,10,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
424  }
425  else
426  {
427  if (strcasecmp(body->Value(),(char*)"box")==0)
428  {
429  char *ss;
430  double x,y,z;
431 
432  ss=(char *)body->Attribute("size");
433  sscanf(ss,"%lf %lf %lf",&x,&y,&z);
434 
435  NewBox(-x/2,-y/2,-z/2,x/2,y/2,z/2,&c,(i==0?DECOR_SHAPE:HIDDEN_SHAPE),&p);
436  }
437  else
438  /* unknow geometric object */
439  haveBody=false;
440  }
441  }
442  }
443  }
444  if (haveBody)
445  {
446  HTransformProduct(&t,&ts,&tt);
447  TransformPolyhedron(&tt,&p);
448 
449  AddBody2Link(&p,&l);
450  DeletePolyhedron(&p);
451  }
452 
453  bodyNode=bodyNode->NextSibling();
454  }
455  geom=FindNextElement((char *)"geometry",&partNode);
456  }
457  }
458  }
459 
460  AddLink2World(&l,FALSE,&w);
461  DeleteLink(&l);
462 
463  link=FindNextElement((char *)"link",&linkNode);
464  }
465  /* Now process the joints */
466  printf("\n Processing joints\n");
467  joint=FindFirstElement((char *)"joint",&jointNode,robotNode);
468  while(joint)
469  {
470  jointName=(char *)joint->Attribute("name");
471  jointType=(char *)joint->Attribute("type");
472 
473  parent=FindFirstElement((char *)"parent",&parentNode,jointNode);
474  jointParent=(char *)parent->Attribute("link");
475 
476  child=FindFirstElement((char *)"child",&childNode,jointNode);
477  jointChild=(char *)child->Attribute("link");
478 
479  l1=GetWorldLinkID(jointParent,&w);
480  if (l1==NO_UINT)
481  Error("Unkown parent link when defining a joint");
482 
483  l2=GetWorldLinkID(jointChild,&w);
484  if (l2==NO_UINT)
485  Error("Unkown child when defining joint");
486 
487  printf(" Processing joint: %s [%s] %s <-> %s\n",jointName,jointType,jointParent,jointChild);
488 
489  /* origin if provided */
490  origin=FindFirstElement((char *)"origin",&geomNode,jointNode);
491  if (origin)
492  {
493  double x,y,z,a,b,c;
494  THTransform r;
495  char *tp;
496 
497  /* Get the translation parametres */
498  tp=(char *)origin->Attribute("xyz");
499  if (!tp)
500  Error("Origin without xyz attribute");
501  sscanf(tp,"%lf %lf %lf",&x,&y,&z);
502  HTransformTxyz(x,y,z,&t);
503 
504  tp=(char *)origin->Attribute("rpy");
505  if (!tp)
506  Error("Origin without xyz attribute");
507  sscanf(tp,"%lf %lf %lf",&a,&b,&c);
508  a=correctAngle(a);b=correctAngle(b);c=correctAngle(c);
509  HTransformYawPitchRoll(c,b,a,&r);
510 
511  HTransformProduct(&t,&r,&t);
512  }
513  else
514  HTransformIdentity(&t);
515 
516  /* axis if provided */
517  axis=FindFirstElement((char *)"axis",&geomNode,jointNode);
518  if (axis)
519  {
520  char *ap;
521 
522  ap=(char *)axis->Attribute("xyz");
523  sscanf(ap,"%lf %lf %lf",&(a[0]),&(a[1]),&(a[2]));
524  }
525  else
526  { a[0]=1.0;a[1]=0.0;a[2]=0.0; }
527 
528  HTransformApply(z,point[0],&t);
529  HTransformApply(a,point[1],&t);
530  point[2][0]=point[2][1]=point[2][2]=0.0;
531  point[3][0]=a[0];point[3][1]=a[1];point[3][2]=a[2];
532 
533  DefineNormalVector(ap,a);
534  HTransformApply(ap,rangePoint[0],&t);
535  rangePoint[1][0]=ap[0];rangePoint[1][1]=ap[1];rangePoint[1][2]=ap[2];
536 
537  /* Limits if provided */
538  limit=FindFirstElement((char *)"limit",&geomNode,jointNode);
539  if (limit)
540  {
541  ll=atof((char *)limit->Attribute("lower"));
542  ul=atof((char *)limit->Attribute("upper"));
543  NewInterval(ll,ul,&range);
544  }
545  else
546  NewInterval(-INF,INF,&range);
547 
548  /* Now define the joint */
549  if (strcasecmp(jointType,(char *)"fixed")==0)
550  NewFixJoint(GetWorldNJoints(&w),
551  l1,GetWorldLink(l1,&w),
552  l2,GetWorldLink(l2,&w),
553  &t,&j);
554  else
555  {
556  if (strcasecmp(jointType,(char *)"revolute")==0)
557  NewRevoluteJoint(GetWorldNJoints(&w),REP_JOINTS,
558  l1,GetWorldLink(l1,&w),
559  l2,GetWorldLink(l2,&w),
560  (double **)point,TRUE,&range,(double **)rangePoint,
561  FALSE,FALSE,NULL,&j);
562  else
563  {
564  if (strcasecmp(jointType,(char *)"continuous")==0)
565  NewRevoluteJoint(GetWorldNJoints(&w),REP_JOINTS,
566  l1,GetWorldLink(l1,&w),
567  l2,GetWorldLink(l2,&w),
568  (double **)point,FALSE,&range,(double **)rangePoint,
569  FALSE,FALSE,NULL,&j);
570  else
571  {
572  if (strcasecmp(jointType,(char *)"prismatic")==0)
573  NewPrismaticJoint(GetWorldNJoints(&w),
574  l1,GetWorldLink(l1,&w),
575  l2,GetWorldLink(l2,&w),
576  (double **)point,&range,
577  FALSE,FALSE,&j);
578  else
579  {
580  if (strcasecmp(jointType,(char *)"floating")==0)
581  NewFreeJoint(GetWorldNJoints(&w),
582  l1,GetWorldLink(l1,&w),
583  l2,GetWorldLink(l2,&w),
584  &j);
585  else
586  Error("Undefined joint type");
587  }
588  }
589  }
590  }
591 
592  AddJoint2World(&j,&w);
593  DeleteJoint(&j);
594 
595  DeleteInterval(&range);
596 
597  joint=FindNextElement((char *)"joint",&jointNode);
598  }
599  printf("\n");
600 
601  /* and print the world */
602  NoCheckAllCollisions(0,0,&w);
603  PrintWorld((argc>3?arg[3]:fileName),argc,arg,&w);
604 
605  /* Generate the ".joint" file? */
606  /* Generate the ".param" file? */
607 
608  DeleteWorld(&w);
609 
610  for(i=0;i<4;i++)
611  free(point[i]);
612  free(point);
613 
614  for(i=0;i<2;i++)
615  free(rangePoint[i]);
616  free(rangePoint);
617 
618  free(z);
619  free(a);
620  free(ap);
621  }
622  free(fileName);
623  }
624  else
625  Error("Unable to parse the XML in the URDF file");
626 
627  DeleteFileName(&furdf);
628  }
629  else
630  {
631  fprintf(stderr," Wrong number of parameters.\n");
632  fprintf(stderr," Use:\n");
633  fprintf(stderr," cuikurdf2world <package path> <problem filename>.urdf [<output>.world]\n");
634  fprintf(stderr," where <package path> is folder with the ROS package files (relative to the current directory).\n");
635  fprintf(stderr," <problem filename> is the URDF file to read (relative to the package)\n");
636  fprintf(stderr," <ouput> is the optional name for the world file to create (relative to the current directory).\n");
637  fprintf(stderr," (the '.urdf' extension is not required)\n");
638  }
639  return(EXIT_SUCCESS);
640 }
641 
REP_JOINTS
#define REP_JOINTS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:60
FALSE
#define FALSE
FALSE.
Definition: boolean.h:30
HTransformApply
void HTransformApply(double *p_in, double *p_out, THTransform *t)
Multiply a homogeneous transform and a vector.
Definition: htransform.c:728
HTransformTxyz
void HTransformTxyz(double tx, double ty, double tz, THTransform *t)
Constructor.
Definition: htransform.c:140
PrintWorld
void PrintWorld(char *fname, int argc, char **arg, Tworld *w)
Prints the world.
Definition: world.c:2742
Tjoint
Relation between two links.
Definition: joint.h:177
NEW
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
Tfilename
Data structure to hold the information about the name of a file.
Definition: filename.h:248
THTransform
A homgeneous transform in R^3.
PI2PI
#define PI2PI(a)
Forces an angle go be in [-pi,pi].
Definition: defines.h:205
filename.h
Definition of the Tfilename type and the associated functions.
TRUE
#define TRUE
TRUE.
Definition: boolean.h:21
NewSphere
void NewSphere(double r, double *center, Tcolor *c, unsigned int gr, unsigned int bs, Tpolyhedron *p)
Constructor.
Definition: polyhedron.c:926
Error
void Error(const char *s)
General error function.
Definition: error.c:80
HTransformYawPitchRoll
void HTransformYawPitchRoll(double a, double b, double c, THTransform *t)
Defines a rotation matrix from the yaw, pitch, and roll parameters.
Definition: htransform.c:590
Tworld
All the necessary information to generate equations for mechanisms.
Definition: world.h:124
Tcolor
A color.
Definition: color.h:23
NewBox
void NewBox(double xl, double yl, double zl, double xu, double yu, double zu, Tcolor *c, unsigned int bs, Tpolyhedron *p)
Constructor.
Definition: polyhedron.c:857
NoCheckAllCollisions
void NoCheckAllCollisions(unsigned int fl, unsigned int fo, Tworld *w)
Desactivates all the possible collision between links and links and obstacles.
Definition: world.c:1637
HTransformProduct
CBLAS_INLINE void HTransformProduct(THTransform *t1, THTransform *t2, THTransform *t3)
Product of two homogeneous transforms.
Definition: htransform.c:404
world.h
Definition of the Tworld type and the associated functions.
DeleteWorld
void DeleteWorld(Tworld *w)
Destructor.
Definition: world.c:2792
Tpolyhedron
A polyhedron.
Definition: polyhedron.h:124
error.h
Error and warning functions.
DeleteFileName
void DeleteFileName(Tfilename *fn)
Destructor.
Definition: filename.c:205
NewFreeJoint
void NewFreeJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
Constructor.
Definition: joint.c:101
FindNextElement
XMLElement * FindNextElement(char *label, XMLNode **child)
Continues a search over the set of child nodes.
Definition: cuikurdf2world.cpp:150
AddLink2World
unsigned int AddLink2World(Tlink *l, boolean object, Tworld *w)
Adds a link to the mechanism in the world.
Definition: world.c:1333
defines.h
Definitions of constants and macros used in several parts of the cuik library.
M_PI_2
#define M_PI_2
Pi/2.
Definition: defines.h:92
joint.h
Definition of the Tjoint type and the associated functions.
NewRevoluteJoint
void NewRevoluteJoint(unsigned int id, unsigned int r, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, boolean hasLimits, Tinterval *range, double **rPoints, boolean avoidLimits, double avoidLimitsWeight, Tjoint *coupled, Tjoint *j)
Constructor.
Definition: joint.c:132
DeleteInterval
void DeleteInterval(Tinterval *i)
Destructor.
Definition: interval.c:1016
cuiksystem.h
Definition of the TCuikSystem type and the associated functions.
CreateFileName
void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn)
Constructor.
Definition: filename.c:22
NewCylinder
void NewCylinder(double r, double *p1, double *p2, Tcolor *c, unsigned int gr, unsigned int bs, Tpolyhedron *p)
Constructor.
Definition: polyhedron.c:950
htransform.h
Definition of the THTransform type and the associated functions.
AddJoint2World
unsigned int AddJoint2World(Tjoint *j, Tworld *w)
Adds a joint to the mechanism in the world.
Definition: world.c:1383
InitPolyhedronFromFile
void InitPolyhedronFromFile(Tfilename *fname, Tcolor *c, unsigned int gr, unsigned int bs, Tpolyhedron *p)
Constructor.
Definition: polyhedron.c:727
NewFixJoint
void NewFixJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, THTransform *t, Tjoint *j)
Constructor.
Definition: joint.c:110
GetFileFullName
char * GetFileFullName(Tfilename *fn)
Gets the file full name (paht+name+extension).
Definition: filename.c:151
M_PI
#define M_PI
Pi.
Definition: defines.h:83
FindFirstElement
XMLElement * FindFirstElement(char *label, XMLNode **child, XMLNode *node)
Finds the first element with a particular label in the list of child nodes.
Definition: cuikurdf2world.cpp:131
polyhedron.h
Definition of the Tpolyhedron type and the associated functions.
color.h
Definition of the Tcolor type and the associated functions.
NO_UINT
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
GetWorldLinkID
unsigned int GetWorldLinkID(char *linkName, Tworld *w)
Gets the identifier of a link from its name.
Definition: world.c:1437
correctAngle
double correctAngle(double a)
Correct the angles given in the URDF.
Definition: cuikurdf2world.cpp:77
GetWorldLink
Tlink * GetWorldLink(unsigned int linkID, Tworld *w)
Gets a link from its identifier.
Definition: world.c:1447
GetWorldNJoints
unsigned int GetWorldNJoints(Tworld *w)
Gets the number of joints in the mechanism included in the world.
Definition: world.c:1462
InitWorld
void InitWorld(Tworld *w)
Constructor.
Definition: world.c:1308
DeletePolyhedron
void DeletePolyhedron(Tpolyhedron *p)
Destructor.
Definition: polyhedron.c:1484
NewInterval
void NewInterval(double lower, double upper, Tinterval *i)
Constructor.
Definition: interval.c:47
HTransformSetElement
void HTransformSetElement(unsigned int i, unsigned int j, double v, THTransform *t)
Sets an element in a homogeneous transform.
Definition: htransform.c:306
INF
#define INF
Infinite.
Definition: defines.h:70
DefineNormalVector
void DefineNormalVector(double *v, double *n)
Defines a unitary vector normal to a given vector.
Definition: geom.c:571
URDF_EXT
#define URDF_EXT
File extension for URDFs.
Definition: filename.h:124
TransformPolyhedron
void TransformPolyhedron(THTransform *t, Tpolyhedron *p)
Applies a homogenoeus transform to a polyhedron.
Definition: polyhedron.c:1113
NewColor
void NewColor(double r, double g, double b, Tcolor *c)
Constructor.
Definition: color.c:14
Tinterval
Defines a interval.
Definition: interval.h:33
parameters.h
Definition of the Tparameters type and the associated functions.
HTransformIdentity
void HTransformIdentity(THTransform *t)
Constructor.
Definition: htransform.c:69
DeleteJoint
void DeleteJoint(Tjoint *j)
Destructor.
Definition: joint.c:3461
main
int main(int argc, char **arg)
Main body of the cuikurdf2world application.
Definition: cuikurdf2world.cpp:190
NewPrismaticJoint
void NewPrismaticJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tinterval *range, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
Constructor.
Definition: joint.c:541
DECOR_SHAPE
#define DECOR_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:46
HIDDEN_SHAPE
#define HIDDEN_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:37