joint.c
Go to the documentation of this file.
1 #include "joint.h"
2 
3 #include "error.h"
4 #include "defines.h"
5 #include "algebra.h"
6 #include "varnames.h"
7 
8 #include <math.h>
9 #include <string.h>
10 
37 void InitJoint(unsigned int t,unsigned int id,
38  unsigned int linkID1,Tlink *link1,
39  unsigned int linkID2,Tlink *link2,
40  Tjoint *j);
41 
42 void InitJoint(unsigned int t,unsigned int id,
43  unsigned int linkID1,Tlink *link1,
44  unsigned int linkID2,Tlink *link2,
45  Tjoint *j)
46 {
47  unsigned int i,k;
48 
49  j->t=t;
50  j->id=id;
51 
52  if ((linkID1==linkID2)||(link1==link2))
53  Error("Setting up a joint on a single link");
54 
55  j->linkID[0]=linkID1;
56  j->linkID[1]=linkID2;
57  j->link[0]=link1;
58  j->link[1]=link2;
59 
60  for(i=0;i<6;i++)
61  {
62  for(k=0;k<3;k++)
63  j->points[i][k]=0.0;
64  }
65 
66  for(i=0;i<3;i++)
67  {
68  for(k=0;k<3;k++)
69  j->normals[i][k]=0.0;
70  }
71 
72  NewInterval(-INF,INF,&(j->normRange));
73 
74  j->hasLimits=FALSE;
75 
76  j->coupled=NULL;
77 
78  j->avoidLimits=FALSE;
79  j->avoidLimitsWeight=0.0;
80 
81  NewInterval(-INF,INF,&(j->range));
82  NewInterval(-INF,INF,&(j->range2));
83 
84  j->offset=0;
85  j->offset2=0;
86 
87  j->obj3d[0]=NO_UINT;
88  j->obj3d[1]=NO_UINT;
89 
90  j->maxCoord=0;
91 
92  j->rad=0;
93  j->length=0;
94 
95  NewColor(0,0,0,&(j->color));
96 
97  HTransformIdentity(&(j->preT));
98  HTransformIdentity(&(j->postT));
99 }
100 
101 void NewFreeJoint(unsigned int id,
102  unsigned int linkID1,Tlink *link1,
103  unsigned int linkID2,Tlink *link2,
104  Tjoint *j)
105 {
106  InitJoint(FREE_JOINT,id,linkID1,link1,linkID2,link2,j);
107  j->maxCoord=INF;
108 }
109 
110 void NewFixJoint(unsigned int id,
111  unsigned int linkID1,Tlink *link1,
112  unsigned int linkID2,Tlink *link2,
113  THTransform *t,
114  Tjoint *j)
115 {
116  InitJoint(FIX_JOINT,id,linkID1,link1,linkID2,link2,j);
117 
118  HTransformCopy(&(j->preT),t);
119 
120  /*
121  The translation form link1 to link2 is used in loops.
122  We store it in points[0] to speed up the access
123  (see GenerateJointEquationsInBranch)
124  */
125  j->points[0][0]=HTransformGetElement(0,3,t);
126  j->points[0][1]=HTransformGetElement(1,3,t);
127  j->points[0][2]=HTransformGetElement(2,3,t);
128 
129  j->maxCoord=Norm(3,j->points[0]);
130 }
131 
132 void NewRevoluteJoint(unsigned int id,unsigned int r,
133  unsigned int linkID1,Tlink *link1,
134  unsigned int linkID2,Tlink *link2,
135  double **points,
136  boolean hasLimits,Tinterval *range,double **rPoints,
137  boolean avoidLimits,double avoidLimitsWeight,Tjoint *coupled,
138  Tjoint *j)
139 {
140  unsigned int i,k;
141  double nr;
142  double *x,*y,z[3],v[3],c;
143 
144  InitJoint(REV_JOINT,id,linkID1,link1,linkID2,link2,j);
145 
146  j->maxCoord=0;
147  for(i=0;i<4;i++)
148  {
149  for(k=0;k<3;k++)
150  j->points[i][k]=ADJUST_REAL(points[i][k]);
151  c=Norm(3,j->points[i]);
152  if (c>j->maxCoord) j->maxCoord=c;
153  }
154 
155  /* compute the normalized vectors along the rotation axis */
156  for(i=0;i<2;i++)
157  {
158  nr=0.0;
159  for(k=0;k<3;k++)
160  {
161  j->normals[i][k]=j->points[2*i+1][k]-j->points[2*i][k];
162  nr+=(j->normals[i][k]*j->normals[i][k]);
163  }
164  nr=sqrt(nr);
165  if (nr<ZERO)
166  Error("Null rotation axis in NewRevoluteJoint");
167  for(k=0;k<3;k++)
168  j->normals[i][k]/=nr;
169  }
170 
171  j->hasLimits=hasLimits;
172 
173  if (j->hasLimits)
174  {
175  double c;
176 
177  for(i=0;i<2;i++)
178  {
179  /*normalize the reference vectors*/
180  nr=0.0;
181  for(k=0;k<3;k++)
182  {
183  j->vrange[i][k]=ADJUST_REAL(rPoints[i][k])-j->points[2*i][k];
184  nr+=(j->vrange[i][k]*j->vrange[i][k]);
185  }
186  nr=sqrt(nr);
187  if (nr<ZERO)
188  Error("Null reference vector in NewRevoluteJoint");
189  for(k=0;k<3;k++)
190  j->vrange[i][k]/=nr;
191 
192  /*Test that the rotation vectors are orthogonal to the rotation axes*/
193  c=0; /*cosinus between the two vectors*/
194  for(k=0;k<3;k++)
195  c+=(j->vrange[i][k]*j->normals[i][k]);
196 
197  if (fabs(c)>ZERO)
198  Error("Rotation axes and rotation reference vectors are not orthonormal (Revolute Joint)");
199  }
200 
201  /*Copy of the range for the rotation angle*/
202  CopyInterval(&(j->range),range);
203  }
204 
205  /* The pre-post transform matrices are computed before applying any offset to the
206  reference axis, if any. */
207  x=j->normals[0];
208  if (j->hasLimits)
209  y=j->vrange[0];
210  else
211  {
212  DefineNormalVector(v,x);
213  y=v;
214  }
215  CrossProduct(x,y,z);
216  HTransformFromVectors(x,y,z,j->points[0],&(j->preT));
217 
218  x=j->normals[1];
219  if (j->hasLimits)
220  y=j->vrange[1];
221  else
222  {
223  DefineNormalVector(v,x);
224  y=v;
225  }
226  CrossProduct(x,y,z);
227  HTransformFromVectors(x,y,z,j->points[2],&(j->postT));
228  HTransformInverse(&(j->postT),&(j->postT));
229 
230  /* Now offset the references for those representation that need it */
231  if ((j->hasLimits)&&(r!=REP_JOINTS))
232  {
233  THTransform t,rx,a;
234  double zero[3]={0,0,0};
235 
236  /*We offset the range so that it is zero-centered*/
237  j->offset=IntervalCenter(&(j->range));
238  IntervalOffset(&(j->range),-j->offset,&(j->range));
239 
240  /* Since we moved the rotation range to make it zero-centered, we
241  have to rotate the reference vectors accordingly.
242  In this way, the angle between the reference vectors is changed in
243  the same way as the offset applied to the range interval. */
244  /* The rotation is around the vector defining the revolute joint for
245  the current link. This rotation is defined by rotating the axis
246  vector to the X axis, rotating around X and then rotating the
247  vector back to the its original position.*/
248 
249  HTransformX2Vect(1,1,zero,j->normals[0],&t); /* this rotates X to the
250  revolute axis vector*/
251  HTransformRx(j->offset,&rx); /*rotation around X*/
252 
253  HTransformInverse(&t,&a); /*this rotates the axis vector to X*/
254  HTransformProduct(&rx,&a,&a);
255  HTransformProduct(&t,&a,&a);
256 
257  HTransformApply(j->vrange[0],j->vrange[0],&a);
258 
259  HTransformDelete(&a);
260  HTransformDelete(&rx);
261  HTransformDelete(&t);
262  }
263 
264  if (IntervalSize(&(j->range))>0)
265  {
266  j->avoidLimits=avoidLimits;
267  j->avoidLimitsWeight=avoidLimitsWeight;
268  }
269  else
270  {
271  j->avoidLimits=FALSE;
272  j->avoidLimitsWeight=0;
273  }
274 
275  if (coupled!=NULL)
276  {
277  if (coupled->t!=REV_JOINT)
278  Error("Revolute joints can only be coupled with other revolute joints");
279 
280  if (!coupled->hasLimits)
281  Error("Revolute joints can not be coupled to an unbounded revolute joint");
282 
283  j->coupled=(void *)coupled;
284  }
285 }
286 
287 
288 void NewUniversalJoint(unsigned int id,unsigned int r,
289  unsigned int linkID1,Tlink *link1,
290  unsigned int linkID2,Tlink *link2,
291  double **points,
292  boolean hasLimits,
293  Tinterval*range1,Tinterval *range2,double **rPoints,
294  boolean avoidLimits,double avoidLimitsWeight,
295  Tjoint *j)
296 {
297  unsigned int i,k;
298  double nr;
299  double *x,*y,z[3],v[3],c;
300 
301  InitJoint(UNV_JOINT,id,linkID1,link1,linkID2,link2,j);
302 
303  j->maxCoord=0;
304  for(i=0;i<4;i++)
305  {
306  for(k=0;k<3;k++)
307  j->points[i][k]=ADJUST_REAL(points[i][k]);
308  c=Norm(3,j->points[i]);
309  if (c>j->maxCoord) j->maxCoord=c;
310  }
311 
312  /* compute the normalized vectors along the rotation axis */
313  for(i=0;i<2;i++)
314  {
315  nr=0.0;
316  for(k=0;k<3;k++)
317  {
318  j->normals[i][k]=j->points[2*i+1][k]-j->points[2*i][k];
319  nr+=(j->normals[i][k]*j->normals[i][k]);
320  }
321  if (nr<ZERO)
322  Error("Null rotation axis in NewUniversalJoint");
323  nr=sqrt(nr);
324  for(k=0;k<3;k++)
325  j->normals[i][k]/=nr;
326  }
327 
328  j->hasLimits=hasLimits;
329 
330  if (j->hasLimits)
331  {
332  double c;
333 
334  for(i=0;i<2;i++)
335  {
336  /*normalize the reference vectors*/
337  nr=0.0;
338  for(k=0;k<3;k++)
339  {
340  j->vrange[i][k]=ADJUST_REAL(rPoints[i][k])-j->points[2*i][k];
341  nr+=(j->vrange[i][k]*j->vrange[i][k]);
342  }
343  nr=sqrt(nr);
344  if (nr<ZERO)
345  Error("Null reference vector in NewUniversalJoint");
346  for(k=0;k<3;k++)
347  j->vrange[i][k]/=nr;
348 
349  /*Test that the rotation vectors are orthogonal to the rotation axes*/
350  c=0; /*cosinus between the two vectors*/
351  for(k=0;k<3;k++)
352  c+=(j->vrange[i][k]*j->normals[i][k]);
353 
354  if (fabs(c)>ZERO)
355  Error("Rotation axes and rotation reference vectors are not orthonormal (Universal Joint)");
356  }
357 
358  /*Copy of the ranges for the 2 rotation angles*/
359  /*and we offset them so that they are zero-centered*/
360 
361  CopyInterval(&(j->range),range1);
362  CopyInterval(&(j->range2),range2);
363  }
364 
365  /* The pre-post transform matrices are computed before applying any offset to the
366  reference axis, if any. */
367  x=j->normals[0];
368  if (j->hasLimits)
369  y=j->vrange[0];
370  else
371  {
372  DefineNormalVector(v,x);
373  y=v;
374  }
375  CrossProduct(x,y,z);
376  HTransformFromVectors(x,y,z,j->points[0],&(j->preT));
377 
378  x=j->normals[1];
379  if (j->hasLimits)
380  y=j->vrange[1];
381  else
382  {
383  DefineNormalVector(v,x);
384  y=v;
385  }
386  CrossProduct(x,y,z);
387  HTransformFromVectors(x,y,z,j->points[2],&(j->postT));
388  HTransformInverse(&(j->postT),&(j->postT));
389 
390  /* Now offset the references for those representation that need it */
391  if ((j->hasLimits)&&(r!=REP_JOINTS))
392  {
393  THTransform t,rx,a;
394  double zero[3]={0,0,0};
395  Tinterval *rd;
396  double *center;
397 
398  for(k=0;k<2;k++)
399  {
400  if (k==0)
401  {
402  rd=&(j->range);
403  center=&(j->offset);
404  }
405  else
406  {
407  rd=&(j->range2);
408  center=&(j->offset2);
409  }
410 
411  /*We offset the ranges so that they are zero-centered*/
412  *center=IntervalCenter(rd);
413  IntervalOffset(rd,-(*center),rd);
414 
415  /* Since we moved the rotation range to make it zero-centered, we
416  have to rotate ONE of the the reference vectors accordingly.
417  In this way, the angle between the reference vectors is changed
418  in the same way as the offset applied to the range interval. */
419  /* The rotation is around the vector defining the revolute joint
420  for the current link. This rotation is defined by rotating the
421  axis vector to the X axis, rotating around X and then rotating
422  the vector back to the its original position.*/
423  HTransformX2Vect(1,1,zero,j->normals[k],&t); /* this rotates X to
424  to the revolute
425  axis vector*/
426  HTransformRx((k==0?1:-1)*(*center),&rx); /* rotation around X */
427 
428  HTransformInverse(&t,&a); /* this rotates the axis vector to X */
429  HTransformProduct(&rx,&a,&a);
430  HTransformProduct(&t,&a,&a);
431 
432  HTransformApply(j->vrange[k],j->vrange[k],&a);
433 
434  HTransformDelete(&a);
435  HTransformDelete(&rx);
436  HTransformDelete(&t);
437  }
438  }
439  if ((IntervalSize(&(j->range))>0)&&
440  (IntervalSize(&(j->range2))>0))
441  {
442  j->avoidLimits=avoidLimits;
443  j->avoidLimitsWeight=avoidLimitsWeight;
444  }
445  else
446  {
447  j->avoidLimits=FALSE;
448  j->avoidLimitsWeight=0;
449  }
450 }
451 
452 void NewSphericalJoint(unsigned int id,
453  unsigned int linkID1,Tlink *link1,
454  unsigned int linkID2,Tlink *link2,
455  double **points,
456  boolean hasLimits,double range,double **rPoints,
457  boolean avoidLimits,double avoidLimitsWeight,
458  Tjoint *j)
459 {
460  unsigned int i,k;
461  double nr,c;
462  double zero[3]={0.0,0.0,0.0};
463 
464  InitJoint(SPH_JOINT,id,linkID1,link1,linkID2,link2,j);
465 
466  /*for spherical joints only one point in each link is given, we replicate
467  if as two points. */
468  for(k=0;k<3;k++)
469  {
470  j->points[0][k]=ADJUST_REAL(points[0][k]);
471  j->points[1][k]=ADJUST_REAL(points[0][k]);
472  }
473  for(k=0;k<3;k++)
474  {
475  j->points[2][k]=ADJUST_REAL(points[2][k]);
476  j->points[3][k]=ADJUST_REAL(points[2][k]);
477  }
478  j->maxCoord=0;
479  for(k=0;k<4;k++)
480  {
481  c=Norm(3,j->points[k]);
482  if (c>j->maxCoord) j->maxCoord=c;
483  }
484 
485  j->hasLimits=hasLimits;
486 
487  if (j->hasLimits)
488  {
489  NewInterval(-range,range,&(j->range));
490  j->offset=0;
491  /*normalize the reference vectors*/
492  for(i=0;i<2;i++)
493  {
494  nr=0.0;
495  for(k=0;k<3;k++)
496  {
497  j->vrange[i][k]=ADJUST_REAL(rPoints[i][k])-j->points[2*i][k];
498  nr+=(j->vrange[i][k]*j->vrange[i][k]);
499  }
500  nr=sqrt(nr);
501  if (nr<ZERO)
502  Error("Null reference vector in NewSphericalJoint");
503  for(k=0;k<3;k++)
504  j->vrange[i][k]/=nr;
505  }
506 
507  if (IntervalSize(&(j->range))>0)
508  {
509  j->avoidLimits=avoidLimits;
510  j->avoidLimitsWeight=avoidLimitsWeight;
511  }
512  else
513  {
514  j->avoidLimits=FALSE;
515  j->avoidLimitsWeight=0;
516  }
517  }
518 
519  HTransformTxyz(j->points[0][0],j->points[0][1],j->points[0][2],
520  &(j->preT));
521  if (j->hasLimits)
522  {
523  THTransform ref;
524 
525  HTransformX2Vect(1,1,zero,j->vrange[0],&ref);
526  HTransformProduct(&(j->preT),&ref,&(j->preT));
527  }
528 
529  HTransformTxyz(j->points[2][0],j->points[2][1],j->points[2][2],
530  &(j->postT));
531  if (j->hasLimits)
532  {
533  THTransform ref;
534 
535  HTransformX2Vect(1,1,zero,j->vrange[1],&ref);
536  HTransformProduct(&(j->postT),&ref,&(j->postT));
537  }
538  HTransformInverse(&(j->postT),&(j->postT));
539 }
540 
541 void NewPrismaticJoint(unsigned int id,
542  unsigned int linkID1,Tlink *link1,
543  unsigned int linkID2,Tlink *link2,
544  double **points,Tinterval *range,
545  boolean avoidLimits,double avoidLimitsWeight,
546  Tjoint *j)
547 {
548  unsigned int i,k;
549  double nr,c;
550  //double *x,v[3],z[3];
551 
552  InitJoint(PRS_JOINT,id,linkID1,link1,linkID2,link2,j);
553 
554  j->maxCoord=0;
555  for(i=0;i<4;i++)
556  {
557  for(k=0;k<3;k++)
558  j->points[i][k]=ADJUST_REAL(points[i][k]);
559 
560  c=Norm(3,j->points[i]);
561  if (c>j->maxCoord) j->maxCoord=c;
562  }
563 
564  for(i=0;i<2;i++)
565  {
566  nr=0.0;
567  for(k=0;k<3;k++)
568  {
569  j->normals[i][k]=j->points[2*i+1][k]-j->points[2*i][k];
570  nr+=(j->normals[i][k]*j->normals[i][k]);
571  }
572  nr=sqrt(nr);
573  if (nr<ZERO)
574  Error("Null axis in NewPrismaticJoint");
575  for(k=0;k<3;k++)
576  j->normals[i][k]/=nr;
577  }
578 
579  j->hasLimits=TRUE;
580  CopyInterval(&(j->range),range);
581  c=IntervalSize(range);
582  if (c>j->maxCoord) j->maxCoord=c;
583 
584  if (IntervalSize(&(j->range))>0)
585  {
586  j->avoidLimits=avoidLimits;
587  j->avoidLimitsWeight=avoidLimitsWeight;
588  }
589 
590  HTransformTxyz(j->points[0][0],j->points[0][1],j->points[0][2],
591  &(j->preT));
592  HTransformTxyz(j->points[2][0],j->points[2][1],j->points[2][2],
593  &(j->postT));
594  HTransformInverse(&(j->postT),&(j->postT));
595  /*
596  x=j->normals[0];
597  DefineNormalVector(v,x);
598  CrossProduct(x,v,z);
599  HTransformFromVectors(x,v,z,j->points[0],&(j->preT));
600 
601  x=j->normals[1];
602  DefineNormalVector(v,x);
603  CrossProduct(x,v,z);
604  HTransformFromVectors(x,v,z,j->points[2],&(j->postT));
605  HTransformInverse(&(j->postT),&(j->postT));
606  */
607 }
608 
609 void NewSphSphJoint(unsigned int id,
610  unsigned int linkID1,Tlink *link1,
611  unsigned int linkID2,Tlink *link2,
612  double **points,
613  double l,double r,
614  Tcolor *color,
615  Tjoint *j)
616 {
617  unsigned int k;
618  double c;
619 
620  InitJoint(SPH_SPH_JOINT,id,linkID1,link1,linkID2,link2,j);
621 
622  /*for spherical joints only one point in each link is given, we replicate
623  if as two points. */
624  for(k=0;k<3;k++)
625  {
626  j->points[0][k]=ADJUST_REAL(points[0][k]);
627  j->points[1][k]=ADJUST_REAL(points[0][k]);
628  }
629  for(k=0;k<3;k++)
630  {
631  j->points[2][k]=ADJUST_REAL(points[1][k]);
632  j->points[3][k]=ADJUST_REAL(points[1][k]);
633  }
634 
635  j->maxCoord=l;
636  for(k=0;k<4;k++)
637  {
638  c=Norm(3,j->points[k]);
639  if (c>j->maxCoord) j->maxCoord=c;
640  }
641 
642  j->rad=r;
643  j->length=l;
644  CopyColor(&(j->color),color);
645 
646  HTransformTxyz(j->points[0][0],j->points[0][1],j->points[0][2],&(j->preT));
647 
648  HTransformTxyz(j->points[2][0],j->points[2][1],j->points[2][2],&(j->postT));
649  HTransformInverse(&(j->postT),&(j->postT));
650 }
651 
652 void NewSphPrsSphJoint(unsigned int id,
653  unsigned int linkID1,Tlink *link1,
654  unsigned int linkID2,Tlink *link2,
655  double **points,
656  Tinterval *range,double r,
657  Tcolor *color,
658  Tjoint *j)
659 {
660  unsigned int k;
661  double l,ra,c;
662 
663  InitJoint(SPH_PRS_SPH_JOINT,id,linkID1,link1,linkID2,link2,j);
664 
665  /*for spherical joints only one point in each link is given, we replicate
666  if as two points. */
667  for(k=0;k<3;k++)
668  {
669  j->points[0][k]=ADJUST_REAL(points[0][k]);
670  j->points[1][k]=ADJUST_REAL(points[0][k]);
671  }
672  for(k=0;k<3;k++)
673  {
674  j->points[2][k]=ADJUST_REAL(points[1][k]);
675  j->points[3][k]=ADJUST_REAL(points[1][k]);
676  }
677 
678  /*Set some empty information for the range (not used)*/
679 
680  l=LowerLimit(range);
681  ra=IntervalSize(range);
682 
683  if (l<0)
684  Error("The range should be positive in Spherical-Prismatic-Spherical joint");
685 
686  if (ra>l)
687  Error("The range should be smaller than the minimum extension in Spherical-Prismatic-Spherical joint");
688 
689  j->maxCoord=ra;
690  for(k=0;k<4;k++)
691  {
692  c=Norm(3,j->points[k]);
693  if (c>j->maxCoord) j->maxCoord=c;
694  }
695 
696  CopyInterval(&(j->range),range);
697  j->offset=ra;
698 
699  j->rad=r;
700  j->length=0;
701  CopyColor(&(j->color),color);
702 
703  HTransformTxyz(j->points[0][0],j->points[0][1],j->points[0][2],&(j->preT));
704 
705  HTransformTxyz(j->points[2][0],j->points[2][1],j->points[2][2],&(j->postT));
706  HTransformInverse(&(j->postT),&(j->postT));
707 }
708 
709 void NewInPatchJoint(unsigned int id,
710  unsigned int linkID1,Tlink *link1,
711  unsigned int linkID2,Tlink *link2,
712  double **points,
713  double **patch,
714  boolean avoidLimits,double avoidLimitsWeight,
715  Tjoint *j)
716 {
717  unsigned int k;
718  double aux0[3],t1[3],t2[3];
719  double n1,n2;
720  Tinterval range,v,a;
721  double nr,c;
722  double vect[3];
723 
724  InitJoint(IN_PATCH_JOINT,id,linkID1,link1,linkID2,link2,j);
725 
726  /* Store the 2 points defining the point and normal on the first link (not
727  in the patch!)*/
728  nr=0.0;
729  for(k=0;k<3;k++)
730  {
731  j->points[0][k]=ADJUST_REAL(points[0][k]);
732  j->points[1][k]=ADJUST_REAL(points[1][k])-j->points[0][k];
733  nr+=(j->points[1][k]*j->points[1][k]);
734  }
735  nr=sqrt(nr);
736  if (nr<ZERO)
737  Error("Null normal vector in NewInPatchJoint");
738  for(k=0;k<3;k++)
739  {
740  j->points[1][k]/=nr;
741  vect[k]=j->points[0][k]+j->points[1][k]; /* Vector used to define preT */
742  }
743 
744  j->maxCoord=Norm(3,j->points[0]);
745  for(k=0;k<4;k++)
746  {
747  c=Norm(3,patch[k]);
748  if (c>j->maxCoord) j->maxCoord=c;
749  }
750 
751  /* Store the 4 points defining the patch in the second link. Actually we do
752  not directly store the points but some differences between them
753  With this we have that
754  p=po+u*(p0->p2)+v*(p0->p1)+w*(p0-p1-p2+p3)
755  p=points[2]+u*points[3]+v*points[4]+w*points[5]
756  where
757  w=u*v
758  */
759  for(k=0;k<3;k++)
760  {
761  /* p0 */
762  j->points[2][k]=patch[0][k];
763  /* (p0->p2)=p2-p0 */
764  j->points[3][k]=patch[2][k]-patch[0][k];
765  /* (p0->p1)=p1-p0 */
766  j->points[4][k]=patch[1][k]-patch[0][k];
767  /* d=(p0-p1-p2+p3) */
768  j->points[5][k]=patch[0][k]-patch[1][k]-patch[2][k]+patch[3][k];
769  }
770 
771  /* Size of the sides of the patch: must be larger than 0 */
772  n1=DotProduct(j->points[3],j->points[3]);
773  n2=DotProduct(j->points[4],j->points[4]);
774 
775  if ((n1<ZERO)||(n2<ZERO))
776  Error("Patch is too small");
777 
778  /*Store information to recover the normal at any point in the patch.
779  The normal is defined as
780  n=n0+u*n1+v*n2
781  with
782  n0=(p0->p2)x(p0->p1)
783  n1=(p0->p2)x d
784  n2=d x (p0->p1)
785  */
786  /* (p0->p2) x (p0->p1) */
787  CrossProduct(j->points[3],j->points[4],j->normals[0]);
788  /* (p0->p2) x d */
789  CrossProduct(j->points[3],j->points[5],j->normals[1]);
790  /* d x (p0->p1) */
791  CrossProduct(j->points[5],j->points[4],j->normals[2]);
792 
793  /*Check if the input points correctly define a patch.
794  Points defining a patch are given as
795  p2------p3
796  | |
797  | |
798  p0------p1
799  Thus, in a correct patch vector p0->p3 is enclosed in
800  vectors p0->p2 and p0->p1.
801  We check if this is so ensuring that angle p2-p0-p3 has the
802  same sign as angle p3-p0-p1
803  */
804  for(k=0;k<3;k++)
805  aux0[k]=patch[3][k]-patch[0][k]; /*This is p0->p3*/
806 
807  if ((DotProduct(aux0,j->points[3])<ZERO)||
808  (DotProduct(aux0,j->points[4])<ZERO))
809  Error("Patch with overlapping points");
810 
811  CrossProduct(j->points[3],aux0,t1); /* t1=(p0->p2) x (p0->p3) */
812  CrossProduct(aux0,j->points[4],t2); /* t2=(p0->p3) x (p0->p1) */
813 
814  /* Now we check that the above vectors have the same orientation */
815  if (DotProduct(t1,t2)<ZERO)
816  Error("Patch points given in the wrong order");
817 
818  /*Now we compute the range for the norm of the normal vector
819  n=n0+u*n1+v*n2
820  The norm is
821  l=sqrt(n0+u*n1+v*n2)*(n0+u*n1+v*n2)=
822  sqrt(n0*n0+u*n1*n0+v*n2*n0+n0*u*n1+u*n1*u*n1+v*n2*u*n1+v*n2*n0+v*n2*u*n1+v*n2*v*n2)=
823  sqrt(n0*n0+n1*n1*u^2+n2*n2*v^2+2(n1*n0)*u+2*(n2*n0)*v+2*(n2*n1)*u*v
824  The range for l is bounded via interval evaluation of the above expression
825  with u=v=u^2=v^3=u*v=[0,1]
826  */
827  NewInterval(0,1,&v); /* range for u,v,u^2,v^2,u*v */
828 
829  n1=DotProduct(j->normals[0],j->normals[0]); /*n0*n0*/
830  NewInterval(n1,n1,&range);
831 
832  n1=DotProduct(j->normals[1],j->normals[1]); /*+n1*n1*u^2*/
833  IntervalScale(&v,n1,&a);
834  IntervalAdd(&range,&a,&range);
835 
836  n1=DotProduct(j->normals[2],j->normals[2]); /*+n2*n2*v^2*/
837  IntervalScale(&v,n1,&a);
838  IntervalAdd(&range,&a,&range);
839 
840  n1=2*DotProduct(j->normals[0],j->normals[1]); /*+2*n0*n1*u*/
841  IntervalScale(&v,n1,&a);
842  IntervalAdd(&range,&a,&range);
843 
844  n1=2*DotProduct(j->normals[0],j->normals[2]); /*+2*n0*n2*v*/
845  IntervalScale(&v,n1,&a);
846  IntervalAdd(&range,&a,&range);
847 
848  n1=2*DotProduct(j->normals[1],j->normals[2]); /*+2*n1*n2*u*v*/
849  IntervalScale(&v,n1,&a);
850  IntervalAdd(&range,&a,&range);
851 
852  IntervalSqrt(&range,&(j->normRange)); /*final sqrt*/
853 
854  /*Now we store the avoid limit information, if any*/
855  j->hasLimits=TRUE; /*Limits are given by the patch*/
856 
857  NewInterval(0,1,&(j->range));
858  NewInterval(0,1,&(j->range2));
859 
860  /* For non-zero area patch considere avoidLimits */
861  if ((DotProduct(j->points[3],j->points[3])>0)&&
862  (DotProduct(j->points[4],j->points[4])>0))
863  {
864  j->avoidLimits=avoidLimits;
865  j->avoidLimitsWeight=avoidLimitsWeight;
866  }
867 
868  /* vect = points[0]+points[1] */
869  HTransformX2Vect(1,1,j->points[0],vect,&(j->preT));
870  HTransformIdentity(&(j->postT));
871 }
872 
873 void CopyJoint(Tjoint *j_dst,Tjoint *j_src)
874 {
875  unsigned int i,k;
876 
877  j_dst->t=j_src->t;
878 
879  j_dst->id=j_src->id;
880 
881  for(i=0;i<2;i++)
882  {
883  j_dst->linkID[i]=j_src->linkID[i];
884  j_dst->link[i]=j_src->link[i];
885  }
886 
887  for(i=0;i<6;i++)
888  {
889  for(k=0;k<3;k++)
890  j_dst->points[i][k]=j_src->points[i][k];
891  }
892 
893  for(i=0;i<3;i++)
894  {
895  for(k=0;k<3;k++)
896  j_dst->normals[i][k]=j_src->normals[i][k];
897  }
898 
899  j_dst->maxCoord=j_src->maxCoord;
900 
901  j_dst->hasLimits=j_src->hasLimits;
902 
903  j_dst->coupled=j_src->coupled;
904 
905  j_dst->avoidLimits=j_src->avoidLimits;
906  j_dst->avoidLimitsWeight=j_src->avoidLimitsWeight;
907 
908  CopyInterval(&(j_dst->normRange),&(j_src->normRange));
909 
910  CopyInterval(&(j_dst->range),&(j_src->range));
911  CopyInterval(&(j_dst->range2),&(j_src->range2));
912  j_dst->offset=j_src->offset;
913  j_dst->offset2=j_src->offset2;
914 
915  for(i=0;i<2;i++)
916  {
917  for(k=0;k<3;k++)
918  j_dst->vrange[i][k]=j_src->vrange[i][k];
919  }
920 
921  j_dst->obj3d[0]=j_src->obj3d[0];
922  j_dst->obj3d[1]=j_src->obj3d[1];
923  j_dst->rad=j_src->rad;
924  j_dst->length=j_src->length;
925  CopyColor(&(j_dst->color),&(j_src->color));
926 
927  HTransformCopy(&(j_dst->preT),&(j_src->preT));
928  HTransformCopy(&(j_dst->postT),&(j_src->postT));
929 }
930 
931 unsigned int GetJointType(Tjoint *j)
932 {
933  return(j->t);
934 }
935 
936 unsigned int GetJointID(Tjoint *j)
937 {
938  return(j->id);
939 }
940 
941 unsigned int JointFromID(Tjoint *j)
942 {
943  return(j->linkID[0]);
944 }
945 
946 Tlink *JointFrom(Tjoint *j)
947 {
948  return(j->link[0]);
949 }
950 
951 unsigned int JointToID(Tjoint *j)
952 {
953  return(j->linkID[1]);
954 }
955 
956 Tlink *JointTo(Tjoint *j)
957 {
958  return(j->link[1]);
959 }
960 
961 void GetJointName(char **name,Tjoint *j)
962 {
963  char *ln1,*ln2;
964 
965  ln1=GetLinkName(j->link[0]);
966  ln2=GetLinkName(j->link[1]);
967  NEW(*name,strlen(ln1)+strlen(ln2)+100,char);
968 
969  switch(j->t)
970  {
971  case FREE_JOINT:
972  sprintf(*name,"Free %s---%s",ln1,ln2);
973  break;
974  case FIX_JOINT:
975  sprintf(*name,"Fix %s---%s",ln1,ln2);
976  break;
977  case PRS_JOINT:
978  sprintf(*name,"Prismatic %s---%s",ln1,ln2);
979  break;
980  case SPH_JOINT:
981  sprintf(*name,"Spheric %s---%s",ln1,ln2);
982  break;
983  case REV_JOINT:
984  sprintf(*name,"Revolute %s---%s",ln1,ln2);
985  break;
986  case UNV_JOINT:
987  sprintf(*name,"Universal %s---%s",ln1,ln2);
988  break;
989  case SPH_SPH_JOINT:
990  sprintf(*name,"Sph-Sph %s---%s",ln1,ln2);
991  break;
992  case SPH_PRS_SPH_JOINT:
993  sprintf(*name,"Sph-Prs-Sph %s---%s",ln1,ln2);
994  break;
995  case IN_PATCH_JOINT:
996  sprintf(*name,"In patch %s-%s",ln1,ln2);
997  break;
998  default:
999  Error("Undefined joint type in GetJointName");
1000  }
1001 
1002 }
1003 
1004 boolean IsJointAllSpheres(Tjoint *j)
1005 {
1006  /* Only sph_sph and sph_prs_sph joints define bodies... and in this
1007  case they are cylinders and not spheres. */
1008 
1009  return((j->t!=SPH_SPH_JOINT)&&(j->t!=SPH_PRS_SPH_JOINT));
1010 }
1011 
1012 void GetJointPoint(unsigned int link,unsigned int point,double *p,Tjoint *j)
1013 {
1014  unsigned int i,k;
1015 
1016  i=link*2+point;
1017 
1018  for(k=0;k<3;k++)
1019  p[k]=j->points[i][k];
1020 }
1021 
1022 boolean LimitedJoint(Tjoint *j)
1023 {
1024  return(j->hasLimits);
1025 }
1026 
1027 Tinterval *GetJointRange(Tjoint *j)
1028 {
1029  return(&(j->range));
1030 }
1031 
1032 Tinterval *GetJointSecondRange(Tjoint *j)
1033 {
1034  return(&(j->range2));
1035 }
1036 
1037 void GetJointRangeN(unsigned int nr,double mt,Tinterval *r,Tjoint *j)
1038 {
1039  switch(j->t)
1040  {
1041  case FREE_JOINT:
1042  if (nr<6)
1043  {
1044  if (nr<3)
1045  NewInterval(-mt,mt,r);
1046  else
1047  NewInterval(-M_PI,M_PI,r);
1048  }
1049  else
1050  Error("Unknown range number in free joint (GetJointRangeN)");
1051  break;
1052  case FIX_JOINT:
1053  Error("Fix Joints have no DOFs");
1054  break;
1055  case PRS_JOINT:
1056  if (nr<1)
1057  CopyInterval(r,&(j->range));
1058  else
1059  Error("Unknown range number in prismatic joint (GetJointRangeN)");
1060  break;
1061  case SPH_JOINT:
1062  if (nr<3)
1063  {
1064  if ((!j->hasLimits)||(nr==2))
1065  NewInterval(-M_PI,M_PI,r);
1066  else
1067  CopyInterval(r,&(j->range));
1068  }
1069  else
1070  Error("Unknown range number in spherical joint (GetJointRangeN)");
1071  break;
1072  case REV_JOINT:
1073  if (nr<1)
1074  {
1075  if (!j->hasLimits)
1076  NewInterval(-M_PI,M_PI,r);
1077  else
1078  IntervalOffset(&(j->range),j->offset,r);
1079  }
1080  else
1081  Error("Unknown range number in revolute joint (GetJointRangeN)");
1082  break;
1083  case UNV_JOINT:
1084  if (nr<2)
1085  {
1086  if (!j->hasLimits)
1087  NewInterval(-M_PI,M_PI,r);
1088  else
1089  {
1090  if (nr==0)
1091  IntervalOffset(&(j->range),j->offset,r);
1092  else
1093  IntervalOffset(&(j->range2),j->offset2,r);
1094  }
1095  }
1096  else
1097  Error("Unknown range number in universal joint (GetJointRangeN)");
1098  break;
1099  case SPH_SPH_JOINT:
1100  if (nr<5)
1101  NewInterval(-M_PI,M_PI,r);
1102  else
1103  Error("Unknown range number in sph_sph joint (GetJointRangeN)");
1104  break;
1105  case SPH_PRS_SPH_JOINT:
1106  if (nr<6)
1107  {
1108  if (nr!=2)
1109  NewInterval(-M_PI,M_PI,r);
1110  else
1111  CopyInterval(r,&(j->range));
1112  }
1113  else
1114  Error("Unknown range number in sph_prs_sph joint (GetJointRangeN)");
1115  break;
1116  case IN_PATCH_JOINT:
1117  if (nr<3)
1118  {
1119  if (nr==0)
1120  CopyInterval(r,&(j->range));
1121  else
1122  {
1123  if (nr==1)
1124  CopyInterval(r,&(j->range2));
1125  else
1126  NewInterval(-M_PI,M_PI,r);
1127  }
1128  }
1129  else
1130  Error("Unknown range number in in_patch joint (GetJointRangeN)");
1131  break;
1132  default:
1133  Error("Undefined joint type in GetJointRangeN");
1134  }
1135 }
1136 
1137 unsigned int GetJointRangeTopology(unsigned int nr,Tjoint *j)
1138 {
1139  unsigned int tp=0;
1140 
1141  switch(j->t)
1142  {
1143  case FREE_JOINT:
1144  if (nr<6)
1145  {
1146  if (nr<3)
1147  tp=TOPOLOGY_R;
1148  else
1149  tp=TOPOLOGY_S;
1150  }
1151  else
1152  Error("Unknown range number in free joint (GetJointRangeN)");
1153  break;
1154  case FIX_JOINT:
1155  Error("Fix Joints have no DOFs");
1156  break;
1157  case PRS_JOINT:
1158  if (nr<1)
1159  tp=TOPOLOGY_R;
1160  else
1161  Error("Unknown range number in prismatic joint (GetJointRangeN)");
1162  break;
1163  case SPH_JOINT:
1164  if (nr<3)
1165  tp=TOPOLOGY_S;
1166  else
1167  Error("Unknown range number in spherical joint (GetJointRangeN)");
1168  break;
1169  case REV_JOINT:
1170  if (nr<1)
1171  tp=TOPOLOGY_S;
1172  else
1173  Error("Unknown range number in revolute joint (GetJointRangeN)");
1174  break;
1175  case UNV_JOINT:
1176  if (nr<2)
1177  tp=TOPOLOGY_S;
1178  else
1179  Error("Unknown range number in universal joint (GetJointRangeN)");
1180  break;
1181  case SPH_SPH_JOINT:
1182  if (nr<5)
1183  tp=TOPOLOGY_S;
1184  else
1185  Error("Unknown range number in sph_sph joint (GetJointRangeN)");
1186  break;
1187  case SPH_PRS_SPH_JOINT:
1188  if (nr<6)
1189  {
1190  if (nr!=2)
1191  tp=TOPOLOGY_S;
1192  else
1193  tp=TOPOLOGY_R;
1194  }
1195  else
1196  Error("Unknown range number in sph_prs_sph joint (GetJointRangeN)");
1197  break;
1198  case IN_PATCH_JOINT:
1199  if (nr<3)
1200  {
1201  if (nr!=2)
1202  tp=TOPOLOGY_R;
1203  else
1204  tp=TOPOLOGY_S;
1205  }
1206  else
1207  Error("Unknown range number in in_patch joint (GetJointRangeN)");
1208  break;
1209  default:
1210  Error("Undefined joint type in GetJointRangeTopology");
1211  }
1212 
1213  return(tp);
1214 }
1215 
1216 unsigned int CoupledWith(Tjoint *j)
1217 {
1218  if (j->coupled==NULL)
1219  return(NO_UINT);
1220  else
1221  return(((Tjoint *)(j->coupled))->id);
1222 }
1223 
1224 signed int GetJointDOF(Tjoint *j)
1225 {
1226  signed int ndof=0;
1227 
1228  switch(j->t)
1229  {
1230  case FREE_JOINT:
1231  ndof=6;
1232  break;
1233  case FIX_JOINT:
1234  ndof=0;
1235  break;
1236  case PRS_JOINT:
1237  ndof=1;
1238  break;
1239  case SPH_JOINT:
1240  ndof=3;
1241  break;
1242  case REV_JOINT:
1243  if (j->coupled==NULL)
1244  ndof=1;
1245  else
1246  ndof=0;
1247  break;
1248  case UNV_JOINT:
1249  ndof=2;
1250  break;
1251  case SPH_SPH_JOINT:
1252  /* The revolute in the middle is not considered */
1253  ndof=5;
1254  break;
1255  case SPH_PRS_SPH_JOINT:
1256  /* The revolute in the middle is not considered */
1257  ndof=6;
1258  break;
1259  case IN_PATCH_JOINT:
1260  ndof=3;
1261  break;
1262  default:
1263  Error("Undefined joint type in GetJointDOF");
1264  }
1265  return(ndof);
1266 }
1267 
1268 double GetJointLength(Tjoint *j)
1269 {
1270  if (j->t!=SPH_SPH_JOINT)
1271  Error("The length is only defined for spherical-spherical composite joints");
1272 
1273  return(j->length);
1274 }
1275 
1276 void GenerateJointRangeSingularityEquations(Tparameters *p,TCuikSystem *cs,Tjoint *j)
1277 {
1278  unsigned int r;
1279 
1280  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1281 
1282  if (r==REP_JOINTS)
1283  Error("GenerateJointRangeSingularityEquations not yet implemented for JOINT-based representations.");
1284 
1285  if (j->hasLimits)
1286  {
1287  unsigned int i,n;
1288  double a,b,c,r;
1289  Tinterval *range;
1290  Tequation eq;
1291  Tmonomial f;
1292  Tinterval r1;
1293  unsigned int vOrig,vID1,vID2;
1294  char *vname,*ln1,*ln2;
1295  Tvariable var;
1296 
1297  ln1=GetLinkName(j->link[0]);
1298  ln2=GetLinkName(j->link[1]);
1299  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
1300 
1301  InitMonomial(&f);
1302 
1303  switch (j->t)
1304  {
1305  case REV_JOINT:
1306  case SPH_JOINT:
1307  case UNV_JOINT:
1308  /* revolute ranges given as cosinus of variables */
1309  n=((j->t==UNV_JOINT)?2:1);
1310 
1311  for(i=0;i<n;i++)
1312  {
1313  range=(i==0?&(j->range):&(j->range2));
1314 
1315  /*Get the original 'cos' variable*/
1316  if (j->t==UNV_JOINT)
1317  COS_VAR_UNI(vname,j->id,ln1,ln2,i);
1318  else
1319  COS_VAR(vname,j->id,ln1,ln2);
1320 
1321  vOrig=GetCSVariableID(vname,cs);
1322  if (vOrig==NO_UINT)
1323  Error("Undefined variable in GenerateJointRangeSigularityEquations");
1324 
1325 
1326  /* introduce the new t var */
1327  if (j->t==UNV_JOINT)
1328  COS_VAR_UNI_SING(vname,j->id,ln1,ln2,i);
1329  else
1330  COS_VAR_SING(vname,j->id,ln1,ln2);
1331  NewInterval(0,1,&r1);
1332 
1333  NewVariable(SYSTEM_VAR,vname,&var);
1334  SetVariableInterval(&r1,&var);
1335 
1336  vID1=AddVariable2CS(&var,cs);
1337 
1338  DeleteVariable(&var);
1339 
1340  /*introduce the new equation: t^2-cos=-cos(lower(range))
1341  Here we assume that range is symmetrical w.r.t. zero
1342  and, thus lower(cos(range))= cos(lower(range))*/
1343  InitEquation(&eq);
1344  SetEquationValue(-cos(LowerLimit(range)),&eq);
1345 
1346  AddVariable2Monomial(NFUN,vID1,2,&f);
1347  AddMonomial(&f,&eq);
1348  ResetMonomial(&f);
1349 
1350  AddCt2Monomial(-1,&f);
1351  AddVariable2Monomial(NFUN,vOrig,1,&f);
1352  AddMonomial(&f,&eq);
1353  ResetMonomial(&f);
1354 
1355  SetEquationCmp(EQU,&eq);
1356  SetEquationType(SYSTEM_EQ,&eq);
1357 
1358  AddEquation2CS(p,&eq,cs);
1359  DeleteEquation(&eq);
1360  }
1361 
1362  break;
1363  case PRS_JOINT:
1364  case SPH_PRS_SPH_JOINT:
1365  case IN_PATCH_JOINT:
1366  /* lineal ranges */
1367  n=((j->t==IN_PATCH_JOINT)?2:1);
1368 
1369  for(i=0;i<n;i++)
1370  {
1371  range=(i==0?&(j->range):&(j->range2));
1372 
1373  a=LowerLimit(range);
1374  b=UpperLimit(range);
1375  c=(b+a)/2; /* center */
1376  r=(b-a)/2; /* radius */
1377 
1378  /*Get the original 'l' variable*/
1379  if (j->t==IN_PATCH_JOINT)
1380  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,i);
1381  else
1382  PRS_JOINT_VAR(vname,j->id,ln1,ln2);
1383 
1384  vOrig=GetCSVariableID(vname,cs);
1385  if (vOrig==NO_UINT)
1386  Error("Undefined variable in GenerateJointRangeSigularityEquations");
1387 
1388  /* introduce the new sin var */
1389  if (j->t==IN_PATCH_JOINT)
1390  IN_PATCH_JOINT_CTRL_VAR_SING_SIN(vname,j->id,ln1,ln2,i);
1391  else
1392  PRS_JOINT_VAR_SING_SIN(vname,j->id,ln1,ln2);
1393 
1394  NewVariable(SYSTEM_VAR,vname,&var);
1395  NewInterval(-1,1,&r1);
1396  SetVariableInterval(&r1,&var);
1397 
1398  vID1=AddVariable2CS(&var,cs);
1399 
1400  DeleteVariable(&var);
1401 
1402  /* introduce the new cos var */
1403  if (j->t==IN_PATCH_JOINT)
1404  IN_PATCH_JOINT_CTRL_VAR_SING_COS(vname,j->id,ln1,ln2,i);
1405  else
1406  PRS_JOINT_VAR_SING_COS(vname,j->id,ln1,ln2);
1407 
1408  NewVariable(SYSTEM_VAR,vname,&var);
1409  NewInterval(0,1,&r1);
1410  SetVariableInterval(&r1,&var);
1411 
1412  vID2=AddVariable2CS(&var,cs);
1413 
1414  DeleteVariable(&var);
1415 
1416  /*Add two new equations*/
1417 
1418  /*vOrig-r*vSin=c*/
1419  InitEquation(&eq);
1420  SetEquationValue(c,&eq);
1421 
1422  AddVariable2Monomial(NFUN,vOrig,1,&f);
1423  AddMonomial(&f,&eq);
1424  ResetMonomial(&f);
1425 
1426  AddCt2Monomial(-r,&f);
1427  AddVariable2Monomial(NFUN,vID1,1,&f);
1428  AddMonomial(&f,&eq);
1429  ResetMonomial(&f);
1430 
1431  SetEquationCmp(EQU,&eq);
1432  SetEquationType(SYSTEM_EQ,&eq);
1433 
1434  AddEquation2CS(p,&eq,cs);
1435  ResetEquation(&eq);
1436 
1437  /* s^2+c^2=1*/
1438  SetEquationValue(1,&eq);
1439 
1440  AddVariable2Monomial(NFUN,vID1,2,&f);
1441  AddMonomial(&f,&eq);
1442  ResetMonomial(&f);
1443 
1444  AddVariable2Monomial(NFUN,vID2,2,&f);
1445  AddMonomial(&f,&eq);
1446  ResetMonomial(&f);
1447 
1448  SetEquationCmp(EQU,&eq);
1449  SetEquationType(SYSTEM_EQ,&eq);
1450  AddEquation2CS(p,&eq,cs);
1451  ResetMonomial(&f);
1452  }
1453  DeleteEquation(&eq);
1454  break;
1455  default:
1456  Error("Unkwon joint type in GenerateJointRangeSingularityEquations");
1457  }
1458  free(vname);
1459  DeleteMonomial(&f);
1460  }
1461 }
1462 
1463 void GenerateJointRangeEquations(Tparameters *p,TCuikSystem *cs,Tjoint *j)
1464 {
1465  unsigned int r;
1466 
1467  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1468 
1469  if (r!=REP_JOINTS)
1470  {
1471  /* Equations associated with the joint
1472  (not joint equations as in the paper :) */
1473 
1474  char *vname;
1475  unsigned int i,k,id,nv;
1476  Tequation eq[3],eq1;
1477  Tinterval rangeOne,rangeInf;
1478  Tmonomial f;
1479  unsigned int jointVars[4][3]; /*Reference vector used in REVOLUTE and
1480  SPHERICAL joints*/
1481  Tvariable var;
1482  char *ln1,*ln2;
1483  Tinterval ic;
1484  double r;
1485 
1486  NewInterval(-1.0,1.0,&rangeOne);
1487  NewInterval(-INF,INF,&rangeInf);
1488 
1489  InitMonomial(&f);
1490 
1491  ln1=GetLinkName(j->link[0]);
1492  ln2=GetLinkName(j->link[1]);
1493 
1494  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
1495 
1496  switch(j->t)
1497  {
1498  case FREE_JOINT:
1499  /* variables already generated in GenerateJointEquations */
1500  break;
1501 
1502  case FIX_JOINT:
1503  /* no variables necessary for fix joints */
1504  break;
1505 
1506  case SPH_PRS_SPH_JOINT:
1507  case PRS_JOINT:
1508  /*Add one variable for the displacement along the prismatic axis*/
1509  PRS_JOINT_VAR(vname,j->id,ln1,ln2);
1510 
1511  NewVariable(SYSTEM_VAR,vname,&var);
1512  SetVariableInterval(&(j->range),&var);
1513 
1514  nv=AddVariable2CS(&var,cs);
1515 
1516  DeleteVariable(&var);
1517 
1518  if (j->avoidLimits)
1519  {
1520  r=IntervalSize(&(j->range))/2;
1521  AddTerm2SearchCriterion(j->avoidLimitsWeight/(r*r),nv,
1522  IntervalCenter(&(j->range)),cs);
1523  }
1524 
1525  break;
1526 
1527  case UNV_JOINT:
1528  case REV_JOINT:
1529  case SPH_JOINT:
1530  if (j->hasLimits)
1531  {
1532  /*We define two vectors normal to the rotation axis, one attached
1533  to link1 (k==0) and another attached to link2 (k==1)*/
1534  for(k=0;k<2;k++)
1535  {
1536  /*Define the variables for the rotation reference vectors*/
1537  for(i=0;i<3;i++)
1538  {
1539  InitEquation(&(eq[i]));
1540  SetEquationValue(0.0,&(eq[i]));
1541 
1542  if (j->t==REV_JOINT)
1543  ROT_JOINT_VAR_REF(vname,j->id,k,ln1,ln2,i);
1544  else
1545  {
1546  if (j->t==SPH_JOINT)
1547  SPH_JOINT_VAR_REF(vname,j->id,k,ln1,ln2,i);
1548  else
1549  UNV_JOINT_VAR_REF(vname,j->id,k,ln1,ln2,i);
1550  }
1551 
1552  NewVariable(SECONDARY_VAR,vname,&var);
1553  SetVariableInterval(&rangeOne,&var);
1554 
1555  id=AddVariable2CS(&var,cs);
1556 
1557  DeleteVariable(&var);
1558 
1559  jointVars[k][i]=id;
1560  AddVariable2Monomial(NFUN,id,1,&f);
1561  AddMonomial(&f,&(eq[i]));
1562  ResetMonomial(&f);
1563  }
1564 
1565  ApplyLinkRot(p,-1.0,NO_UINT,j->vrange[k],eq,cs,
1566  IsGroundLink(j->linkID[k]),j->link[k]);
1567 
1568  for(i=0;i<3;i++)
1569  {
1570  SetEquationCmp(EQU,&(eq[i]));
1571  SetEquationType(SYSTEM_EQ,&(eq[i]));
1572 
1573  AddEquation2CS(p,&(eq[i]),cs);
1574  DeleteEquation(&(eq[i]));
1575  }
1576  }
1577 
1578  if (j->t==UNV_JOINT)
1579  {
1580  /* The angles for a universal joint are defined from the two
1581  vectors defining the two copunctual, orthogonal rotation
1582  axis towards the two reference vectors given by the user.
1583  The first angle is from the 1st reference vector to the
1584  second rotation vector. The second angle is from the
1585  2nd reference vector to the 1st rotation vector.
1586 
1587  Above we defined variables for the two reference vectors.
1588  Below we retrive the variable identifiers for the two
1589  vectors defining the copunctual, orthogonal rotation
1590  axis (those variables are defined in function
1591  GenerateJointEquations.
1592  */
1593  for(k=2;k<4;k++)
1594  {
1595  for(i=0;i<3;i++)
1596  {
1597  UNV_JOINT_VAR(vname,j->id,ln1,ln2,k-2,i);
1598  jointVars[k][i]=GetCSVariableID(vname,cs);
1599  if (jointVars[k][i]==NO_UINT)
1600  Error("Undefined variables when defining universal joint range equations");
1601  }
1602  }
1603 
1604  for(k=0;k<2;k++)
1605  {
1606  GenerateDotProductEquation(jointVars[3-k][0],
1607  jointVars[3-k][1],
1608  jointVars[3-k][2],
1609  jointVars[k][0],
1610  jointVars[k][1],
1611  jointVars[k][2],
1612  NO_UINT,0,&eq1);
1613 
1614  COS_VAR_UNI(vname,j->id,ln1,ln2,k);
1615  NewVariable(SECONDARY_VAR,vname,&var);
1616  if (k==0)
1617  IntervalCosine(&(j->range),&ic);
1618  else
1619  IntervalCosine(&(j->range2),&ic);
1620  SetVariableInterval(&ic,&var);
1621  DeleteInterval(&ic);
1622  id=AddVariable2CS(&var,cs);
1623  DeleteVariable(&var);
1624 
1625  AddVariable2Monomial(NFUN,id,1,&f);
1626  AddCt2Monomial(-1,&f);
1627  AddMonomial(&f,&eq1);
1628  ResetMonomial(&f);
1629 
1630  SetEquationCmp(EQU,&eq1);
1631  SetEquationType(SYSTEM_EQ,&eq1);
1632  AddEquation2CS(p,&eq1,cs);
1633  DeleteEquation(&eq1);
1634 
1635  if (j->avoidLimits)
1636  {
1637  r=IntervalSize(&ic);
1638  AddTerm2SearchCriterion(j->avoidLimitsWeight/(r*r),id,1,cs);
1639  }
1640  }
1641  }
1642  else
1643  {
1644  /* Revolute and spherical */
1645 
1646  GenerateDotProductEquation(jointVars[0][0],
1647  jointVars[0][1],
1648  jointVars[0][2],
1649  jointVars[1][0],
1650  jointVars[1][1],
1651  jointVars[1][2],
1652  NO_UINT,0,&eq1);
1653 
1654  COS_VAR(vname,j->id,ln1,ln2);
1655  NewVariable(SECONDARY_VAR,vname,&var);
1656  IntervalCosine(&(j->range),&ic);
1657  SetVariableInterval(&ic,&var);
1658  DeleteInterval(&ic);
1659  id=AddVariable2CS(&var,cs);
1660  DeleteVariable(&var);
1661 
1662  AddVariable2Monomial(NFUN,id,1,&f);
1663  AddCt2Monomial(-1,&f);
1664  AddMonomial(&f,&eq1);
1665  ResetMonomial(&f);
1666 
1667  SetEquationCmp(EQU,&eq1);
1668  SetEquationType(SYSTEM_EQ,&eq1);
1669  AddEquation2CS(p,&eq1,cs);
1670  DeleteEquation(&eq1);
1671 
1672  if (j->avoidLimits)
1673  {
1674  r=IntervalSize(&ic);
1675  AddTerm2SearchCriterion(j->avoidLimitsWeight/(r*r),id,1,cs);
1676  }
1677 
1678  if ((j->t==REV_JOINT)&&(j->coupled!=NULL))
1679  {
1680  char *ln1p,*ln2p,*vnamep;
1681  unsigned int c1,c2;
1682  Tjoint *jp;
1683 
1684  jp=(Tjoint *)j->coupled;
1685 
1686  COS_VAR(vname,j->id,ln1,ln2);
1687  c1=GetCSVariableID(vname,cs);
1688  if (c1==NO_UINT)
1689  Error("Trying to couple a non-limited revolute joint");
1690 
1691  ln1p=GetLinkName(jp->link[0]);
1692  ln2p=GetLinkName(jp->link[1]);
1693  NEW(vnamep,strlen(ln1p)+strlen(ln2p)+100,char);
1694  COS_VAR(vnamep,jp->id,ln1p,ln2p);
1695  c2=GetCSVariableID(vnamep,cs);
1696  if (c2==NO_UINT)
1697  Error("Trying to couple to a non-limited revolute joint");
1698  free(vnamep);
1699 
1700  /* Add the equation cos1=cos2 */
1701  InitEquation(&eq1);
1702  SetEquationValue(0.0,&eq1);
1703 
1704  AddVariable2Monomial(NFUN,c1,1,&f);
1705  AddMonomial(&f,&eq1);
1706  ResetMonomial(&f);
1707 
1708  AddVariable2Monomial(NFUN,c2,1,&f);
1709  AddCt2Monomial(-1,&f);
1710  AddMonomial(&f,&eq1);
1711  ResetMonomial(&f);
1712 
1713  SetEquationCmp(EQU,&eq1);
1714  SetEquationType(SYSTEM_EQ,&eq1);
1715  AddEquation2CS(p,&eq1,cs);
1716  DeleteEquation(&eq1);
1717  }
1718  }
1719 
1720  }
1721  break;
1722 
1723  case SPH_SPH_JOINT:
1724  /*We assume non-constrained spherical-spherical joints*/
1725  break;
1726 
1727  case IN_PATCH_JOINT:
1728  /*In patch joints are limited by the 4 points defining the patch.
1729  These limits are implicitly given by the ranges for the control
1730  variables giving the patch. */
1731  if (j->avoidLimits)
1732  {
1733  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,0);
1734  nv=GetCSVariableID(vname,cs);
1735  AddTerm2SearchCriterion(j->avoidLimitsWeight*4,nv,0.5,cs);
1736 
1737  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,1);
1738  nv=GetCSVariableID(vname,cs);
1739  AddTerm2SearchCriterion(j->avoidLimitsWeight*4,nv,0.5,cs);
1740  }
1741  break;
1742 
1743  default:
1744  Error("Undefined joint type in GenerateJointRangeEquations");
1745  }
1746 
1747  DeleteInterval(&rangeOne);
1748  DeleteInterval(&rangeInf);
1749 
1750  free(vname);
1751  DeleteMonomial(&f);
1752  }
1753 }
1754 
1755 void GenerateJointEquationsInBranch(Tparameters *p,double s,TCuikSystem *cs,
1756  Tequation *eq,Tjoint *j)
1757 {
1758  unsigned int r;
1759 
1760  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1761 
1762  if (r==REP_JOINTS)
1763  Error("GenerateJointEquationsInBranch should not be used with JOINT-based representation");
1764  else
1765  {
1766  char *l1name,*l2name,*vname;
1767  unsigned int i,vID,d;
1768  Tmonomial f;
1769 
1770  l1name=GetLinkName(j->link[0]);
1771  l2name=GetLinkName(j->link[1]);
1772 
1773  switch (j->t)
1774  {
1775  case FREE_JOINT:
1776 
1777  NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
1778  InitMonomial(&f);
1779  for(i=0;i<3;i++)
1780  {
1781  FREE_JOINT_VAR(vname,j->id,j->linkID[0],l1name,j->linkID[1],l2name,i);
1782 
1783  vID=GetCSVariableID(vname,cs);
1784  if (vID==NO_UINT)
1785  Error("Undefined FREE_JOINT variable in GenerateJointEquationsInBranch");
1786 
1787  AddVariable2Monomial(NFUN,vID,1,&f);
1788  AddCt2Monomial(s,&f);
1789  AddMonomial(&f,&(eq[i]));
1790  ResetMonomial(&f);
1791  }
1792  DeleteMonomial(&f);
1793  free(vname);
1794 
1795  break;
1796 
1797  case FIX_JOINT:
1798  /*The translation part of the homogeneous transform between the two
1799  links has been stored in points[0] (see NewFixJoint). We apply
1800  it here. The rotation constraints are imposed in
1801  GenerateJointEquations. */
1802  ApplyLinkRot(p, s,NO_UINT,j->points[0],eq,cs,
1803  IsGroundLink(j->linkID[0]),j->link[0]);
1804  break;
1805 
1806  case UNV_JOINT:
1807  case REV_JOINT:
1808  case SPH_JOINT:
1809  ApplyLinkRot(p, s,NO_UINT,j->points[0],eq,cs,
1810  IsGroundLink(j->linkID[0]),j->link[0]);
1811  ApplyLinkRot(p,-s,NO_UINT,j->points[2],eq,cs,
1812  IsGroundLink(j->linkID[1]),j->link[1]);
1813  break;
1814 
1815  case PRS_JOINT:
1816  ApplyLinkRot(p, s,NO_UINT,j->points[0],eq,cs,
1817  IsGroundLink(j->linkID[0]),j->link[0]);
1818 
1819  NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
1820  PRS_JOINT_VAR(vname,j->id,l1name,l2name);
1821  d=GetCSVariableID(vname,cs);
1822  free(vname);
1823  if (d==NO_UINT)
1824  Error("Undefined PRS_JOINT variable in GenerateJointEquationsInBranch");
1825 
1826  ApplyLinkRot(p, s,d,j->normals[0],eq,cs,
1827  IsGroundLink(j->linkID[0]),j->link[0]);
1828 
1829  ApplyLinkRot(p,-s,NO_UINT,j->points[2],eq,cs,
1830  IsGroundLink(j->linkID[1]),j->link[1]);
1831  break;
1832 
1833  case SPH_SPH_JOINT:
1834  ApplyLinkRot(p, s,NO_UINT,j->points[0],eq,cs,
1835  IsGroundLink(j->linkID[0]),j->link[0]);
1836 
1837  NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
1838  InitMonomial(&f);
1839  for(i=0;i<3;i++)
1840  {
1841  SPH_SPH_JOINT_VAR(vname,j->id,l1name,l2name,i);
1842 
1843  vID=GetCSVariableID(vname,cs);
1844  if (vID==NO_UINT)
1845  Error("Undefined SPH_SPH_JOINT variable in GenerateJointEquationsInBranch");
1846 
1847  AddVariable2Monomial(NFUN,vID,1,&f);
1848  AddCt2Monomial(s*j->length,&f);
1849  AddMonomial(&f,&(eq[i]));
1850  ResetMonomial(&f);
1851  }
1852  DeleteMonomial(&f);
1853  free(vname);
1854 
1855  ApplyLinkRot(p,-s,NO_UINT,j->points[2],eq,cs,
1856  IsGroundLink(j->linkID[1]),j->link[1]);
1857  break;
1858 
1859 
1860  case SPH_PRS_SPH_JOINT:
1861 
1862  ApplyLinkRot(p, s,NO_UINT,j->points[0],eq,cs,
1863  IsGroundLink(j->linkID[0]),j->link[0]);
1864 
1865  NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
1866 
1867  PRS_JOINT_VAR(vname,j->id,l1name,l2name);
1868  d=GetCSVariableID(vname,cs);
1869  if (d==NO_UINT)
1870  Error("Undefined SPH_PRS_SPH_JOINT translation variable in GenerateJointEquationsInBranch");
1871 
1872  InitMonomial(&f);
1873  for(i=0;i<3;i++)
1874  {
1875  SPH_SPH_JOINT_VAR(vname,j->id,l1name,l2name,i);
1876 
1877  vID=GetCSVariableID(vname,cs);
1878  if (vID==NO_UINT)
1879  Error("Undefined SPH_PRS_SPH_JOINT vector variable in GenerateJointEquationsInBranch");
1880 
1881  AddVariable2Monomial(NFUN,vID,1,&f);
1882  AddVariable2Monomial(NFUN,d,1,&f);
1883  AddCt2Monomial(s,&f);
1884  AddMonomial(&f,&(eq[i]));
1885  ResetMonomial(&f);
1886  }
1887  DeleteMonomial(&f);
1888  free(vname);
1889 
1890  ApplyLinkRot(p,-s,NO_UINT,j->points[2],eq,cs,
1891  IsGroundLink(j->linkID[1]),j->link[1]);
1892  break;
1893 
1894  case IN_PATCH_JOINT:
1895  ApplyLinkRot(p, s,NO_UINT,j->points[0],eq,cs,
1896  IsGroundLink(j->linkID[0]),j->link[0]);
1897 
1898  ApplyLinkRot(p,-s,NO_UINT,j->points[2],eq,cs,
1899  IsGroundLink(j->linkID[1]),j->link[1]);
1900  NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
1901  for(i=0;i<3;i++)
1902  {
1903  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,l1name,l2name,i);
1904  vID=GetCSVariableID(vname,cs);
1905  if (vID==NO_UINT)
1906  Error("Undefined IN_PATCH_JOINT variable in GenerateJointEquationsInBranch");
1907 
1908  ApplyLinkRot(p,-s,vID,j->points[3+i],eq,cs,
1909  IsGroundLink(j->linkID[1]),j->link[1]);
1910  }
1911  free(vname);
1912  break;
1913 
1914  default:
1915  Error("Unkownd joint type");
1916  }
1917  }
1918 }
1919 
1920 void GenerateJointEquations(Tparameters *p,double maxCoord,
1921  TCuikSystem *cs,Tjoint *j)
1922 {
1923  unsigned int r;
1924 
1925  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1926 
1927  if (r==REP_JOINTS)
1928  {
1929  unsigned int i,n;
1930  char *vname;
1931  Tvariable var;
1932  Tinterval range;
1933  char *ln1,*ln2;
1934  unsigned int tp;
1935 
1936  ln1=GetLinkName(j->link[0]);
1937  ln2=GetLinkName(j->link[1]);
1938 
1939  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
1940 
1941  n=GetJointDOF(j);
1942  for(i=0;i<n;i++)
1943  {
1944  DOF_VAR(vname,j->id,ln1,ln2,i);
1945  NewVariable(SYSTEM_VAR,vname,&var);
1946  GetJointRangeN(i,maxCoord,&range,j);
1947  SetVariableInterval(&range,&var);
1948  tp=GetJointRangeTopology(i,j);
1949  if (tp==TOPOLOGY_S)
1950  SetVariableTopology(TOPOLOGY_S,&var);
1951 
1952  AddVariable2CS(&var,cs);
1953  DeleteVariable(&var);
1954  DeleteInterval(&range);
1955  }
1956  free(vname);
1957  }
1958  else
1959  {
1960  double v[3];
1961  unsigned int i,k;
1962  Tequation eq[3],eq1;
1963  char *ln1,*ln2,*vname;
1964  Tinterval rangeOne,rangeInf,rangeZeroOne;
1965  Tmonomial f;
1966  unsigned int vID[3]; /* variables generated */
1967  unsigned int vIDuni[2][3]; /* for the universal joints -> 2 vectors*/
1968  unsigned int l; /*scale variable of the normal vectors in_patch_joint*/
1969  Tvariable var;
1970 
1971  ln1=GetLinkName(j->link[0]);
1972  ln2=GetLinkName(j->link[1]);
1973 
1974  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
1975 
1976  NewInterval(-1,1,&rangeOne);
1977  NewInterval(0,1,&rangeZeroOne);
1978 
1979  InitMonomial(&f);
1980 
1981  switch(j->t)
1982  {
1983  case FREE_JOINT:
1984  /*We generate 3 new variables representing the (unlimited)
1985  displacement from the origin of the first link to the
1986  origin of the second link.
1987  No equations constraint these variables. */
1988  NewInterval(-maxCoord,maxCoord,&rangeInf);
1989  for(i=0;i<3;i++)
1990  {
1991  FREE_JOINT_VAR(vname,j->id,j->linkID[0],ln1,j->linkID[1],ln2,i);
1992 
1993  NewVariable(SYSTEM_VAR,vname,&var);
1994  SetVariableInterval(&rangeInf,&var);
1995  AddVariable2CS(&var,cs);
1996 
1997  DeleteVariable(&var);
1998  }
1999  break;
2000 
2001  case FIX_JOINT:
2002  /* link1 * transform = link2 * identity */
2003  /* link1 * transform - link2 * identity = 0 */
2004  for(i=0;i<3;i++)
2005  {
2006  for(k=0;k<3;k++)
2007  InitEquation(&(eq[k]));
2008 
2009  for(k=0;k<3;k++)
2010  v[k]=HTransformGetElement(k,i,&(j->preT));
2011 
2012  ApplyLinkRot(p,1,NO_UINT,v,eq,cs,
2013  IsGroundLink(j->linkID[0]),j->link[0]);
2014 
2015  v[0]=v[1]=v[2]=0;
2016  v[i]=1;
2017 
2018  ApplyLinkRot(p,-1,NO_UINT,v,eq,cs,
2019  IsGroundLink(j->linkID[1]),j->link[1]);
2020 
2021  for(k=0;k<3;k++)
2022  {
2023  SetEquationType(SYSTEM_EQ,&(eq[k]));
2024  SetEquationCmp(EQU,&(eq[k]));
2025  AddEquation2CS(p,&(eq[k]),cs);
2026  DeleteEquation(&(eq[k]));
2027  }
2028  }
2029  break;
2030 
2031  case PRS_JOINT:
2032  /* The three reference vectors (X,Y,Z) for the two links
2033  are the same when transformed to global coordinates. */
2034  for(k=0;k<3;k++)
2035  InitEquation(&(eq[k]));
2036 
2037  for(i=0;i<3;i++)
2038  {
2039  v[0]=v[1]=v[2]=0.0;
2040  v[i]=1.0;
2041 
2042  for(k=0;k<3;k++)
2043  InitEquation(&(eq[k]));
2044 
2045  for(k=0;k<2;k++)
2046  ApplyLinkRot(p,(k==0?+1.0:-1.0),NO_UINT,v,eq,cs,
2047  IsGroundLink(j->linkID[k]),j->link[k]);
2048 
2049  for(k=0;k<3;k++)
2050  {
2051  SetEquationType(SYSTEM_EQ,&(eq[k]));
2052  SetEquationCmp(EQU,&(eq[k]));
2053  AddEquation2CS(p,&(eq[k]),cs);
2054  DeleteEquation(&(eq[k]));
2055  }
2056  }
2057  break;
2058 
2059  case REV_JOINT:
2060  /* The vector defining the rotation axis is the same as seen
2061  from the two links. */
2062  for(k=0;k<3;k++)
2063  InitEquation(&(eq[k]));
2064 
2065  for(k=0;k<2;k++) /* for the two links involved in the joint */
2066  ApplyLinkRot(p,(k==0?+1.0:-1.0),NO_UINT,j->normals[k],eq,cs,
2067  IsGroundLink(j->linkID[k]),j->link[k]);
2068 
2069  for(k=0;k<3;k++)
2070  {
2071  SetEquationType(SYSTEM_EQ,&(eq[k]));
2072  SetEquationCmp(EQU,&(eq[k]));
2073  AddEquation2CS(p,&(eq[k]),cs);
2074  DeleteEquation(&(eq[k]));
2075  }
2076  break;
2077 
2078  case UNV_JOINT:
2079  /* Two vectors one attached to link1 and another attached to link2 */
2080  for(i=0;i<2;i++)
2081  {
2082  for(k=0;k<3;k++)
2083  InitEquation(&(eq[k]));
2084 
2085  ApplyLinkRot(p,1,NO_UINT,j->normals[i],eq,cs,
2086  IsGroundLink(j->linkID[i]),j->link[i]);
2087 
2088  for(k=0;k<3;k++)
2089  {
2090  UNV_JOINT_VAR(vname,j->id,ln1,ln2,i,k);
2091 
2092  NewVariable(SECONDARY_VAR,vname,&var);
2093  SetVariableInterval(&rangeOne,&var);
2094 
2095  vIDuni[i][k]=AddVariable2CS(&var,cs);
2096 
2097  DeleteVariable(&var);
2098 
2099  AddCt2Monomial(-1.0,&f);
2100  AddVariable2Monomial(NFUN,vIDuni[i][k],1,&f);
2101  AddMonomial(&f,&(eq[k]));
2102  ResetMonomial(&f);
2103 
2104  SetEquationType(SYSTEM_EQ,&(eq[k]));
2105  SetEquationCmp(EQU,&(eq[k]));
2106  AddEquation2CS(p,&(eq[k]),cs);
2107  DeleteEquation(&(eq[k]));
2108  }
2109 
2110  }
2111 
2112  /* the two vectors must be orthogonal */
2113  GenerateDotProductEquation(vIDuni[0][0],vIDuni[0][1],vIDuni[0][2],
2114  vIDuni[1][0],vIDuni[1][1],vIDuni[1][2],
2115  NO_UINT,0,&eq1);
2116  AddEquation2CS(p,&eq1,cs);
2117  DeleteEquation(&eq1);
2118  break;
2119 
2120  case SPH_JOINT:
2121  /*spherical joints introduce no joint equations (all the constraints
2122  of a spherical joint are in the loop equations.*/
2123  break;
2124 
2125  case SPH_SPH_JOINT:
2126  case SPH_PRS_SPH_JOINT:
2127  /* We define a vector (with norm 1) along the sph-sph axis.
2128  These variables are later used in the loops. */
2129  for(i=0;i<3;i++)
2130  {
2131  SPH_SPH_JOINT_VAR(vname,j->id,ln1,ln2,i);
2132 
2133  NewVariable(SYSTEM_VAR,vname,&var);
2134  SetVariableInterval(&rangeOne,&var);
2135 
2136  vID[i]=AddVariable2CS(&var,cs);
2137 
2138  DeleteVariable(&var);
2139  }
2140  GenerateNormEquation(vID[0],vID[1],vID[2],1.0,&eq1);
2141  AddEquation2CS(p,&eq1,cs);
2142  DeleteEquation(&eq1);
2143 
2144  break;
2145 
2146  case IN_PATCH_JOINT:
2147  /* Define the path control variables (u,v) and the dummy w=u*w */
2148  for(i=0;i<3;i++)
2149  {
2150  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,i);
2151 
2152  NewVariable((i<2?SYSTEM_VAR:DUMMY_VAR),vname,&var);
2153  SetVariableInterval(&rangeZeroOne,&var);
2154 
2155  vID[i]=AddVariable2CS(&var,cs);
2156 
2157  DeleteVariable(&var);
2158  }
2159 
2160  /* define the variable for the norm of the normal vector */
2161  IN_PATCH_JOINT_SCALE_VAR(vname,j->id,ln1,ln2);
2162 
2163  NewVariable(SECONDARY_VAR,vname,&var);
2164  SetVariableInterval(&(j->normRange),&var);
2165  l=AddVariable2CS(&var,cs);
2166  DeleteVariable(&var);
2167 
2168  /* define the equation for the dummy variable w=u*v */
2169  GenerateSaddleEquation(vID[0],vID[1],vID[2],&eq1);
2170  AddEquation2CS(p,&eq1,cs);
2171  DeleteEquation(&eq1);
2172 
2173  /*Define the equation giving the (square) norm of the normal vector
2174  (i.e., the equation defining 'l')*/
2175  InitEquation(&eq1);
2176  SetEquationValue(0.0,&eq1);
2177  /*n0*n0*/
2178  AddCt2Monomial(DotProduct(j->normals[0],j->normals[0]),&f);
2179  AddMonomial(&f,&eq1);
2180  ResetMonomial(&f);
2181  /*+n1*n1*u^2*/
2182  AddCt2Monomial(DotProduct(j->normals[1],j->normals[1]),&f);
2183  AddVariable2Monomial(NFUN,vID[0],2,&f);
2184  AddMonomial(&f,&eq1);
2185  ResetMonomial(&f);
2186  /*+n2*n2*v^2*/
2187  AddCt2Monomial(DotProduct(j->normals[2],j->normals[2]),&f);
2188  AddVariable2Monomial(NFUN,vID[1],2,&f);
2189  AddMonomial(&f,&eq1);
2190  ResetMonomial(&f);
2191  /*+2*n0*n1*u*/
2192  AddCt2Monomial(2*DotProduct(j->normals[0],j->normals[1]),&f);
2193  AddVariable2Monomial(NFUN,vID[0],1,&f);
2194  AddMonomial(&f,&eq1);
2195  ResetMonomial(&f);
2196  /*+2*n0*n2*v*/
2197  AddCt2Monomial(2*DotProduct(j->normals[0],j->normals[2]),&f);
2198  AddVariable2Monomial(NFUN,vID[1],1,&f);
2199  AddMonomial(&f,&eq1);
2200  ResetMonomial(&f);
2201  /*+2*n1*n2*u*v=2*n1*n2*w*/
2202  AddCt2Monomial(2*DotProduct(j->normals[1],j->normals[2]),&f);
2203  AddVariable2Monomial(NFUN,vID[2],1,&f);
2204  AddMonomial(&f,&eq1);
2205  ResetMonomial(&f);
2206 
2207  AddCt2Monomial(-1.0,&f); /*-l^2*/
2208  AddVariable2Monomial(NFUN,l,2,&f);
2209  AddMonomial(&f,&eq1);
2210  ResetMonomial(&f);
2211 
2212  SetEquationType(SYSTEM_EQ,&eq1);
2213  SetEquationCmp(EQU,&eq1);
2214  AddEquation2CS(p,&eq1,cs);
2215 
2216  DeleteEquation(&eq1);
2217 
2218  /* Define the equations aligning the unitary vector on non-patch
2219  link with the normal vector on the patch. Note that the vector
2220  on the non-patch link is scaled by the norm of the patch
2221  normal vector. */
2222  for(k=0;k<3;k++)
2223  InitEquation(&(eq[k]));
2224 
2225  ApplyLinkRot(p, 1,l,j->points[1],eq,cs,
2226  IsGroundLink(j->linkID[0]),j->link[0]);
2227 
2228  ApplyLinkRot(p,-1,NO_UINT,j->normals[0],eq,cs,
2229  IsGroundLink(j->linkID[1]),j->link[1]);
2230  ApplyLinkRot(p,-1, vID[0],j->normals[1],eq,cs,
2231  IsGroundLink(j->linkID[1]),j->link[1]);
2232  ApplyLinkRot(p,-1, vID[1],j->normals[2],eq,cs,
2233  IsGroundLink(j->linkID[1]),j->link[1]);
2234 
2235  for(k=0;k<3;k++)
2236  {
2237  SetEquationType(SYSTEM_EQ,&(eq[k]));
2238  SetEquationCmp(EQU,&(eq[k]));
2239  AddEquation2CS(p,&(eq[k]),cs);
2240  DeleteEquation(&(eq[k]));
2241  }
2242 
2243  break;
2244 
2245  default:
2246  Error("Unknown joint type in GenerateJointEquations");
2247  }
2248 
2249  free(vname);
2250  DeleteInterval(&rangeOne);
2251  DeleteMonomial(&f);
2252  }
2253 }
2254 
2255 
2256 void RegenerateJointSolution(Tparameters *p,TCuikSystem *cs,double *sol,
2257  Tjoint *j)
2258 {
2259  unsigned int r;
2260 
2261  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2262 
2263  /* for DOF representations -> nothing to reconstruct */
2264  if (r!=REP_JOINTS)
2265  {
2266  unsigned int i;
2267  unsigned int vID[3];
2268  char *vname,*ln1,*ln2;
2269 
2270  ln1=GetLinkName(j->link[0]);
2271  ln2=GetLinkName(j->link[1]);
2272 
2273  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
2274 
2275  switch(j->t)
2276  {
2277  case IN_PATCH_JOINT:
2278 
2279  for(i=0;i<3;i++)
2280  {
2281  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,i);
2282  vID[i]=GetCSVariableID(vname,cs);
2283  }
2284 
2285  sol[vID[2]]=sol[vID[0]]*sol[vID[1]];
2286  break;
2287  }
2288  free(vname);
2289  }
2290 }
2291 
2292 void RegenerateJointBox(Tparameters *p,TCuikSystem *cs,Tbox *b,Tjoint *j)
2293 {
2294  unsigned int r;
2295  char *vname,*ln1,*ln2;
2296 
2297  ln1=GetLinkName(j->link[0]);
2298  ln2=GetLinkName(j->link[1]);
2299  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
2300 
2301  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2302 
2303  /* for DOF representations -> nothing to reconstruct */
2304  if (r!=REP_JOINTS)
2305  {
2306  unsigned int i;
2307  unsigned int vID[3];
2308  Tinterval r;
2309 
2310  switch(j->t)
2311  {
2312  case IN_PATCH_JOINT:
2313  for(i=0;i<3;i++)
2314  {
2315  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,i);
2316  vID[i]=GetCSVariableID(vname,cs);
2317  }
2318 
2319  IntervalProduct(GetBoxInterval(vID[0],b),
2320  GetBoxInterval(vID[1],b),&r);
2321 
2322  SetBoxInterval(vID[2],&r,b);
2323 
2324  break;
2325  }
2326  }
2327  free(vname);
2328 }
2329 
2330 void GenerateJointSolution(Tparameters *p,double *dof,THTransform *t1,
2331  THTransform *t2,TCuikSystem *cs,double *sol,
2332  Tjoint *j)
2333 {
2334  unsigned int r;
2335 
2336  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2337 
2338  if (r==REP_JOINTS)
2339  Error("GenerateJointSolution is not defined for JOINTS-based representations");
2340  else
2341  {
2342  char *vname,*ln1,*ln2;
2343  unsigned int i,k,vID;
2344  double vector[4][3];
2345  double d[3];
2346 
2347  ln1=GetLinkName(j->link[0]);
2348  ln2=GetLinkName(j->link[1]);
2349 
2350  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
2351 
2352  switch(j->t)
2353  {
2354  case FREE_JOINT:
2355  for(i=0;i<3;i++)
2356  {
2357  FREE_JOINT_VAR(vname,j->id,j->linkID[0],ln1,j->linkID[1],ln2,i);
2358 
2359  vID=GetCSVariableID(vname,cs);
2360  if (vID==NO_UINT)
2361  Error("Undefined variable in GenerateJointSolution");
2362 
2363  sol[vID]=dof[i];
2364  }
2365  break;
2366 
2367  case FIX_JOINT:
2368  break;
2369 
2370  case PRS_JOINT:
2371  PRS_JOINT_VAR(vname,j->id,ln1,ln2);
2372 
2373  vID=GetCSVariableID(vname,cs);
2374  if (vID==NO_UINT)
2375  Error("Undefined variable in GenerateJointSolution");
2376 
2377  sol[vID]=dof[0];
2378  break;
2379 
2380  case UNV_JOINT:
2381  for(i=0;i<2;i++)
2382  {
2383  /* We only rotate vectors (do not use the translation in
2384  the homogeneous matrix). */
2385  HTransformApplyRot(j->normals[i],vector[2+i],(i==0?t1:t2));
2386 
2387  for(k=0;k<3;k++)
2388  {
2389  UNV_JOINT_VAR(vname,j->id,ln1,ln2,i,k);
2390 
2391  vID=GetCSVariableID(vname,cs);
2392  if (vID==NO_UINT)
2393  Error("Undefined variable in GenerateJointSolution");
2394  sol[vID]=vector[2+i][k];
2395  }
2396  }
2397  /* we continue to the following block (no break here) */
2398 
2399  case REV_JOINT:
2400  case SPH_JOINT:
2401  if (j->hasLimits)
2402  {
2403  for(i=0;i<2;i++)
2404  {
2405  /* We only rotate vectors (do not use the translation in
2406  the homogeneous matrix). */
2407  HTransformApplyRot(j->vrange[i],vector[i],(i==0?t1:t2));
2408 
2409  for(k=0;k<3;k++)
2410  {
2411  if (j->t==REV_JOINT)
2412  ROT_JOINT_VAR_REF(vname,j->id,i,ln1,ln2,k);
2413  else
2414  {
2415  if (j->t==SPH_JOINT)
2416  SPH_JOINT_VAR_REF(vname,j->id,i,ln1,ln2,k);
2417  else
2418  UNV_JOINT_VAR_REF(vname,j->id,i,ln1,ln2,k);
2419  }
2420 
2421  vID=GetCSVariableID(vname,cs);
2422  if (vID==NO_UINT)
2423  Error("Undefined variable in GenerateJointSolution");
2424 
2425  sol[vID]=vector[i][k];
2426  }
2427  }
2428  if (j->t==UNV_JOINT)
2429  {
2430  for(k=0;k<2;k++)
2431  {
2432  COS_VAR_UNI(vname,j->id,ln1,ln2,k);
2433  vID=GetCSVariableID(vname,cs);
2434  if (vID==NO_UINT)
2435  Error("Undefined variable in GenerateJointSolution");
2436 
2437  sol[vID]=DotProduct(vector[3-k],vector[k]);
2438  }
2439  }
2440  else
2441  {
2442  COS_VAR(vname,j->id,ln1,ln2);
2443  vID=GetCSVariableID(vname,cs);
2444  if (vID==NO_UINT)
2445  Error("Undefined variable in GenerateJointSolution");
2446 
2447  sol[vID]=DotProduct(vector[0],vector[1]);
2448  }
2449  }
2450  break;
2451 
2452  case SPH_PRS_SPH_JOINT:
2453  PRS_JOINT_VAR(vname,j->id,ln1,ln2);
2454 
2455  vID=GetCSVariableID(vname,cs);
2456  if (vID==NO_UINT)
2457  Error("Undefined variable in GenerateJointSolution");
2458 
2459  sol[vID]=dof[2];
2460  /* Do not break. Continue to SPH_SPH_JOINT */
2461 
2462  case SPH_SPH_JOINT:
2463  /* Define a normal vector along the leg. The leg position is defined
2464  by the two end-points. */
2465  HTransformApply(j->points[0],vector[0],t1);
2466  HTransformApply(j->points[2],vector[1],t2);
2467 
2468  DifferenceVector(3,vector[1],vector[0],d);
2469  Normalize(3,d);
2470 
2471  for(i=0;i<3;i++)
2472  {
2473  SPH_SPH_JOINT_VAR(vname,j->id,ln1,ln2,i);
2474 
2475  vID=GetCSVariableID(vname,cs);
2476  if (vID==NO_UINT)
2477  Error("Undefined SPH_SPH_JOINT variable in GenerateJointSolution");
2478 
2479  sol[vID]=d[i];
2480  }
2481  break;
2482 
2483  case IN_PATCH_JOINT:
2484  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,0);
2485  vID=GetCSVariableID(vname,cs);
2486  if (vID==NO_UINT)
2487  Error("Undefined variable in GenerateJointSolution");
2488 
2489  sol[vID]=dof[0];
2490 
2491  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,1);
2492  vID=GetCSVariableID(vname,cs);
2493  if (vID==NO_UINT)
2494  Error("Undefined variable in GenerateJointSolution");
2495 
2496  sol[vID]=dof[1];
2497 
2498  IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,2);
2499  vID=GetCSVariableID(vname,cs);
2500  if (vID==NO_UINT)
2501  Error("Undefined variable in GenerateJointSolution");
2502 
2503  sol[vID]=dof[0]*dof[1];
2504 
2505  IN_PATCH_JOINT_SCALE_VAR(vname,j->id,ln1,ln2);
2506  vID=GetCSVariableID(vname,cs);
2507  if (vID==NO_UINT)
2508  Error("Undefined variable in GenerateJointSolution");
2509 
2510  sol[vID]=sqrt(DotProduct(j->normals[0],j->normals[0])+
2511  DotProduct(j->normals[1],j->normals[1])*dof[0]*dof[0]+
2512  DotProduct(j->normals[2],j->normals[2])*dof[1]*dof[1]+
2513  2*DotProduct(j->normals[0],j->normals[1])*dof[0]+
2514  2*DotProduct(j->normals[0],j->normals[2])*dof[1]+
2515  2*DotProduct(j->normals[1],j->normals[2])*dof[0]*dof[1]);
2516 
2517  break;
2518 
2519  default:
2520  Error("Unknown joint type in GenerateJointSolution");
2521 
2522  }
2523  free(vname);
2524  }
2525 }
2526 
2527 void GetJointTransform(double *dof,THTransform *t,Tjoint *j)
2528 {
2529  THTransform T;
2530 
2531  switch(j->t)
2532  {
2533  case FREE_JOINT:
2534  HTransformRx(dof[5],t);
2535  HTransformRy(dof[4],&T);HTransformProduct(&T,t,t);
2536  HTransformRz(dof[3],&T);HTransformProduct(&T,t,t);
2537  HTransformTxyz(dof[0],dof[1],dof[2],&T);HTransformProduct(&T,t,t);
2538  HTransformDelete(&T);
2539  break;
2540 
2541  case FIX_JOINT:
2542  HTransformCopy(t,&(j->preT));
2543  break;
2544 
2545  case REV_JOINT:
2546  HTransformCopy(t,&(j->preT));
2547  HTransformAcumRot(RX,sin(dof[0]),cos(dof[0]),t);
2548  HTransformProduct(t,&(j->postT),t);
2549  break;
2550 
2551  case UNV_JOINT:
2552  HTransformRx(dof[0],&T);
2553  HTransformAcumRot(RZ,1,0,&T);
2554  HTransformRx(M_PI+dof[1],t);
2555  HTransformProduct(&T,t,&T);
2556 
2557  HTransformProduct(&(j->preT),&T,t);
2558  HTransformProduct(t,&(j->postT),t);
2559 
2560  HTransformDelete(&T);
2561  break;
2562 
2563  case SPH_JOINT:
2564  HTransformRz(dof[0],t);
2565  HTransformRy(dof[1],&T);HTransformProduct(t,&T,t);
2566  HTransformRx(dof[2],&T);HTransformProduct(t,&T,t);
2567 
2568  HTransformProduct(&(j->preT),t,t);
2569  HTransformProduct(t,&(j->postT),t);
2570  break;
2571 
2572  case PRS_JOINT:
2573  HTransformTxyz(dof[0]*j->normals[0][0],
2574  dof[0]*j->normals[0][1],
2575  dof[0]*j->normals[0][2],&T);
2576 
2577  HTransformProduct(&(j->preT),&T,t);
2578  HTransformProduct(t,&(j->postT),t);
2579 
2580  HTransformDelete(&T);
2581  break;
2582 
2583  case SPH_SPH_JOINT:
2584  HTransformRz(dof[0],t);
2585  HTransformRy(dof[1],&T);HTransformProduct(t,&T,t);
2586  HTransformTx(j->length,&T);HTransformProduct(t,&T,t);
2587  HTransformRz(dof[2],&T);HTransformProduct(t,&T,t);
2588  HTransformRy(dof[3],&T);HTransformProduct(t,&T,t);
2589  HTransformRx(dof[4],&T);HTransformProduct(t,&T,t);
2590 
2591  HTransformProduct(&(j->preT),t,t);
2592  HTransformProduct(t,&(j->postT),t);
2593  break;
2594 
2595  case SPH_PRS_SPH_JOINT:
2596  HTransformRz(dof[0],t);
2597  HTransformRy(dof[1],&T);HTransformProduct(t,&T,t);
2598  HTransformTx(dof[2],&T);HTransformProduct(t,&T,t);
2599  HTransformRz(dof[3],&T);HTransformProduct(t,&T,t);
2600  HTransformRy(dof[4],&T);HTransformProduct(t,&T,t);
2601  HTransformRx(dof[5],&T);HTransformProduct(t,&T,t);
2602 
2603  HTransformProduct(&(j->preT),t,t);
2604  HTransformProduct(t,&(j->postT),t);
2605  break;
2606 
2607  case IN_PATCH_JOINT:
2608  {
2609  double x[3],y[3],z[3],p[3];
2610 
2611  HTransformRx(dof[2],t);
2612 
2613  /* x=n0+u*n1+v*n2 */
2614  SumVectorScale(3,j->normals[0],dof[0],j->normals[1],x);
2615  SumVectorScale(3,x,dof[1],j->normals[2],x);
2616  Normalize(3,x);
2617 
2618  /* y=p02+v*d */
2619  SumVectorScale(3,j->points[3],dof[1],j->points[5],y);
2620  Normalize(3,y);
2621 
2622  /* z=x X y */
2623  CrossProduct(x,y,z);
2624 
2625  /* p=p0+u*p02+v*p01+u*v*d */
2626  SumVectorScale(3,j->points[2],dof[0],j->points[3],p);
2627  SumVectorScale(3,p,dof[1],j->points[4],p);
2628  SumVectorScale(3,p,dof[0]*dof[1],j->points[5],p);
2629 
2630  HTransformFromVectors(x,y,z,p,&T);
2631  HTransformInverse(&T,&T);
2632 
2633  HTransformProduct(t,&T,t);
2634 
2635  HTransformProduct(&(j->preT),t,t);
2636  }
2637  break;
2638 
2639  default:
2640  Error("Unknown joint type in GetJointTransform");
2641  }
2642 }
2643 
2644 void GetJointDOFValues(Tparameters *p,THTransform *t1,THTransform *t2,double *dof,Tjoint *j)
2645 {
2646  THTransform it1,rel,it;
2647  double s,c;
2648 
2649  /* Get the parameters from the relative transfrom from t1 to t2 */
2650  /* t1*rel=t2 -> rel = inv(t1)*t2 */
2651  HTransformInverse(t1,&it1);
2652  HTransformProduct(&it1,t2,&rel);
2653  HTransformDelete(&it1);
2654 
2655  if ((j->t!=SPH_SPH_JOINT)&&(j->t!=SPH_PRS_SPH_JOINT))
2656  {
2657  /* Dicound the pre-post matrices */
2658  HTransformInverse(&(j->preT),&it);
2659  HTransformProduct(&it,&rel,&rel);
2660  HTransformDelete(&it1);
2661  HTransformInverse(&(j->postT),&it);
2662  HTransformProduct(&rel,&it,&rel);
2663  HTransformDelete(&it);
2664  }
2665 
2666  switch(j->t)
2667  {
2668  case FREE_JOINT:
2669  /* Translation is direct */
2670  *dof=HTransformGetElement(AXIS_X,AXIS_H,&rel); dof++;
2671  *dof=HTransformGetElement(AXIS_Y,AXIS_H,&rel); dof++;
2672  *dof=HTransformGetElement(AXIS_Z,AXIS_H,&rel); dof++;
2673  /* do not break */
2674 
2675  case SPH_JOINT:
2676  /* Euler Angles from a Transform matrix */
2677  GetYawPitchRoll(&(dof[0]),&(dof[1]),&(dof[2]),&rel);
2678  break;
2679 
2680  case FIX_JOINT:
2681  /* Check if 'rel' is actually Id? */
2682  break;
2683 
2684  case REV_JOINT:
2685  if (j->coupled==NULL)
2686  {
2687  /* Check if 'rel' is actually a Rx transform? */
2688  c=HTransformGetElement(AXIS_Y,AXIS_Y,&rel);
2689  s=HTransformGetElement(AXIS_Z,AXIS_Y,&rel);
2690 
2691  dof[0]=atan2(s,c);
2692  }
2693  break;
2694 
2695  case UNV_JOINT:
2696  /* An universal joint with a1, and a2 the dof is
2697  Rx(a1)*Rz(pi/2)*Rx(pi)*Rx(a2)
2698  that evaluates to
2699  [ 0, cos(a2), -sin(a2), 0]
2700  [ cos(a1), sin(a1)*sin(a2), cos(a2)*sin(a1), 0]
2701  [ sin(a1), -cos(a1)*sin(a2), -cos(a1)*cos(a2), 0]
2702  [ 0, 0, 0, 1]
2703  */
2704  c=HTransformGetElement(AXIS_Y,AXIS_X,&rel);
2705  s=HTransformGetElement(AXIS_Z,AXIS_X,&rel);
2706  dof[0]=atan2(s,c);
2707 
2708  c= HTransformGetElement(AXIS_X,AXIS_Y,&rel);
2709  s=-HTransformGetElement(AXIS_X,AXIS_Z,&rel);
2710  dof[1]=atan2(s,c);
2711  break;
2712 
2713  case PRS_JOINT:
2714  {
2715  double d[3];
2716 
2717  /* Check that 'rel' is a translation and the translation
2718  is aligned with j->normals[0]? */
2719 
2720  d[0]=HTransformGetElement(AXIS_X,AXIS_H,&rel);
2721  d[1]=HTransformGetElement(AXIS_Y,AXIS_H,&rel);
2722  d[2]=HTransformGetElement(AXIS_Z,AXIS_H,&rel);
2723  dof[0]=Norm(3,d);
2724  }
2725  break;
2726 
2727  case SPH_PRS_SPH_JOINT:
2728  case SPH_SPH_JOINT:
2729  {
2730  THTransform R,T,R1,iP;
2731  double p[3],v[3],d;
2732 
2733  /* Given the vector along the leg (v) and its length (d),
2734  the full transform from link1 to link2 is
2735  rel=Trans2Vect(v)*Tx(d)*R_2(a,b,c)*PostT
2736  where
2737  Trans2Vect(v) = PreT*R_1(a,b)
2738  thus
2739  R_1(a,b)=PreT\inv * Trans2Vect(v)
2740  and
2741  R_2(a,b,c)=(Trans2Vect(v)*Tx(d))\inv * rel * postT\inv
2742  When R_1 is isolated, we can easily identify
2743  a,b given that it is of type Rz(a)*Ry(b)
2744  [ cos(a)*cos(b), -sin(a), cos(a)*sin(b), 0]
2745  [ cos(b)*sin(a), cos(a), sin(a)*sin(b), 0]
2746  [ -sin(b), 0, cos(b), 0]
2747  [ 0, 0, 0, 1]
2748  When R_2 is isolated, we can recover (a,b,c) as
2749  the roll, pitch, and yaw of the rotation.
2750  */
2751 
2752  /* Transform the two end points to local coordinates of link1
2753  (j->point[0] is already in the correct frame)*/
2754  HTransformInverse(t1,&R);
2755  HTransformProduct(&R,t2,&R);
2756  HTransformApply(j->points[2],p,&R);
2757 
2758  DifferenceVector(3,p,j->points[0],v);
2759  d=Norm(3,v);
2760  /* Check if distance in in range? */
2761  SumVectorScale(3,j->points[0],1/d,v,p);
2762 
2763  HTransformX2Vect(1,1,j->points[0],p,&R1);
2764 
2765  HTransformInverse(&(j->preT),&iP);
2766  HTransformProduct(&iP,&R1,&R);
2767 
2768  s=-HTransformGetElement(AXIS_X,AXIS_Y,&R);
2769  c= HTransformGetElement(AXIS_Y,AXIS_Y,&R);
2770  *dof=atan2(s,c); dof++;
2771 
2772  s=-HTransformGetElement(AXIS_Z,AXIS_X,&R);
2773  c= HTransformGetElement(AXIS_Z,AXIS_Z,&R);
2774  *dof=atan2(s,c); dof++;
2775 
2776  if (j->t==SPH_PRS_SPH_JOINT)
2777  { *dof=d;dof++; }
2778  /* else check that the distance is the given one? */
2779 
2780  HTransformTx(d,&T);
2781 
2782  HTransformProduct(&R1,&T,&R1);
2783  HTransformInverse(&R1,&R1);
2784  HTransformProduct(&R1,&rel,&R1);
2785  HTransformInverse(&(j->postT),&iP);
2786  HTransformProduct(&R1,&iP,&R);
2787  /* Check if R is rotation only? */
2788 
2789  GetYawPitchRoll(&(dof[0]),&(dof[1]),&(dof[2]),&R);
2790 
2791  HTransformDelete(&R);
2792  HTransformDelete(&T);
2793  HTransformDelete(&iP);
2794  HTransformDelete(&R1);
2795  }
2796  break;
2797 
2798  case IN_PATCH_JOINT:
2799  {
2800  double x[3],y[3],z[3],t[3];
2801  THTransform R,P;
2802  double d;
2803 
2804  /* Here rel = Rx * P\inv
2805  = Rx * (T(u,v) * R(u,v))\inv
2806  = Rx * R(u,v)\tr * (-T(u,v))
2807  = R * (-T(u,v))
2808  where R is a rotation resulting from Rx * R(u,v)\tr
2809  That is, R is the rotation part of 'rel'
2810  and T(u,v) = -R\inv * rel
2811  and Rx=rel*P(u,v)
2812  */
2813  HTransformCopy(&R,&rel);
2814  HTransformSetElement(AXIS_X,AXIS_H,0,&R);
2815  HTransformSetElement(AXIS_Y,AXIS_H,0,&R);
2816  HTransformSetElement(AXIS_Z,AXIS_H,0,&R);
2817  HTransformInverse(&R,&R);
2818  HTransformProduct(&R,&rel,&R);
2819 
2820  t[0]=-HTransformGetElement(AXIS_X,AXIS_H,&R);
2821  t[1]=-HTransformGetElement(AXIS_Y,AXIS_H,&R);
2822  t[2]=-HTransformGetElement(AXIS_Z,AXIS_H,&R);
2823  HTransformDelete(&R);
2824  DifferenceVector(3,t,j->points[2],t);
2825 
2826  if (Norm(3,j->points[5])<ZERO)
2827  {
2828  double c1[2],c2[2],c3[2];
2829 
2830  /* t-p0=u*p02+v*p01 */
2831  /* solve the overconstrained system */
2832  c1[0]=DotProduct(j->points[3],j->points[3]);
2833  c1[1]=DotProduct(j->points[4],j->points[3]);
2834 
2835  c2[0]=c1[1];
2836  c2[1]=DotProduct(j->points[4],j->points[4]);
2837 
2838  c3[0]=DotProduct(j->points[3],t);
2839  c3[1]=DotProduct(j->points[4],t);
2840 
2841  d=Det2x2(c1,c2);
2842  dof[0]=Det2x2(c3,c2)/d;
2843  dof[1]=Det2x2(c1,c3)/d;
2844  }
2845  else
2846  {
2847  /* t-p0=u*p02+v*p01+u*v*d */
2848  /* solve the lineal system to recover u,v, and w=u*v */
2849  d=Det3x3(j->points[3],j->points[4],j->points[5]);
2850  dof[0]=Det3x3(t,j->points[4],j->points[5])/d;
2851  dof[1]=Det3x3(j->points[3],t,j->points[5])/d;
2852  /* There is no need to recover the last variable
2853  (it is u*v) */
2854  }
2855 
2856  /*Now define P(u,v) to recover Rx*/
2857 
2858  /* x=n0+u*n1+v*n2 */
2859  SumVectorScale(3,j->normals[0],dof[0],j->normals[1],x);
2860  SumVectorScale(3,x,dof[1],j->normals[2],x);
2861  Normalize(3,x);
2862 
2863  /* y=p02+v*d */
2864  SumVectorScale(3,j->points[3],dof[1],j->points[5],y);
2865  Normalize(3,y);
2866 
2867  /* z=x X y */
2868  CrossProduct(x,y,z);
2869 
2870  /* t=p0+u*p02+v*p01+u*v*d */
2871  SumVectorScale(3,j->points[2],dof[0],j->points[3],t);
2872  SumVectorScale(3,t,dof[1],j->points[4],t);
2873  SumVectorScale(3,t,dof[0]*dof[1],j->points[5],t);
2874 
2875  HTransformFromVectors(x,y,z,t,&P);
2876 
2877  HTransformProduct(&rel,&P,&rel);
2878  HTransformDelete(&P);
2879 
2880  /* Chec that rel is actually a Rx? */
2881 
2882  /* Now just recover the rotation */
2883  s=HTransformGetElement(AXIS_Z,AXIS_Y,&rel);
2884  c=HTransformGetElement(AXIS_Y,AXIS_Y,&rel);
2885  dof[2]=atan2(s,c);
2886  }
2887  break;
2888 
2889  default:
2890  Error("Unknow joint type in GetJointDOFValues");
2891  }
2892  HTransformDelete(&rel);
2893 }
2894 
2895 void GetJointTransSeq(Tparameters *p,TCuikSystem *cs,TTransSeq *ts,Tjoint *j)
2896 {
2897  unsigned int r;
2898 
2899  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2900 
2901  if (r!=REP_JOINTS)
2902  Error("GetJointSTransform is only defined for JOINT-based representations");
2903  else
2904  {
2905  THTransform cT;
2906  char *vname;
2907  unsigned int uID,vID;
2908  unsigned int i,id;
2909  char *ln1,*ln2;
2910 
2911  InitTransSeq(ts);
2912 
2913  if (j->coupled!=NULL)
2914  {
2915  ln1=GetLinkName(((Tjoint *)j->coupled)->link[0]);
2916  ln2=GetLinkName(((Tjoint *)j->coupled)->link[1]);
2917  id=((Tjoint *)j->coupled)->id;
2918  }
2919  else
2920  {
2921  ln1=GetLinkName(j->link[0]);
2922  ln2=GetLinkName(j->link[1]);
2923  id=j->id;
2924  }
2925  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);
2926 
2927  switch(j->t)
2928  {
2929  case FREE_JOINT:
2930  for(i=0;i<6;i++)
2931  {
2932  DOF_VAR(vname,id,ln1,ln2,i);
2933  vID=GetCSVariableID(vname,cs);
2934  if (vID==NO_UINT)
2935  Error("Undefined variable in GetJointTransSeq");
2936  switch(i)
2937  {
2938  case 0:
2939  case 1:
2940  case 2:
2941  AddVarTrans2TransSeq(i,1,vID,ts);
2942  break;
2943  case 3:
2944  AddVarTrans2TransSeq(RZ,1,vID,ts);
2945  break;
2946  case 4:
2947  AddVarTrans2TransSeq(RY,1,vID,ts);
2948  break;
2949  case 5:
2950  AddVarTrans2TransSeq(RX,1,vID,ts);
2951  break;
2952  }
2953  }
2954  break;
2955 
2956  case FIX_JOINT:
2957  AddCtTrans2TransSeq(&(j->preT),ts);
2958  break;
2959 
2960  case REV_JOINT:
2961  AddCtTrans2TransSeq(&(j->preT),ts);
2962  DOF_VAR(vname,id,ln1,ln2,0);
2963  vID=GetCSVariableID(vname,cs);
2964  if (vID==NO_UINT)
2965  Error("Undefined variable in GetJointTransSeq");
2966  AddVarTrans2TransSeq(RX,1,vID,ts);
2967 
2968  AddCtTrans2TransSeq(&(j->postT),ts);
2969  break;
2970 
2971  case UNV_JOINT:
2972  AddCtTrans2TransSeq(&(j->preT),ts);
2973 
2974  DOF_VAR(vname,id,ln1,ln2,0);
2975  vID=GetCSVariableID(vname,cs);
2976  if (vID==NO_UINT)
2977  Error("Undefined variable in GetJointTransSeq");
2978  AddVarTrans2TransSeq(RX,1,vID,ts);
2979 
2980  HTransformRz2(1,0,&cT); /* Rz(pi/2) */
2981  AddCtTrans2TransSeq(&cT,ts);
2982 
2983  HTransformRx2(0,-1,&cT); /* Rx(pi) */
2984  AddCtTrans2TransSeq(&cT,ts);
2985 
2986  DOF_VAR(vname,id,ln1,ln2,1);
2987  vID=GetCSVariableID(vname,cs);
2988  if (vID==NO_UINT)
2989  Error("Undefined variable in GetJointTransSeq");
2990  AddVarTrans2TransSeq(RX,1,vID,ts);
2991 
2992  AddCtTrans2TransSeq(&(j->postT),ts);
2993  break;
2994 
2995  case SPH_JOINT:
2996  AddCtTrans2TransSeq(&(j->preT),ts);
2997 
2998  for(i=0;i<3;i++)
2999  {
3000  DOF_VAR(vname,id,ln1,ln2,i);
3001  vID=GetCSVariableID(vname,cs);
3002  if (vID==NO_UINT)
3003  Error("Undefined variable in GetJointTransSeq");
3004  AddVarTrans2TransSeq(RZ-i,1,vID,ts); /* Rz Ry Rx */
3005  }
3006 
3007  AddCtTrans2TransSeq(&(j->postT),ts);
3008  break;
3009 
3010  case PRS_JOINT:
3011  AddCtTrans2TransSeq(&(j->preT),ts);
3012 
3013  DOF_VAR(vname,id,ln1,ln2,0);
3014  vID=GetCSVariableID(vname,cs);
3015  if (vID==NO_UINT)
3016  Error("Undefined variable in GetJointTransSeq");
3017  AddDispTrans2TransSeq(1,vID,j->normals[0],ts);
3018 
3019  AddCtTrans2TransSeq(&(j->postT),ts);
3020  break;
3021 
3022  case SPH_PRS_SPH_JOINT:
3023  AddCtTrans2TransSeq(&(j->preT),ts);
3024 
3025  for(i=0;i<6;i++)
3026  {
3027  DOF_VAR(vname,id,ln1,ln2,i);
3028  vID=GetCSVariableID(vname,cs);
3029  if (vID==NO_UINT)
3030  Error("Undefined variable in GetJointTransSeq");
3031  switch(i)
3032  {
3033  case 0:
3034  case 3:
3035  AddVarTrans2TransSeq(RZ,1,vID,ts);
3036  break;
3037  case 1:
3038  case 4:
3039  AddVarTrans2TransSeq(RY,1,vID,ts);
3040  break;
3041  case 2:
3042  AddVarTrans2TransSeq(TX,1,vID,ts);
3043  break;
3044  case 5:
3045  AddVarTrans2TransSeq(RX,1,vID,ts);
3046  break;
3047  }
3048  }
3049 
3050  AddCtTrans2TransSeq(&(j->postT),ts);
3051  break;
3052 
3053  case SPH_SPH_JOINT:
3054  AddCtTrans2TransSeq(&(j->preT),ts);
3055 
3056  for(i=0;i<5;i++)
3057  {
3058  DOF_VAR(vname,id,ln1,ln2,i);
3059  vID=GetCSVariableID(vname,cs);
3060  if (vID==NO_UINT)
3061  Error("Undefined variable in GetJointTransSeq");
3062  switch(i)
3063  {
3064  case 0:
3065  case 2:
3066  if (i==2)
3067  {
3068  HTransformTx(j->length,&cT);
3069  AddCtTrans2TransSeq(&cT,ts);
3070  }
3071  AddVarTrans2TransSeq(RZ,1,vID,ts);
3072  break;
3073  case 1:
3074  case 3:
3075  AddVarTrans2TransSeq(RY,1,vID,ts);
3076  break;
3077  case 4:
3078  AddVarTrans2TransSeq(RX,1,vID,ts);
3079  break;
3080  }
3081  }
3082 
3083  AddCtTrans2TransSeq(&(j->postT),ts);
3084  break;
3085 
3086  case IN_PATCH_JOINT:
3087  AddCtTrans2TransSeq(&(j->preT),ts);
3088 
3089  DOF_VAR(vname,id,ln1,ln2,2);
3090  vID=GetCSVariableID(vname,cs);
3091  AddVarTrans2TransSeq(RX,1,vID,ts);
3092 
3093  DOF_VAR(vname,id,ln1,ln2,0);
3094  uID=GetCSVariableID(vname,cs);
3095 
3096  DOF_VAR(vname,id,ln1,ln2,1);
3097  vID=GetCSVariableID(vname,cs);
3098 
3099  /* The patch transform inverted */
3100  {
3101  /* Get the combination of points defined from the patch vertices */
3102  unsigned int k;
3103  double **p;
3104 
3105  NEW(p,4,double*);
3106  for(k=0;k<4;k++)
3107  { NEW(p[k],3,double); }
3108  memcpy(p[0],j->points[2],3*sizeof(double)); /* p_0=p_0 */
3109  SumVector(3,j->points[4],j->points[2],p[1]); /* p_1=p_01+p_0 */
3110  SumVector(3,j->points[3],j->points[2],p[2]); /* p_2=p_02+p_0 */
3111  SumVector(3,j->points[5],j->points[2],p[3]); /* p_3=d+p_01+p_02+p_0 */
3112  SumVector(3,p[3],j->points[3],p[3]);
3113  SumVector(3,p[3],j->points[4],p[3]);
3114 
3115  AddPatchTrans2TransSeq(PA,-1,uID,vID,p,ts);
3116 
3117  for(k=0;k<4;k++)
3118  free(p[k]);
3119  free(p);
3120  }
3121  break;
3122 
3123  default:
3124  Error("Unknow joint type in GetJointTransSeq");
3125  }
3126  free(vname);
3127  }
3128 }
3129 
3130 double GetJointMaxCoordinate(Tjoint *j)
3131 {
3132  return(j->maxCoord);
3133 }
3134 
3135 void PlotJoint(Tplot3d *pt,Tjoint *j)
3136 {
3137  switch(j->t)
3138  {
3139  case SPH_SPH_JOINT:
3140  if (j->rad>0)
3141  {
3142  double zero[3]={0,0,0};
3143  double end[3]={0,0,0};
3144 
3145  /* Define a cylinder along the X axis that is latter moved with HTransformX2Vect */
3146 
3147  end[0]=j->length;
3148 
3149  j->obj3d[0]=StartNew3dObject(&(j->color),pt);
3150  PlotSphere(j->rad*1.5,0,0,0,pt);
3151  PlotCylinder(j->rad,zero,end,pt);
3152  PlotSphere(j->rad*1.5,end[0],end[1],end[2],pt);
3153  Close3dObject(pt);
3154 
3155  j->obj3d[1]=NO_UINT;
3156  }
3157  break;
3158 
3159  case SPH_PRS_SPH_JOINT:
3160  if (j->rad>0)
3161  {
3162  double zero[3]={0,0,0};
3163  double zero2[3]={0,0,0};
3164  double end[3]={0,0,0};
3165  double l,r;
3166  Tcolor c;
3167 
3168  /* Define two cylinders along the X axis one inside the other.
3169  The first cylinder is latter moved with HTransformX2Vect
3170  and the second with a translation + HTransformX2Vect */
3171 
3172  l=LowerLimit(&(j->range));
3173  r=IntervalSize(&(j->range));
3174 
3175  end[0]=l;
3176  zero2[0]=l-r;
3177 
3178  j->obj3d[0]=StartNew3dObject(&(j->color),pt);
3179  PlotSphere(j->rad*1.5,0,0,0,pt);
3180  PlotCylinder(j->rad,zero,end,pt);
3181  Close3dObject(pt);
3182 
3183  CopyColor(&c,&(j->color));
3184  ScaleColor(0.5,&c); /* The inner cylinder of the sph-prs-sphs joint
3185  is darker */
3186 
3187  j->obj3d[1]=StartNew3dObject(&c,pt);
3188  PlotCylinder(j->rad*0.75,zero2,end,pt);
3189  PlotSphere(j->rad*1.5,end[0],end[1],end[2],pt);
3190  Close3dObject(pt);
3191 
3192  DeleteColor(&c);
3193  }
3194  break;
3195  }
3196 
3197 }
3198 
3199 void MoveJointFromTransforms(Tparameters *p,
3200  Tplot3d *pt,THTransform *t1,THTransform *t2,
3201  Tjoint *j)
3202 {
3203 #if (0)
3204  /* For debug only: We compute the dof of the joint and then we re-construct
3205  the transform between the two links from the dof. */
3206  switch(j->t)
3207  {
3208  case SPH_SPH_JOINT:
3209  case SPH_PRS_SPH_JOINT:
3210  {
3211  double d[6];
3212  THTransform tc;
3213 
3214  GetJointDOFValues(p,t1,t2,d,j);
3215 
3216  HTransformProduct(t1,&(j->preT),&tc);
3217  HTransformAcumRot2(&tc,RZ,sin(d[0]),cos(d[0]),&tc);
3218  HTransformAcumRot2(&tc,RY,sin(d[1]),cos(d[1]),&tc);
3219 
3220  Move3dObject(j->obj3d[0],&tc,pt);
3221 
3222  if (j->t==SPH_PRS_SPH_JOINT)
3223  {
3224  HTransformAcumTrans(d[2]-j->offset,0,0,&tc);
3225  Move3dObject(j->obj3d[1],&tc,pt);
3226  }
3227  HTransformDelete(&tc);
3228  }
3229  break;
3230  }
3231 #else
3232  /* Normal operation: Whe pose of the object in the links is deduced form the
3233  position of the end-points of the link. */
3234  THTransform tc;
3235  double po[2][3];
3236  double d[3],n;
3237 
3238  switch(j->t)
3239  {
3240  case SPH_SPH_JOINT:
3241  case SPH_PRS_SPH_JOINT:
3242 
3243  /* Compute the position of the two attach points (the ones defining the
3244  spherical joints) */
3245  HTransformApply(j->points[0],po[0],t1);
3246  HTransformApply(j->points[2],po[1],t2);
3247 
3248  /* Compute the distance between the attach points and
3249  a unitary vector from the origin of the leg towards
3250  the end point */
3251  DifferenceVector(3,po[1],po[0],d);
3252  n=Norm(3,d);
3253  SumVectorScale(3,po[0],1/n,d,po[1]);
3254 
3255  HTransformX2Vect(1,1,po[0],po[1],&tc);
3256 
3257  Move3dObject(j->obj3d[0],&tc,pt);
3258 
3259  if (j->t==SPH_PRS_SPH_JOINT)
3260  {
3261  HTransformAcumTrans(n-j->offset,0,0,&tc);
3262  Move3dObject(j->obj3d[1],&tc,pt);
3263  }
3264  HTransformDelete(&tc);
3265 
3266  break;
3267  }
3268 #endif
3269 }
3270 
3271 void PrintJointAxes(Tparameters *p,FILE *f,TCuikSystem *cs,double *sol,
3272  double *r,Tjoint *j)
3273 {
3274  THTransform t;
3275  double point[3];
3276 
3277  switch(j->t)
3278  {
3279  case REV_JOINT:
3280  case PRS_JOINT:
3281  GetTransform2Link(p,cs,sol,IsGroundLink(j->linkID[0]),&(r[j->linkID[0]*3]),
3282  &t,j->link[0]);
3283  HTransformApply(j->points[0],point,&t);
3284  fprintf(f,"%.12f %.12f %.12f ",point[0],point[1],point[2]);
3285  HTransformApplyRot(j->normals[0],point,&t);
3286  fprintf(f,"%.12f %.12f %.12f ",point[0],point[1],point[2]);
3287  break;
3288  case UNV_JOINT:
3289  GetTransform2Link(p,cs,sol,IsGroundLink(j->linkID[0]),&(r[j->linkID[0]*3]),
3290  &t,j->link[0]);
3291  HTransformApply(j->points[0],point,&t);
3292  fprintf(f,"%.12f %.12f %.12f ",point[0],point[1],point[2]);
3293  HTransformApplyRot(j->normals[0],point,&t);
3294  fprintf(f,"%.12f %.12f %.12f ",point[0],point[1],point[2]);
3295 
3296  GetTransform2Link(p,cs,sol,IsGroundLink(j->linkID[1]),&(r[j->linkID[1]*3]),
3297  &t,j->link[1]);
3298  HTransformApply(j->points[2],point,&t);
3299  fprintf(f,"%.12f %.12f %.12f ",point[0],point[1],point[2]);
3300  HTransformApplyRot(j->normals[1],point,&t);
3301  fprintf(f,"%.12f %.12f %.12f ",point[0],point[1],point[2]);
3302  break;
3303  case FREE_JOINT:
3304  break;
3305  default:
3306  Error("PrintJointAxes is not fully implemented yet");
3307  }
3308 }
3309 
3310 void PrintJoint(FILE *f,Tjoint *j)
3311 {
3312  char *ln1,*ln2;
3313 
3314  ln1=GetLinkName(j->link[0]);
3315  ln2=GetLinkName(j->link[1]);
3316 
3317  switch(j->t)
3318  {
3319  case FREE_JOINT:
3320  break;
3321 
3322  case FIX_JOINT:
3323  fprintf(f," fix: %s %s ",ln1,ln2);
3324  HTransformPrettyPrint(f,&(j->preT));
3325  fprintf(f,"\n\n");
3326  break;
3327 
3328  case PRS_JOINT:
3329  fprintf(f," prismatic: %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln1,
3330  j->points[0][0],j->points[0][1],j->points[0][2],
3331  j->points[1][0],j->points[1][1],j->points[1][2]);
3332  fprintf(f," %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln2,
3333  j->points[2][0],j->points[2][1],j->points[2][2],
3334  j->points[3][0],j->points[3][1],j->points[3][2]);
3335  fprintf(f," range ");
3336  PrintInterval(f,&(j->range));
3337  fprintf(f,"\n");
3338  if (j->avoidLimits)
3339  fprintf(f," avoid limits\n");
3340  fprintf(f,"\n");
3341  break;
3342 
3343  case SPH_JOINT:
3344  fprintf(f," spherical: %s (%.16f,%.16f,%.16f)\n",ln1,
3345  j->points[0][0],j->points[0][1],j->points[0][2]);
3346  fprintf(f," %s (%.16f,%.16f,%.16f)\n",ln2,
3347  j->points[2][0],j->points[2][1],j->points[2][2]);
3348  if (j->hasLimits)
3349  {
3350  fprintf(f," range ");
3351  PrintInterval(f,&(j->range));
3352  fprintf(f," +(%.16f,%.16f,%.16f)",
3353  j->vrange[0][0],j->vrange[0][1],j->vrange[0][2]);
3354  fprintf(f," +(%.16f,%.16f,%.16f)\n",
3355  j->vrange[1][0],j->vrange[2][1],j->vrange[3][2]);
3356  }
3357  if (j->avoidLimits)
3358  fprintf(f," avoid limits\n");
3359  fprintf(f,"\n");
3360  break;
3361 
3362  case REV_JOINT:
3363  fprintf(f," revolute: %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln1,
3364  j->points[0][0],j->points[0][1],j->points[0][2],
3365  j->points[1][0],j->points[1][1],j->points[1][2]);
3366  fprintf(f," %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln2,
3367  j->points[2][0],j->points[2][1],j->points[2][2],
3368  j->points[3][0],j->points[3][1],j->points[3][2]);
3369 
3370  if (j->hasLimits)
3371  {
3372  fprintf(f," range ");
3373  PrintInterval(f,&(j->range));
3374  fprintf(f," +(%.16f,%.16f,%.16f)",
3375  j->vrange[0][0],j->vrange[0][1],j->vrange[0][2]);
3376  fprintf(f," +(%.16f,%.16f,%.16f)\n",
3377  j->vrange[1][0],j->vrange[1][1],j->vrange[1][2]);
3378  }
3379  if (j->avoidLimits)
3380  fprintf(f," avoid limits\n");
3381  fprintf(f,"\n");
3382  break;
3383 
3384  case UNV_JOINT:
3385  fprintf(f," universal: %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln1,
3386  j->points[0][0],j->points[0][1],j->points[0][2],
3387  j->points[1][0],j->points[1][1],j->points[1][2]);
3388  fprintf(f," %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln2,
3389  j->points[2][0],j->points[2][1],j->points[2][2],
3390  j->points[3][0],j->points[3][1],j->points[3][2]);
3391  if (j->hasLimits)
3392  {
3393  fprintf(f," range ");
3394  PrintInterval(f,&(j->range));
3395  fprintf(f," +(%.16f,%.16f,%.16f)",
3396  j->vrange[0][0],j->vrange[0][1],j->vrange[0][2]);
3397  PrintInterval(f,&(j->range2));
3398  fprintf(f," +(%.16f,%.16f,%.16f)\n",
3399  j->vrange[1][0],j->vrange[1][1],j->vrange[1][2]);
3400  }
3401  if (j->avoidLimits)
3402  fprintf(f," avoid limits\n");
3403  fprintf(f,"\n");
3404  break;
3405 
3406  case SPH_SPH_JOINT:
3407  fprintf(f," sph_sph : %s (%.16f,%.16f,%.16f)\n",ln1,
3408  j->points[0][0],j->points[0][1],j->points[0][2]);
3409  fprintf(f," %s (%.16f,%.16f,%.16f)\n",ln2,
3410  j->points[2][0],j->points[2][1],j->points[2][2]);
3411  fprintf(f," length %.16f radius %.16f\n",j->length,j->rad);
3412  fprintf(f," color (%.16f,%.16f,%.16f)\n\n",
3413  GetRed(&(j->color)),GetGreen(&(j->color)),GetBlue(&(j->color)));
3414  break;
3415 
3416  case SPH_PRS_SPH_JOINT:
3417  fprintf(f," sph_prs_sph : %s (%.16f,%.16f,%.16f)\n",ln1,
3418  j->points[0][0],j->points[0][1],j->points[0][2]);
3419  fprintf(f," %s (%.16f,%.16f,%.16f)\n",ln2,
3420  j->points[2][0],j->points[2][1],j->points[2][2]);
3421  fprintf(f," range ");
3422  PrintInterval(f,&(j->range));
3423  fprintf(f,"\n");
3424  fprintf(f," radius %.16f\n",j->rad);
3425  fprintf(f," color (%.16f,%.16f,%.16f)\n\n",
3426  GetRed(&(j->color)),GetGreen(&(j->color)),GetBlue(&(j->color)));
3427  break;
3428 
3429  case IN_PATCH_JOINT:
3430  {
3431  double v[3];
3432 
3433  fprintf(f," in_patch: %s (%.16f,%.16f,%.16f) (%.16f,%.16f,%.16f)\n",ln1,
3434  j->points[0][0],j->points[0][1],j->points[0][2],
3435  j->points[1][0],j->points[1][1],j->points[1][2]);
3436  /* p_0 is directly stored */
3437  fprintf(f," %s (%.16f,%.16f,%.16f)",ln2,
3438  j->points[2][0],j->points[2][1],j->points[2][2]);
3439  /* p_1=p_01+p_0 */
3440  SumVector(3,j->points[4],j->points[2],v);
3441  fprintf(f," (%.16f,%.16f,%.16f)",v[0],v[1],v[2]);
3442  /* p_2=p_02+p_0 */
3443  SumVector(3,j->points[3],j->points[2],v);
3444  fprintf(f," (%.16f,%.16f,%.16f)",v[0],v[1],v[2]);
3445  /* p_3=d+p_01+p_02+p_0 */
3446  SumVector(3,j->points[5],j->points[2],v);
3447  SumVector(3,v,j->points[3],v);
3448  SumVector(3,v,j->points[4],v);
3449  fprintf(f," (%.16f,%.16f,%.16f)\n",v[0],v[1],v[2]);
3450  if (j->avoidLimits)
3451  fprintf(f," avoid limits\n");
3452  fprintf(f,"\n");
3453  }
3454  break;
3455 
3456  default:
3457  Error("Undefined joint type in PrintJoint");
3458  }
3459 }
3460 
3461 void DeleteJoint(Tjoint *j)
3462 {
3463  DeleteInterval(&(j->range));
3464  DeleteInterval(&(j->range2));
3465  HTransformDelete(&(j->preT));
3466  HTransformDelete(&(j->postT));
3467  DeleteColor(&(j->color));
3468 }
NewFreeJoint
void NewFreeJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
Constructor.
Definition: joint.c:101
AXIS_X
#define AXIS_X
One of the dimension of R^3.
Definition: htransform.h:83
FIX_JOINT
#define FIX_JOINT
One of the possible type of joints.
Definition: joint.h:51
COS_VAR_UNI_SING
#define COS_VAR_UNI_SING(vname, id, ln1, ln2, k)
End-range singularity variable for each universal joint.
Definition: varnames.h:469
GetBoxInterval
Tinterval * GetBoxInterval(unsigned int n, Tbox *b)
Returns a pointer to one of the intervals defining the box.
Definition: box.c:270
UNV_JOINT
#define UNV_JOINT
One of the possible type of joints.
Definition: joint.h:67
SYSTEM_EQ
#define SYSTEM_EQ
One of the possible type of equations.
Definition: equation.h:146
REP_JOINTS
#define REP_JOINTS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:60
HTransformRz2
void HTransformRz2(double s, double c, THTransform *t)
Constructor.
Definition: htransform.c:226
PrintJoint
void PrintJoint(FILE *f, Tjoint *j)
Stores the joint information into a file.
Definition: joint.c:3310
FALSE
#define FALSE
FALSE.
Definition: boolean.h:30
Tjoint::maxCoord
double maxCoord
Definition: joint.h:234
IN_PATCH_JOINT_CTRL_VAR_SING_COS
#define IN_PATCH_JOINT_CTRL_VAR_SING_COS(vname, id, ln1, ln2, i)
End-range singularity variable for each in_patch joint.
Definition: varnames.h:261
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
GenerateSaddleEquation
void GenerateSaddleEquation(unsigned int vx, unsigned int vy, unsigned int vz, Tequation *eq)
Construtor. Generates a saddle equation.
Definition: equation.c:1392
HTransformRx
void HTransformRx(double rx, THTransform *t)
Constructor.
Definition: htransform.c:155
HTransformPrettyPrint
void HTransformPrettyPrint(FILE *f, THTransform *t)
Prints a homogenoeus transform compactly.
Definition: htransform.c:809
PRS_JOINT_VAR_SING_SIN
#define PRS_JOINT_VAR_SING_SIN(vname, id, ln1, ln2)
One end-range singularity variable for each prismatic joint.
Definition: varnames.h:180
Tjoint
Relation between two links.
Definition: joint.h:177
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
SetEquationType
void SetEquationType(unsigned int type, Tequation *eq)
Changes the type of the equation (SYSTEM_EQ, CARTESIAN_EQ, DUMMY_EQ, DERIVED_EQ). ...
Definition: equation.c:1013
NEW
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
GenerateNormEquation
void GenerateNormEquation(unsigned int vx, unsigned int vy, unsigned int vz, double n, Tequation *eq)
Construtor. Generates an equation that is the norm of a 3d vector.
Definition: equation.c:1431
Tjoint::link
Tlink * link[2]
Definition: joint.h:187
NewSphPrsSphJoint
void NewSphPrsSphJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tinterval *range, double r, Tcolor *color, Tjoint *j)
Constructor.
Definition: joint.c:652
HTransformAcumRot2
CBLAS_INLINE void HTransformAcumRot2(THTransform *t_in, unsigned int type, double s, double c, THTransform *t)
Computes the result of multiplying a homogeneous transform by a rotation matrix.
Definition: htransform.c:716
DeleteEquation
void DeleteEquation(Tequation *eq)
Destructor.
Definition: equation.c:1748
GenerateJointEquationsInBranch
void GenerateJointEquationsInBranch(Tparameters *p, double s, TCuikSystem *cs, Tequation *eq, Tjoint *j)
Generate the constraints of a joint in a sequence.
Definition: joint.c:1755
AddPatchTrans2TransSeq
void AddPatchTrans2TransSeq(unsigned int t, int s, unsigned int u, unsigned int v, double **p, TTransSeq *ts)
Adds a Parametrized-Patch transform to a transform sequence.
Definition: trans_seq.c:375
Tjoint::avoidLimits
boolean avoidLimits
Definition: joint.h:216
THTransform
A homgeneous transform in R^3.
SetBoxInterval
void SetBoxInterval(unsigned int n, Tinterval *is, Tbox *b)
Replaces a particular interval in a box.
Definition: box.c:259
Det3x3
double Det3x3(double *c1, double *c2, double *c3)
Determinant of a 3x3 matrix.
Definition: geom.c:730
AXIS_Y
#define AXIS_Y
One of the dimension of R^3.
Definition: htransform.h:91
SYSTEM_VAR
#define SYSTEM_VAR
One of the possible type of variables.
Definition: variable.h:24
RZ
#define RZ
One of the types of homogeneous transforms in R^3.
Definition: htransform.h:75
Tjoint::hasLimits
boolean hasLimits
Definition: joint.h:212
Norm
CBLAS_INLINE double Norm(unsigned int s, double *v)
Computes the norm of a vector.
Definition: basic_algebra.c:265
AddVariable2CS
unsigned int AddVariable2CS(Tvariable *v, TCuikSystem *cs)
Adds a variable to the system.
Definition: cuiksystem.c:2511
IntervalAdd
void IntervalAdd(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Addition of two intervals.
Definition: interval.c:418
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
varnames.h
Definition of variable names.
SumVector
CBLAS_INLINE void SumVector(unsigned int s, double *v1, double *v2, double *v)
Adds two vectors.
Definition: basic_algebra.c:67
HTransformApplyRot
void HTransformApplyRot(double *p_in, double *p_out, THTransform *t)
Multiply the rotation part of the homogeneous transform and a vector.
Definition: htransform.c:747
NewSphericalJoint
void NewSphericalJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, boolean hasLimits, double range, double **rPoints, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
Constructor.
Definition: joint.c:452
GetGreen
double GetGreen(Tcolor *c)
Gets the green component of a color.
Definition: color.c:78
EQU
#define EQU
In a Tequation, the equation relational operator is equal.
Definition: equation.h:201
HTransformAcumTrans
CBLAS_INLINE void HTransformAcumTrans(double tx, double ty, double tz, THTransform *t)
Computes the result of multiplying a homogeneous transform by a translation matrix with parameters tx...
Definition: htransform.c:641
FREE_JOINT
#define FREE_JOINT
One of the possible type of joints.
Definition: joint.h:40
RY
#define RY
One of the types of homogeneous transforms in R^3.
Definition: htransform.h:67
SetVariableInterval
void SetVariableInterval(Tinterval *i, Tvariable *v)
Sets the new range for the variable.
Definition: variable.c:70
TRUE
#define TRUE
TRUE.
Definition: boolean.h:21
Normalize
CBLAS_INLINE void Normalize(unsigned int s, double *v)
Normalizes a vector.
Definition: basic_algebra.c:442
Det2x2
double Det2x2(double *c1, double *c2)
Determinant of a 2x2 matrix.
Definition: geom.c:725
Tjoint::offset2
double offset2
Definition: joint.h:226
GenerateJointRangeSingularityEquations
void GenerateJointRangeSingularityEquations(Tparameters *p, TCuikSystem *cs, Tjoint *j)
Modifies the cuik system to detect end range singularities.
Definition: joint.c:1276
InitEquation
void InitEquation(Tequation *eq)
Constructor.
Definition: equation.c:86
Error
void Error(const char *s)
General error function.
Definition: error.c:80
InitTransSeq
void InitTransSeq(TTransSeq *ts)
Constructor.
Definition: trans_seq.c:213
NFUN
#define NFUN
No trigonometric function for the variable.
Definition: variable_set.h:36
TOPOLOGY_R
#define TOPOLOGY_R
One of the possible topologies.
Definition: defines.h:122
CopyColor
void CopyColor(Tcolor *c_dst, Tcolor *c_src)
Copy constructor.
Definition: color.c:21
Tcolor
A color.
Definition: color.h:23
NewUniversalJoint
void NewUniversalJoint(unsigned int id, unsigned int r, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, boolean hasLimits, Tinterval *range1, Tinterval *range2, double **rPoints, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
Constructor.
Definition: joint.c:288
GetJointDOFValues
void GetJointDOFValues(Tparameters *p, THTransform *t1, THTransform *t2, double *dof, Tjoint *j)
Recovers the joint DOFs from the absolute poses of the links.
Definition: joint.c:2644
PlotSphere
void PlotSphere(double r, double x, double y, double z, Tplot3d *p)
Adds a sphere to the current object.
Definition: plot3d.c:369
NewInPatchJoint
void NewInPatchJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, double **patch, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
Constructor.
Definition: joint.c:709
LimitedJoint
boolean LimitedJoint(Tjoint *j)
Checks if a joint has limits.
Definition: joint.c:1022
SPH_JOINT
#define SPH_JOINT
One of the possible type of joints.
Definition: joint.h:75
AXIS_H
#define AXIS_H
The homogeneous dimension in R^3.
Definition: htransform.h:108
HTransformProduct
CBLAS_INLINE void HTransformProduct(THTransform *t1, THTransform *t2, THTransform *t3)
Product of two homogeneous transforms.
Definition: htransform.c:404
ZERO
#define ZERO
Floating point operations giving a value below this constant (in absolute value) are considered 0...
Definition: defines.h:37
CopyInterval
void CopyInterval(Tinterval *i_dst, Tinterval *i_org)
Copy constructor.
Definition: interval.c:59
COS_VAR_UNI
#define COS_VAR_UNI(vname, id, ln1, ln2, k)
Cosinus between two links for universal joints.
Definition: varnames.h:455
SetEquationValue
void SetEquationValue(double v, Tequation *eq)
Changes the right-hand value of the equation.
Definition: equation.c:1026
HTransformX2Vect
void HTransformX2Vect(double sy, double sz, double *p1, double *p2, THTransform *t)
Transform a unitary vector along the X axis to a generic vector.
Definition: htransform.c:539
Move3dObject
void Move3dObject(unsigned int nobj, THTransform *t, Tplot3d *p)
Moves a 3d object.
Definition: plot3d.c:217
AddMonomial
void AddMonomial(Tmonomial *f, Tequation *eq)
Adds a new monomial to the equation.
Definition: equation.c:1356
AddTerm2SearchCriterion
void AddTerm2SearchCriterion(double w, unsigned int v, double val, TCuikSystem *cs)
Adds penalty terms to the search criterion.
Definition: cuiksystem.c:2438
Tjoint::points
double points[6][3]
Definition: joint.h:189
GenerateJointEquations
void GenerateJointEquations(Tparameters *p, double maxCoord, TCuikSystem *cs, Tjoint *j)
Generate the constraints related with the joint.
Definition: joint.c:1920
Tjoint::length
double length
Definition: joint.h:239
GetJointRangeN
void GetJointRangeN(unsigned int nr, double mt, Tinterval *r, Tjoint *j)
Returns one of the ranges of the joint.
Definition: joint.c:1037
SPH_JOINT_VAR_REF
#define SPH_JOINT_VAR_REF(vname, id, v, ln1, ln2, k)
Two vectors defined for when limiting the movement of a spherical joints.
Definition: varnames.h:484
GetJointTransSeq
void GetJointTransSeq(Tparameters *p, TCuikSystem *cs, TTransSeq *ts, Tjoint *j)
Build the sequence of transforms passing through the joint.
Definition: joint.c:2895
error.h
Error and warning functions.
Tplot3d
A 3D plot.
Definition: plot3d.h:54
JointToID
unsigned int JointToID(Tjoint *j)
Gets the identifier of the second link involved in the joint.
Definition: joint.c:951
NewSphSphJoint
void NewSphSphJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, double l, double r, Tcolor *color, Tjoint *j)
Constructor.
Definition: joint.c:609
IntervalSqrt
boolean IntervalSqrt(Tinterval *i, Tinterval *i_out)
Interval square root.
Definition: interval.c:525
HTransformGetElement
double HTransformGetElement(unsigned int i, unsigned int j, THTransform *t)
Gets an element in a homogeneous transform.
Definition: htransform.c:323
RegenerateJointBox
void RegenerateJointBox(Tparameters *p, TCuikSystem *cs, Tbox *b, Tjoint *j)
Computes the values for the dummy variables used in the joint equations.
Definition: joint.c:2292
Tjoint::range
Tinterval range
Definition: joint.h:220
Tjoint::normals
double normals[3][3]
Definition: joint.h:203
HTransformFromVectors
void HTransformFromVectors(double *x, double *y, double *z, double *h, THTransform *t)
Defines a homogeneous transform from 4 vectors.
Definition: htransform.c:335
SetEquationCmp
void SetEquationCmp(unsigned int cmp, Tequation *eq)
Changes the relational operator (LEQ, GEQ, EQU) of the equation.
Definition: equation.c:1018
PlotJoint
void PlotJoint(Tplot3d *pt, Tjoint *j)
Adds a joint to a 3d scene.
Definition: joint.c:3135
ResetMonomial
void ResetMonomial(Tmonomial *f)
Reset the monomial information.
Definition: monomial.c:24
HTransformRz
void HTransformRz(double rz, THTransform *t)
Constructor.
Definition: htransform.c:189
DotProduct
double DotProduct(double *v1, double *v2)
Computes the dot product of two 3d vectors.
Definition: geom.c:643
LowerLimit
double LowerLimit(Tinterval *i)
Gets the lower limit.
Definition: interval.c:79
HTransformInverse
void HTransformInverse(THTransform *t, THTransform *ti)
Inverse of a homogeneous transform.
Definition: htransform.c:468
Tjoint::id
unsigned int id
Definition: joint.h:181
IsJointAllSpheres
boolean IsJointAllSpheres(Tjoint *j)
Identifies joint formed only by spheres.
Definition: joint.c:1004
PRS_JOINT
#define PRS_JOINT
One of the possible type of joints.
Definition: joint.h:127
AXIS_Z
#define AXIS_Z
One of the dimension of R^3.
Definition: htransform.h:99
GetJointPoint
void GetJointPoint(unsigned int link, unsigned int point, double *p, Tjoint *j)
Gets one of the points defining the rotation/sliding axis for the joint.
Definition: joint.c:1012
IN_PATCH_JOINT
#define IN_PATCH_JOINT
One of the possible type of joints.
Definition: joint.h:142
UNV_JOINT_VAR
#define UNV_JOINT_VAR(vname, id, ln1, ln2, i, k)
Two vectors for each universal joint.
Definition: varnames.h:207
IN_PATCH_JOINT_SCALE_VAR
#define IN_PATCH_JOINT_SCALE_VAR(vname, id, ln1, ln2)
Scale factor for the normal vector to a first order Bezier patch.
Definition: varnames.h:277
TX
#define TX
One of the types of homogeneous transforms in R^3.
Definition: htransform.h:35
Tequation
An equation.
Definition: equation.h:236
CoupledWith
unsigned int CoupledWith(Tjoint *j)
Returns the identifier of the joint coupled with the query joint.
Definition: joint.c:1216
DOF_VAR
#define DOF_VAR(vname, id, ln1, ln2, i)
Name for the degrees of freedom.
Definition: varnames.h:52
DeleteVariable
void DeleteVariable(Tvariable *v)
Destructor.
Definition: variable.c:95
GetJointMaxCoordinate
double GetJointMaxCoordinate(Tjoint *j)
Returns the maximum coordinate value for all the objects in the joint.
Definition: joint.c:3130
defines.h
Definitions of constants and macros used in several parts of the cuik library.
Tjoint::coupled
void * coupled
Definition: joint.h:213
SPH_PRS_SPH_JOINT
#define SPH_PRS_SPH_JOINT
One of the possible type of joints.
Definition: joint.h:119
joint.h
Definition of the Tjoint type and the associated functions.
GetYawPitchRoll
void GetYawPitchRoll(double *a, double *b, double *c, THTransform *t)
Recovers the Euler angles from a rotation matrix.
Definition: htransform.c:599
Tjoint::postT
THTransform postT
Definition: joint.h:248
DifferenceVector
void DifferenceVector(unsigned int s, double *v1, double *v2, double *v)
Substracts two vectors.
Definition: basic_algebra.c:223
GenerateJointRangeEquations
void GenerateJointRangeEquations(Tparameters *p, TCuikSystem *cs, Tjoint *j)
Generate the constraints related with the joint limits.
Definition: joint.c:1463
NewVariable
void NewVariable(unsigned int type, char *name, Tvariable *v)
Constructor.
Definition: variable.c:21
SECONDARY_VAR
#define SECONDARY_VAR
One of the possible type of variables.
Definition: variable.h:44
ScaleColor
void ScaleColor(double s, Tcolor *c)
Scales a color.
Definition: color.c:33
InitJoint
void InitJoint(unsigned int t, unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
Initializes all fields for a joint with dummy values.
Definition: joint.c:42
DeleteInterval
void DeleteInterval(Tinterval *i)
Destructor.
Definition: interval.c:1016
Tjoint::color
Tcolor color
Definition: joint.h:241
AddEquation2CS
void AddEquation2CS(Tparameters *p, Tequation *eq, TCuikSystem *cs)
Adds an equation to the system.
Definition: cuiksystem.c:2481
Tmonomial
A scaled product of powers of variables.
Definition: monomial.h:32
Tparameters
A table of parameters.
Tjoint::preT
THTransform preT
Definition: joint.h:244
COS_VAR
#define COS_VAR(vname, id, ln1, ln2)
Cosinus between two links for rotation/spherical joints.
Definition: varnames.h:425
UpperLimit
double UpperLimit(Tinterval *i)
Gets the uppser limit.
Definition: interval.c:87
ADJUST_REAL
#define ADJUST_REAL(a)
Adjust a real to 0,1,-1, if possible.
Definition: defines.h:195
TTransSeq
A sequence of transforms.
Definition: trans_seq.h:296
Tjoint::vrange
double vrange[2][3]
Definition: joint.h:227
Tjoint::linkID
unsigned int linkID[2]
Definition: joint.h:186
HTransformAcumRot
CBLAS_INLINE void HTransformAcumRot(unsigned int type, double s, double c, THTransform *t)
Computes the result of multiplying a homogeneous transform by a rotation matrix.
Definition: htransform.c:673
GetJointName
void GetJointName(char **name, Tjoint *j)
Returns a string identifying the joint.
Definition: joint.c:961
UNV_JOINT_VAR_REF
#define UNV_JOINT_VAR_REF(vname, id, k, ln1, ln2, i)
Two vectors to define the angle in a universal joint.
Definition: varnames.h:408
JointTo
Tlink * JointTo(Tjoint *j)
Gets a pointer to the second link involved in the joint.
Definition: joint.c:956
GetJointTransform
void GetJointTransform(double *dof, THTransform *t, Tjoint *j)
Computes the transform induced by the joint.
Definition: joint.c:2527
IN_PATCH_JOINT_CTRL_VAR
#define IN_PATCH_JOINT_CTRL_VAR(vname, id, ln1, ln2, i)
Control variables defining a first order Bezier patch.
Definition: varnames.h:235
Tjoint::obj3d
unsigned int obj3d[2]
Definition: joint.h:230
COS_VAR_SING
#define COS_VAR_SING(vname, id, ln1, ln2)
One end-range singularity variable for each revolute joint.
Definition: varnames.h:438
M_PI
#define M_PI
Pi.
Definition: defines.h:83
CT_REPRESENTATION
#define CT_REPRESENTATION
Representation.
Definition: parameters.h:215
AddVarTrans2TransSeq
void AddVarTrans2TransSeq(unsigned int t, int s, unsigned int v, TTransSeq *ts)
Adds a variable transform to the sequence.
Definition: trans_seq.c:338
REV_JOINT
#define REV_JOINT
One of the possible type of joints.
Definition: joint.h:59
GetJointLength
double GetJointLength(Tjoint *j)
Returns the length of a spherical-spherical joint.
Definition: joint.c:1268
ResetEquation
void ResetEquation(Tequation *eq)
Reset equation information.
Definition: equation.c:442
Tbox
A box.
Definition: box.h:83
SPH_SPH_JOINT
#define SPH_SPH_JOINT
One of the possible type of joints.
Definition: joint.h:98
GetRed
double GetRed(Tcolor *c)
Gets the red component of a color.
Definition: color.c:73
Tvariable
Data associated with each variable in the problem.
Definition: variable.h:84
ROT_JOINT_VAR_REF
#define ROT_JOINT_VAR_REF(vname, id, k, ln1, ln2, i)
Two vectors to define the angle in a revolute joint.
Definition: varnames.h:387
TCuikSystem
A cuiksystem, i.e., a set of variables and equations defining a position analysis problem...
Definition: cuiksystem.h:181
PrintInterval
void PrintInterval(FILE *f, Tinterval *i)
Prints an interval.
Definition: interval.c:1001
NO_UINT
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
Tjoint::normRange
Tinterval normRange
Definition: joint.h:209
SumVectorScale
CBLAS_INLINE void SumVectorScale(unsigned int s, double *v1, double w, double *v2, double *v)
Adds two vectors with a scale.
Definition: basic_algebra.c:86
RX
#define RX
One of the types of homogeneous transforms in R^3.
Definition: htransform.h:59
GetJointRange
Tinterval * GetJointRange(Tjoint *j)
Checks the limits of a joint.
Definition: joint.c:1027
IntervalScale
void IntervalScale(Tinterval *i1, double e, Tinterval *i_out)
Scales an interval.
Definition: interval.c:355
AddVariable2Monomial
void AddVariable2Monomial(unsigned int fn, unsigned int varid, unsigned int p, Tmonomial *f)
Adds a power variable to the monomial.
Definition: monomial.c:171
IntervalCenter
double IntervalCenter(Tinterval *i)
Gets the interval center.
Definition: interval.c:129
DeleteJoint
void DeleteJoint(Tjoint *j)
Destructor.
Definition: joint.c:3461
HTransformTx
void HTransformTx(double tx, THTransform *t)
Constructor.
Definition: htransform.c:106
CopyJoint
void CopyJoint(Tjoint *j_dst, Tjoint *j_src)
Copy constructor.
Definition: joint.c:873
SetVariableTopology
void SetVariableTopology(unsigned int t, Tvariable *v)
Sets the topology of the variable.
Definition: variable.c:44
AddDispTrans2TransSeq
void AddDispTrans2TransSeq(int s, unsigned int v, double *vect, TTransSeq *ts)
Adds a displacement transform to the sequence.
Definition: trans_seq.c:361
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
Tjoint::rad
double rad
Definition: joint.h:235
HTransformDelete
void HTransformDelete(THTransform *t)
Destructor.
Definition: htransform.c:833
PA
#define PA
Point on a patch.
Definition: trans_seq.h:31
GetParameter
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
IntervalProduct
void IntervalProduct(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Product of two intervals.
Definition: interval.c:384
GetJointID
unsigned int GetJointID(Tjoint *j)
Gets the joint identifier.
Definition: joint.c:936
FREE_JOINT_VAR
#define FREE_JOINT_VAR(vname, id, id1, ln1, id2, ln2, k)
Variables for a free joint.
Definition: varnames.h:155
RegenerateJointSolution
void RegenerateJointSolution(Tparameters *p, TCuikSystem *cs, double *sol, Tjoint *j)
Computes the values for the dummy variables used in the joint equations.
Definition: joint.c:2256
PRS_JOINT_VAR
#define PRS_JOINT_VAR(vname, id, ln1, ln2)
One variable for each prismatic joint.
Definition: varnames.h:168
DeleteColor
void DeleteColor(Tcolor *c)
Destructor.
Definition: color.c:93
JointFrom
Tlink * JointFrom(Tjoint *j)
Gets a pointer to the first link involved in the joint.
Definition: joint.c:946
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
MoveJointFromTransforms
void MoveJointFromTransforms(Tparameters *p, Tplot3d *pt, THTransform *t1, THTransform *t2, Tjoint *j)
Displaces a joint in a 3d scene.
Definition: joint.c:3199
AddCt2Monomial
void AddCt2Monomial(double k, Tmonomial *f)
Scales a monomial.
Definition: monomial.c:158
GetJointDOF
signed int GetJointDOF(Tjoint *j)
Computes the degrees of freedom allowed by a given joint.
Definition: joint.c:1224
AddCtTrans2TransSeq
void AddCtTrans2TransSeq(THTransform *t, TTransSeq *ts)
Adds a constant transform to the sequence.
Definition: trans_seq.c:313
PRS_JOINT_VAR_SING_COS
#define PRS_JOINT_VAR_SING_COS(vname, id, ln1, ln2)
One end-range singularity variable for each prismatic joint.
Definition: varnames.h:192
HTransformCopy
void HTransformCopy(THTransform *t_dst, THTransform *t_src)
Copy constructor.
Definition: htransform.c:83
Tjoint::avoidLimitsWeight
double avoidLimitsWeight
Definition: joint.h:218
DefineNormalVector
void DefineNormalVector(double *v, double *n)
Defines a unitary vector normal to a given vector.
Definition: geom.c:571
GetCSVariableID
unsigned int GetCSVariableID(char *name, TCuikSystem *cs)
Gets the numerical identifier of a variable given its name.
Definition: cuiksystem.c:2586
NewColor
void NewColor(double r, double g, double b, Tcolor *c)
Constructor.
Definition: color.c:14
Tjoint::t
unsigned int t
Definition: joint.h:178
IntervalCosine
void IntervalCosine(Tinterval *i, Tinterval *i_out)
Interval cosine.
Definition: interval.c:697
Tjoint::offset
double offset
Definition: joint.h:223
Tjoint::range2
Tinterval range2
Definition: joint.h:221
Tinterval
Defines a interval.
Definition: interval.h:33
HTransformRy
void HTransformRy(double ry, THTransform *t)
Constructor.
Definition: htransform.c:172
SPH_SPH_JOINT_VAR
#define SPH_SPH_JOINT_VAR(vname, id, ln1, ln2, i)
A vector for each spherical-spherical composite joint.
Definition: varnames.h:220
GetBlue
double GetBlue(Tcolor *c)
Gets the blue component of a color.
Definition: color.c:83
HTransformIdentity
void HTransformIdentity(THTransform *t)
Constructor.
Definition: htransform.c:69
InitMonomial
void InitMonomial(Tmonomial *f)
Constructor.
Definition: monomial.c:17
PlotCylinder
void PlotCylinder(double r, double *p1, double *p2, Tplot3d *p)
Adds a cylinder to the current object.
Definition: plot3d.c:377
IntervalSize
double IntervalSize(Tinterval *i)
Gets the uppser limit.
Definition: interval.c:96
CrossProduct
void CrossProduct(double *v1, double *v2, double *v3)
Computes the cross product of two 3d vectors.
Definition: geom.c:630
JointFromID
unsigned int JointFromID(Tjoint *j)
Gets the identifier of the first link involved in the joint.
Definition: joint.c:941
StartNew3dObject
unsigned int StartNew3dObject(Tcolor *c, Tplot3d *p)
Start a composed object.
Definition: plot3d.c:157
GetJointRangeTopology
unsigned int GetJointRangeTopology(unsigned int nr, Tjoint *j)
Returns the topology of one of the ranges of the joint.
Definition: joint.c:1137
IN_PATCH_JOINT_CTRL_VAR_SING_SIN
#define IN_PATCH_JOINT_CTRL_VAR_SING_SIN(vname, id, ln1, ln2, i)
End-range singularity variable for each in_patch joint.
Definition: varnames.h:248
DUMMY_VAR
#define DUMMY_VAR
One of the possible type of variables.
Definition: variable.h:53
HTransformRx2
void HTransformRx2(double s, double c, THTransform *t)
Constructor.
Definition: htransform.c:206
PrintJointAxes
void PrintJointAxes(Tparameters *p, FILE *f, TCuikSystem *cs, double *sol, double *r, Tjoint *j)
Prints the joint axis in world coordinates.
Definition: joint.c:3271
Close3dObject
void Close3dObject(Tplot3d *p)
Closes a composed object.
Definition: plot3d.c:171
GenerateJointSolution
void GenerateJointSolution(Tparameters *p, double *dof, THTransform *t1, THTransform *t2, TCuikSystem *cs, double *sol, Tjoint *j)
Generates a solution point from degrees of freedom.
Definition: joint.c:2330
DeleteMonomial
void DeleteMonomial(Tmonomial *f)
Destructor.
Definition: monomial.c:289
GetJointType
unsigned int GetJointType(Tjoint *j)
Gets the joint type.
Definition: joint.c:931
GenerateDotProductEquation
void GenerateDotProductEquation(unsigned int v1x, unsigned int v1y, unsigned int v1z, unsigned int v2x, unsigned int v2y, unsigned int v2z, unsigned int vc, double c, Tequation *eq)
Construtor. Generates the equation of the dot product of two unitary vectors.
Definition: equation.c:1526
GetJointSecondRange
Tinterval * GetJointSecondRange(Tjoint *j)
Checks the second limit of a universal joint.
Definition: joint.c:1032
TOPOLOGY_S
#define TOPOLOGY_S
One of the possible topologies.
Definition: defines.h:139
IntervalOffset
void IntervalOffset(Tinterval *i, double offset, Tinterval *i_out)
Interval offset.
Definition: interval.c:622