world.c
Go to the documentation of this file.
1 #include "world.h"
2 #include "defines.h"
3 #include "error.h"
4 #include "varnames.h"
5 #include "random.h"
6 #include "list.h"
7 #include "shtransform.h"
8 #include "basic_algebra.h"
9 
10 #include <math.h>
11 #include <string.h>
12 #include <ctype.h>
13 #include <xlocale.h>
14 #include <time.h>
15 #include <unistd.h>
16 #ifdef _OPENMP
17  #include <omp.h>
18 #endif
19 
36 void CreateBranch(Tbranch *b);
37 
46 void CopyBranch(Tbranch *b_dst,Tbranch *b_src);
47 
58 void AddStepToBranch(double s,Tjoint *joint,Tbranch *b);
59 
69 unsigned int nStepsBranch(Tbranch *b);
70 
84 void GetBranchStep(unsigned int i,double *s,Tjoint **joint,Tbranch *b);
85 
95 unsigned int GetBranchOrigin(Tbranch *b);
96 
106 unsigned int GetBranchDestination(Tbranch *b);
107 
108 
122 void MergeBranches(Tbranch *b1,Tbranch *b2,Tbranch *b);
123 
136 
144 void DeleteBranch(Tbranch *b);
145 
146 /*********************************************************************/
147 
149 {
151 }
152 
153 void CopyBranch(Tbranch *b_dst,Tbranch *b_src)
154 {
155  unsigned int i,n;
156  double s;
157  Tjoint *jo;
158 
159  CreateBranch(b_dst);
160  n=nStepsBranch(b_src);
161  for(i=0;i<n;i++)
162  {
163  GetBranchStep(i,&s,&jo,b_src);
164  AddStepToBranch(s,jo,b_dst);
165  }
166 }
167 
168 void AddStepToBranch(double s,Tjoint *joint,Tbranch *b)
169 {
170  TBranchStep *st;
171 
172  NEW(st,1,TBranchStep);
173 
174  st->joint=joint;
175  st->sign=s;
176 
177  NewVectorElement(&st,&(b->steps));
178 }
179 
180 unsigned int nStepsBranch(Tbranch *b)
181 {
182  return(VectorSize(&(b->steps)));
183 }
184 
185 void GetBranchStep(unsigned int i,double *s,Tjoint **joint,Tbranch *b)
186 {
187  TBranchStep **st;
188 
189  st=(TBranchStep **)GetVectorElement(i,&(b->steps));
190  if (st==NULL)
191  Error("Wrong index in GetBranchStep");
192  else
193  {
194  *s=(*st)->sign;
195  *joint=(*st)->joint;
196  }
197 }
198 
199 unsigned int GetBranchOrigin(Tbranch *b)
200 {
201  Tjoint *jo;
202  double s;
203  unsigned int k;
204 
205  GetBranchStep(0,&s,&jo,b);
206  if (s>0)
207  k=JointFromID(jo);
208  else
209  k=JointToID(jo);
210 
211  return(k);
212 }
213 
215 {
216  Tjoint *jo;
217  double s;
218  unsigned int k;
219 
220  GetBranchStep(nStepsBranch(b)-1,&s,&jo,b);
221  if (s>0)
222  k=JointToID(jo);
223  else
224  k=JointFromID(jo);
225 
226  return(k);
227 }
228 
230 {
231  /* We get two branches (b1,b2) representing two paths from
232  the groundLink to the same link.
233  The output 'b' will be the closed branch resulting from
234  eliminating from 'b1' and 'b2' the initial common path
235  from the groudnLink to where the paths differ */
236 
237  signed int k,n1,n2,i,nm;
238  Tjoint *jo1,*jo2;
239  boolean equal;
240  double s;
241 
242  n1=(signed int)nStepsBranch(b1);
243  n2=(signed int)nStepsBranch(b2);
244  nm=(n1<n2?n1:n2);
245  k=0;
246  equal=((n1>0)&&(n2>0));
247  while ((k<nm)&&(equal))
248  {
249  GetBranchStep((unsigned int)k,&s,&jo1,b1);
250  GetBranchStep((unsigned int)k,&s,&jo2,b2);
251  if (jo1==jo2)
252  k++;
253  else
254  equal=FALSE;
255  }
256 
257  CreateBranch(b);
258 
259  for(i=k;i<n1;i++)
260  {
261  GetBranchStep(i,&s,&jo1,b1);
262  AddStepToBranch(s,jo1,b);
263  }
264 
265  for(i=n2-1;i>=k;i--)
266  {
267  GetBranchStep(i,&s,&jo2,b2);
268  AddStepToBranch(-s,jo2,b);
269  }
270 }
271 
273 {
274  unsigned int i,n;
275  TBranchStep *st;
276  THTransform ti;
277 
279  n=nStepsBranch(b);
280  for(i=0;i<n;i++)
281  {
282  st=*(TBranchStep **)GetVectorElement(i,&(b->steps));
283  if (st->sign>0)
284  HTransformProduct(t,&(tj[GetJointID(st->joint)]),t);
285  else
286  {
287  HTransformInverse(&(tj[GetJointID(st->joint)]),&ti);
288  HTransformProduct(t,&ti,t);
289  }
290  }
291 }
292 
294 {
295  unsigned int i,n;
296  TBranchStep *st;
297 
298  n=nStepsBranch(b);
299  for(i=0;i<n;i++)
300  {
301  st=*(TBranchStep **)GetVectorElement(i,&(b->steps));
302  free(st);
303  }
304  DeleteVector(&(b->steps));
305 }
306 
307 
308 /*********************************************************************/
309 
337 unsigned int Branches2Links(unsigned int from,unsigned int *links,unsigned int *jointTo,
338  boolean *isLeaf,Tbranch *b,Tworld *w);
339 
363 void GenerateKinTree(Tvector *bOut,Tworld *w);
364 
377 
397 void GenerateTransEquationsFromBranch(Tparameters *p,unsigned int eq_type,
398  Tequation *eqs,Tbranch *b,Tworld *w);
399 
425 boolean GenerateEquationsFromBranch(Tparameters *p,unsigned int eq_type,
426  TCuikSystem *cs,Tbranch *b,Tworld *w);
427 
428 
438 
447 void WorldInitDOFInfo(Tworld *w);
448 
456 void WorldDeleteDOFInfo(Tworld *w);
457 
470 void GetLinkTransformsFromDOF(double *dof,THTransform **tl,TLinkConf **def,Tworld *w);
471 
493 void GetLinkTransformsFromSolution(Tparameters *p,double *sol,
494  THTransform **tl,TLinkConf **def,Tworld *w);
495 
516 boolean IsRevoluteBinaryLink(unsigned int nl,double ***p,Tworld *w);
517 
527 void InitWorldKinCS(Tparameters *p,Tworld *w);
528 
539 void InitWorldCS(Tworld *w);
540 
548 void DeleteWorldCS(Tworld *w);
549 
557 void DeleteWorldCD(Tworld *w);
558 
559 /************************************************************************/
560 unsigned int Branches2Links(unsigned int from,unsigned int *links,unsigned int *jointTo,
561  boolean *isLeaf,Tbranch *b,Tworld *w)
562 {
563  boolean *visited; /* visited links */
564  unsigned int c; /* current link in expansion (index in 'links' array) */
565  unsigned int i,j,t,f,nte,o,d;
566  Tjoint *jo;
567  double s;
568  boolean found;
569 
570  NEW(visited,w->nl,boolean);
571  for(i=0;i<w->nl;i++)
572  visited[i]=FALSE;
573 
574  c=0; /* current position in 'links' */
575  links[c]=from; /* we start expanding form node 'from' */
576  visited[from]=TRUE;
577  nte=1; /* number of elements in 'links' */
578  jointTo[from]=NO_UINT; /* node 'from' is the root (no joint lead to it). */
579  isLeaf[from]=TRUE; /* initially 'from' is a leaf link */
580 
581  /* Initialize the branch to the first link (the only visited so far) */
582  CreateBranch(&(b[from]));
583 
584  while(c<nte)
585  {
586  /* look all the joints that end/start at current links[c] */
587  found=FALSE;
588  for(j=0;((!found)&&(j<w->nj));j++)
589  {
590  jo=GetMechanismJoint(j,&(w->m));
591  t=JointToID(jo);
592  f=JointFromID(jo);
593  /* It is probably a good idea to try to avoid "inverse" joints
594  as much as possible */
595  if ((t==links[c])||(f==links[c]))
596  {
597  if (t==links[c])
598  {o=t;d=f;s=-1.0;}
599  else
600  {o=f;d=t;s=+1.0;}
601 
602  if (!visited[d])
603  {
604  /* Depth-first search produces longer equations */
605  /*found=TRUE;*/ /* comment this to get a Breadth-first search */
606 
607  isLeaf[o]=FALSE; /* Now link 'o' has a child */
608  links[nte++]=d; /* Add link 'd' to the list of nodes to expand */
609  visited[d]=TRUE; /* link 'd' is already reached */
610  jointTo[d]=j;
611  isLeaf[d]=TRUE; /* initially link 'd' is a leaf */
612  CopyBranch(&(b[d]),&(b[o]));
613  AddStepToBranch(s,jo,&(b[d]));
614  }
615  }
616  }
617  c++;
618  }
619 
620  /* Create an empty branch for the non-visited links (if any) */
621  for(i=0;i<w->nl;i++)
622  {
623  if (!visited[i])
624  CreateBranch(&(b[i]));
625  }
626 
627  free(visited);
628 
629  return(nte);
630 }
631 
633 {
634  boolean *usedJoint;
635  Tbranch *b,*b1;
636  unsigned int i,n,j,lfID,ltID;
637  Tjoint newJoint,*jo;
638  Tlink *ground;
639 
640  /*First we ensure that the mechanism is fully connected*/
641  ground=GetMechanismLink(0,&(w->m));
642 
643  NEW(w->branch2Link,w->nl,Tbranch);
644  NEW(w->sortedLinks,w->nl,unsigned int);
645  NEW(w->jointTo,w->nl,unsigned int);
646  NEW(w->openLink,w->nl,boolean);
648  if (n!=w->nl)
649  {
650  /* we need to reach all links (link 0 is root!) */
651  for(i=1;i<w->nl;i++)
652  {
653  if (nStepsBranch(&(w->branch2Link[i]))==0)
654  {
655  NewFreeJoint(w->nj,
656  0,ground,
657  i,GetMechanismLink(i,&(w->m)),&newJoint);
658  j=AddJoint2Mechanism(&newJoint,&(w->m));
659  DeleteJoint(&newJoint);
660 
661  /* define the tree info for this link */
662  AddStepToBranch(1.0,GetMechanismJoint(j,&(w->m)),&(w->branch2Link[i]));
663  w->sortedLinks[n++]=i;
664  w->jointTo[i]=j;
665  w->openLink[0]=FALSE; /* just in case :) */
666  w->openLink[i]=TRUE;
667 
668  /*The number of joints is larger now*/
669  w->nj++;
670  }
671  }
672  }
673 
674  NEW(usedJoint,w->nj,boolean);
675  for(i=0;i<w->nj;i++)
676  usedJoint[i]=FALSE;
677  for(i=1;i<w->nl;i++) /* no joint takes to link 0! */
678  usedJoint[w->jointTo[i]]=TRUE;
679 
680  /* Initialize the vector with the pointers to the loop (or not loop) equations. */
681  InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,w->nl,bOut);
682 
683  /* Here, the non-used joints form a basis of cycles */
684  for(i=0;i<w->nj;i++)
685  {
686  if (!usedJoint[i])
687  {
688  jo=GetMechanismJoint(i,&(w->m));
689  lfID=JointFromID(jo);
690  ltID=JointToID(jo);
691 
692  /* Add the non-used joint to one of the branches to close the loop */
693  NEW(b1,1,Tbranch);
694  CopyBranch(b1,&(w->branch2Link[lfID]));
695  AddStepToBranch(1,jo,b1);
696 
697  NEW(b,1,Tbranch);
698  MergeBranches(b1,&(w->branch2Link[ltID]),b);
699  NewVectorElement(&b,bOut);
700 
701  DeleteBranch(b1);
702  free(b1);
703 
704  /* Just in case, the two links are not open any more */
705  w->openLink[lfID]=FALSE;
706  w->openLink[ltID]=FALSE;
707  }
708  }
709 
710  /* Open links define open branches */
711  for(i=0;i<w->nl;i++)
712  {
713  if (w->openLink[i])
714  {
715  NEW(b,1,Tbranch);
716  CopyBranch(b,&(w->branch2Link[i]));
717  NewVectorElement(&b,bOut);
718  }
719  }
720 
721  /* Release memory */
722  free(usedJoint);
723 }
724 
726 {
727  unsigned int i,n;
728  double s;
729  Tjoint *jo;
730  Tlink *l;
731  TTransSeq ts;
732 
733  /* Id = Id */
734  InitMEquation(meq);
735 
736  /* Now add the variable terms, if any */
737  n=nStepsBranch(b);
738  for(i=0;i<n;i++)
739  {
740  GetBranchStep(i,&s,&jo,b);
741 
742  /* Traverse the current link (only has effect in case of
743  deformable links) */
744  l=JointFrom(jo);
745  GetLinkTransSeq(p,&(w->kinCS),&ts,l);
746  AddTransSeq2MEquation(s,&ts,meq);
747  DeleteTransSeq(&ts);
748 
749  GetJointTransSeq(p,&(w->kinCS),&ts,jo);
750  AddTransSeq2MEquation(s,&ts,meq);
751  DeleteTransSeq(&ts);
752  }
753  SimplifyMEquation(meq);
754 }
755 
756 void GenerateTransEquationsFromBranch(Tparameters *p,unsigned int eq_type,
757  Tequation *eq,Tbranch *b,Tworld *w)
758 {
759  unsigned int r;
760 
761  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
762 
763  if (r==REP_JOINTS)
764  Error("GenerateTransEquationsFromBranch should not be used in JOINT-based representations");
765  else
766  {
767  /*A Loop is a closed branch: a path from one link to itself
768  going through other links*/
769  unsigned int n,i;
770  double s;
771  Tjoint *jo;
772 
773  for(i=0;i<3;i++)
774  {
775  InitEquation(&(eq[i]));
776  SetEquationType(eq_type,&(eq[i]));
777  SetEquationCmp(EQU,&(eq[i]));
778  }
779 
780  n=nStepsBranch(b);
781  for(i=0;i<n;i++)
782  {
783  /*Get current joint destination link (by treating the
784  destitation we deal with all links since the first link
785  is the ground link and does not require to be
786  processed)*/
787  GetBranchStep(i,&s,&jo,b);
788 
789  GenerateJointEquationsInBranch(p,s,&(w->kinCS),eq,jo);
790  }
791  }
792 }
793 
794 boolean GenerateEquationsFromBranch(Tparameters *p,unsigned int eq_type,
795  TCuikSystem *cs,Tbranch *b,Tworld *w)
796 {
797  unsigned int origin,destination,i;
798  boolean closedBranch;
799  Tequation eq[3];
800  Tmonomial f;
801  char *vname,*l1name;
802  Tlink *l1;
803  Tvariable var;
804  Tinterval rangeInf;
805  unsigned int vID;
806  unsigned int r;
807 
808  if (nStepsBranch(b)==0)
809  {
810  /* An empty branch is a branch from ground link to ground link and it
811  ony appears when trying to give coordinates to the ground link ??*/
812  origin=0;
813  destination=0;
814  closedBranch=FALSE;
815  }
816  else
817  {
818  origin=GetBranchOrigin(b);
819  destination=GetBranchDestination(b);
820  closedBranch=(origin==destination);
821  }
822 
823  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
824 
825  if (r==REP_JOINTS)
826  {
827  if (closedBranch)
828  {
829  TMequation meq;
830 
831  GenerateMEquationFromBranch(p,b,&meq,w);
832  AddMatrixEquation2CS(p,&meq,cs);
833  DeleteMEquation(&meq);
834  }
835  }
836  else
837  {
838  GenerateTransEquationsFromBranch(p,eq_type,eq,b,w);
839 
840  InitMonomial(&f);
841  NewInterval(-w->maxCoord,w->maxCoord,&rangeInf);
842  for(i=0;i<3;i++)
843  {
844  if (!closedBranch)
845  {
846  l1=GetMechanismLink(destination,&(w->m));
847  l1name=GetLinkName(l1);
848 
849  NEW(vname,strlen(l1name)+100,char);
850  LINK_TRANS(vname,l1name,i);
851 
852  NewVariable(SYSTEM_VAR,vname,&var);
853  free(vname);
854  SetVariableInterval(&rangeInf,&var);
855 
856  vID=AddVariable2CS(&var,cs);
857 
858  DeleteVariable(&var);
859 
860  AddCt2Monomial(-1.0,&f);
861  AddVariable2Monomial(NFUN,vID,1,&f);
862  AddMonomial(&f,&(eq[i]));
863  ResetMonomial(&f);
864 
865  SetEquationType(SYSTEM_EQ,&(eq[i]));
866  }
867 
868  AddEquation2CS(p,&(eq[i]),cs);
869  DeleteEquation(&(eq[i]));
870  }
871  DeleteInterval(&rangeInf);
872  DeleteMonomial(&f);
873 
874  }
875  return(!closedBranch);
876 }
877 
878 
879 boolean IsRevoluteBinaryLink(unsigned int nl,double ***p,Tworld *w)
880 {
881  unsigned int i,j,nr,f,t,nnr;
882  Tjoint *jo;
883 
884  i=0;
885  nr=0; /* number of revolute joints in link */
886  nnr=0;/* numnrt of non-revolute joints in link */
887  while ((i<w->nj)&&(nnr==0)&&(nr<3))
888  {
889  jo=GetMechanismJoint(i,&(w->m));
890  f=JointFromID(jo);
891  t=JointToID(jo);
892 
893  if ((f==nl)||(t==nl))
894  {
895  if (GetJointType(jo)==REV_JOINT)
896  {
897  if (nr<2)
898  {
899  for(j=0;j<2;j++)
900  GetJointPoint(((f==nl)?0:1),j,p[nr][j],jo);
901  }
902  nr++; /*a new revolute joint on the link*/
903  }
904  else
905  nnr++; /*a new non-revolute joint on the link*/
906  }
907 
908  i++;
909  }
910  /*
911  Uncomment the last expression to force the axes to be co-puntual
912  */
913  return((nnr==0)&&(nr==2)); //&&(Distance(3,p[0][0],p[1][0])<ZERO));
914 }
915 
917 {
918  unsigned int i,j;
919  Tvector branches;
920  unsigned int nBranches;
921  Tbranch *b;
922  Tjoint *jo;
923  double ***points;
924  unsigned int r;
925 
926  if (w->nl==0)
927  Error("Empty mechanism");
928 
929  if (w->nl==1)
930  Error("A world with just one link makes no sense");
931  else
932  {
933  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
934 
935  /*In some cases it is advisable to re-define the reference frame for the
936  links to get simpler equations*/
937  if ((r!=REP_JOINTS)&&(AllRevolute(&(w->m))))
938  {
939  NEW(points,2,double **);
940  for(i=0;i<2;i++)
941  {
942  NEW(points[i],2,double *);
943  for(j=0;j<2;j++)
944  NEW(points[i][j],3,double);
945  }
946 
947  for(i=0;i<w->nl;i++)
948  {
949  if (IsRevoluteBinaryLink(i,points,w))
950  ChangeLinkReferenceFrame(r,points[0],points[1],
951  GetMechanismLink(i,&(w->m)));
952  }
953 
954  for(i=0;i<2;i++)
955  {
956  for(j=0;j<2;j++)
957  free(points[i][j]);
958  free(points[i]);
959  }
960  free(points);
961  }
962 
963  /* All links appear in the kinCS: we add the variables and equations
964  defining the rotation matrix for each link. */
965  for(i=0;i<w->nl;i++)
966  GenerateLinkRot(p,i,&(w->kinCS),GetMechanismLink(i,&(w->m)));
967 
968  /* Compute the paths from ground link to all other links (this ensures
969  that links are fully connected adding as many free joints as
970  necessary. */
971  GenerateKinTree(&branches,w);
972  nBranches=VectorSize(&branches);
973 
974  /* All joints appear in the kinCS: we add variables and equations
975  for the joint and joint limits. IMPORTANT: the GenerateKinTree
976  function can generate new joints !!! */
977  for(i=0;i<w->nj;i++)
978  {
979  jo=GetMechanismJoint(i,&(w->m));
980  GenerateJointEquations(p,w->maxCoord,&(w->kinCS),jo);
982  }
983 
984  /* Generate the equations for the loops (and branches to terminal links)
985  involved in the mechanism */
986  for(i=0;i<nBranches;i++)
987  {
988  b=*(Tbranch **)GetVectorElement(i,&branches);
990  DeleteBranch(b);
991  free(b);
992  }
993 
994  DeleteVector(&branches);
995  }
996 }
997 
999 {
1000  InitCuikSystem(&(w->kinCS));
1001 }
1002 
1004 {
1005  unsigned int i;
1006 
1007  for(i=0;i<w->nl;i++)
1008  DeleteBranch(&(w->branch2Link[i]));
1009  free(w->branch2Link);
1010 
1011  DeleteEquations(&(w->refEqs));
1012  DeleteJacobian(&(w->refEqsJ));
1013 
1014  free(w->openLink);
1015  free(w->sortedLinks);
1016  free(w->jointTo);
1017 
1018  DeleteCuikSystem(&(w->kinCS));
1019 
1020  free(w->systemVars);
1021 
1022  WorldDeleteDOFInfo(w);
1023 }
1024 
1025 
1026 /*********************************************************************/
1027 /*********************************************************************/
1028 
1029 
1030 void InitWorldCD(Tparameters *pr,unsigned int mt,Tworld *w)
1031 {
1032  if (w->stage!=WORLD_DEFINED)
1033  Error("Complete the workd definition before using the InitWorldCD function");
1034 
1035  if (w->wcd!=NULL)
1036  {
1037  DeleteCD(w->wcd);
1038  free(w->wcd);
1039  w->wcd=NULL;
1040  }
1041 
1042  /* The collision detection engine is only initialized if there is any
1043  collision to detect. */
1044  if (AnyCollision(w))
1045  {
1046  unsigned int e,i;
1047 
1048  e=(unsigned int)(GetParameter(CT_CD_ENGINE,pr));
1049 
1050  if (mt==0)
1051  Error("Wrong maximum number of threads in InitWorldCD");
1052 
1053  w->nwcd=mt;
1054  NEW(w->wcd,w->nwcd,TworldCD);
1055 
1056  /* when mt>0 is because OpenMP is available and will be used -> initialize in parallel */
1057  #pragma omp parallel for private(i) if (mt>1)
1058  for(i=0;i<mt;i++)
1059  InitCD(e,(mt>1),&(w->m),&(w->e),w->checkCollisionsLL,w->checkCollisionsLO,&(w->wcd[i]));
1060  }
1061 }
1062 
1064 {
1065  return(w->wcd!=NULL);
1066 }
1067 
1069 {
1070  if (w->wcd==NULL)
1071  return(FALSE);
1072  else
1073  /* we query the first collision detecion structure, even if we have many. */
1074  return(GetCDEngine(w->wcd)==C_FCL);
1075 }
1076 
1077 boolean MoveAndCheckCDFromTransforms(boolean all,unsigned int tID,
1078  THTransform *tl,TLinkConf *def,
1079  THTransform *tlPrev,TLinkConf *defPrev,
1080  Tworld *w)
1081 {
1082  if (w->wcd!=NULL)
1083  {
1084  boolean col;
1085 
1086  /* If sequential force it, even if tID is different from 0. */
1087  if (w->nwcd==0)
1088  col=CheckCollision(all,tl,def,tlPrev,defPrev,&(w->wcd[0]));
1089  else
1090  col=CheckCollision(all,tl,def,tlPrev,defPrev,&(w->wcd[tID]));
1091 
1092  if ((all)&&((w->nwcd==0)||(tID==0)))
1093  PrintCollisionInfo(tl,&(w->m),&(w->e),w->wcd);
1094 
1095  return(col);
1096  }
1097  else
1098  return(FALSE); /* no collison to detect */
1099 }
1100 
1101 
1102 boolean MoveAndCheckCD(Tparameters *p,boolean all,unsigned int tID,
1103  double *sol,double *solPrev,Tworld *w)
1104 {
1105  if (w->wcd!=NULL)
1106  {
1107  THTransform *tl,*tlPrev=NULL;
1108  boolean c;
1109  TLinkConf *def,*defPrev=NULL;
1110 
1111  GetLinkTransformsFromSolution(p,sol,&tl,&def,w);
1112  if ((solPrev!=NULL)&&(GetCDEngine(w->wcd)==C_FCL))
1113  GetLinkTransformsFromSolution(p,sol,&tlPrev,&defPrev,w);
1114 
1115  c=MoveAndCheckCDFromTransforms(all,tID,tl,def,tlPrev,defPrev,w);
1116 
1117  DeleteLinkTransforms(tl,def,w);
1118  if (tlPrev!=NULL)
1119  DeleteLinkTransforms(tlPrev,defPrev,w);
1120 
1121  return(c);
1122  }
1123  else
1124  return(FALSE); /* no collison to detect */
1125 }
1126 
1128 {
1129  if (w->wcd!=NULL)
1130  {
1131  unsigned int i;
1132 
1133  for(i=0;i<w->nwcd;i++)
1134  DeleteCD(&(w->wcd[i]));
1135 
1136  free(w->wcd);
1137  w->wcd=NULL;
1138  }
1139 }
1140 
1141 
1142 /*********************************************************************/
1143 /*********************************************************************/
1144 
1145 void GetLinkTransformsFromDOF(double *dof,THTransform **tl,TLinkConf **def,Tworld *w)
1146 {
1147  unsigned int i,lID,parent,jID;
1148  THTransform *tj,t;
1149  Tjoint *j;
1150  Tlink *l;
1151 
1152  /* get the configuration parameters for each link. */
1153  NEW(*def,w->nl,TLinkConf);
1154  for(i=0;i<w->nl;i++)
1155  {
1156  l=GetMechanismLink(i,&(w->m));
1157  GetLinkConfFromDOF(&(dof[w->link2dof[i]]),&((*def)[i]),l);
1158  }
1159 
1160  NEW(tj,w->nj,THTransform);
1161  for(i=0;i<w->nj;i++)
1162  {
1163  j=GetMechanismJoint(i,&(w->m));
1164  GetJointTransform(&(dof[w->joint2dof[i]]),&(tj[i]),j);
1165 
1166  /* if necessary apply transforms depending on the
1167  previous link configuration (i.e., traverse a
1168  deformable link). */
1169  l=JointFrom(j);
1170  lID=JointFromID(j);
1171  if (IsVariableLengthLink(l))
1172  {
1173  GetLinkTransform(&(dof[w->link2dof[lID]]),&t,l);
1174  HTransformProduct(&t,&(tj[i]),&(tj[i]));
1175  HTransformDelete(&t);
1176  }
1177  }
1178 
1179  NEW(*tl,w->nl,THTransform);
1180 #if (0)
1181  /* The old, slower way to obtain the transform for each link. */
1182  for(i=0;i<w->nj;i++)
1183  GetTransformFromBranch(tj,&((*tl)[i]),&(w->branch2Link[i]));
1184 #else
1185  /* The new way takes advantage of the kinematic tree structure
1186  to reduce the number of matrix products (does not repeat
1187  computations). */
1188  HTransformIdentity(&((*tl)[0]));
1189  for(i=1;i<w->nl;i++)
1190  {
1191  lID=w->sortedLinks[i]; /* next link whose pose can be easily computed */
1192  jID=w->jointTo[lID]; /* joint to use to compute such pose */
1193  j=GetMechanismJoint(jID,&(w->m));
1194  l=GetMechanismLink(lID,&(w->m)); /* REMOVE ME for debug only */
1195  if (lID==JointToID(j))
1196  {
1197  parent=JointFromID(j);
1198  HTransformProduct(&((*tl)[parent]),&(tj[jID]),&((*tl)[lID]));
1199  }
1200  else
1201  {
1202  parent=JointToID(j);
1203  HTransformInverse(&(tj[jID]),&t);
1204  HTransformProduct(&((*tl)[parent]),&t,&((*tl)[lID]));
1205  HTransformDelete(&t);
1206  }
1207  }
1208 #endif
1209 
1210  for(i=0;i<w->nj;i++)
1211  HTransformDelete(&(tj[i]));
1212  free(tj);
1213 }
1214 
1215 void GetLinkTransformsFromSolutionPoint(Tparameters *p,boolean simp,double *sol,
1216  THTransform **tl,TLinkConf **def,Tworld *w)
1217 {
1218  unsigned int r;
1219 
1220  if (w->stage!=WORLD_DEFINED)
1221  Error("The equations and variables are not yet defined");
1222 
1223  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1224  if (r==REP_JOINTS)
1225  {
1226  if (simp)
1227  {
1228  double *fullDOF;
1229 
1230  /* 'sol' is a simplified point in JOINTS */
1231  RegenerateOriginalPoint(p,sol,&fullDOF,&(w->kinCS));
1232  GetLinkTransformsFromDOF(fullDOF,tl,def,w);
1233 
1234  free(fullDOF);
1235  }
1236  else
1237  /* 'sol' already includes all the JOINTS */
1238  GetLinkTransformsFromDOF(sol,tl,def,w);
1239  }
1240  else
1241  {
1242  double *v;
1243 
1244  if (simp)
1245  {
1246  /* unsimplify */
1247  RegenerateOriginalPoint(p,sol,&v,&(w->kinCS));
1248  /* fill the gaps (dummy/force) vars not appearing in simplification */
1249  RegenerateMechanismSolution(p,&(w->kinCS),v,&(w->m));
1250  }
1251  else
1252  /* get dummy/force vars (i.e. not system vars) */
1253  RegenerateWorldSolutionPoint(p,sol,&v,w);
1254 
1255  GetLinkTransformsFromSolution(p,v,tl,def,w);
1256 
1257  free(v);
1258  }
1259 }
1260 
1262  double **sol,Tworld *w)
1263 {
1264  unsigned int rep,n;
1265 
1266  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1267 
1268  n=GetCSNumSystemVariables(&(w->kinCS));
1269  NEW(*sol,n,double);
1270 
1271  if (rep==REP_JOINTS)
1272  GetMechanismDOFsFromTransforms(p,tl,def,*sol,&(w->m));
1273  else
1274  {
1275  unsigned int i,nv,k;
1276  double *dof,*sample;
1277 
1278  /* Get the dof from the transforms */
1279  NEW(dof,w->ndof,double);
1280  GetMechanismDOFsFromTransforms(p,tl,def,dof,&(w->m));
1281 
1282  /* Get the sample from the dof */
1283  nv=WorldDOF2Sol(p,dof,&sample,NULL,w);
1284 
1285  /* and keep only the system variables */
1286  k=0;
1287  for(i=0;i<nv;i++)
1288  {
1289  if (w->systemVars[i])
1290  {
1291  (*sol)[k]=sample[i];
1292  k++;
1293  }
1294  }
1295 
1296  /* Release used memory */
1297  free(sample);
1298  free(dof);
1299  }
1300 
1301  return(GetCSNumSystemVariables(&(w->kinCS)));
1302 }
1303 
1304 
1306  THTransform **tl,TLinkConf **def,Tworld *w)
1307 {
1308  unsigned int rep;
1309 
1310  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
1311 
1312  if (rep==REP_JOINTS)
1313  GetLinkTransformsFromDOF(sol,tl,def,w);
1314  else
1315  {
1316  unsigned int i;
1317  Tlink *l;
1318  double *r;
1319 
1320  NEW(*tl,w->nl,THTransform);
1321  NEW(*def,w->nl,TLinkConf);
1322 
1323  NEW(r,3*w->nl,double);
1324  EvaluateEqualityEquations(FALSE,sol,r,&(w->refEqs));
1325 
1326  for(i=0;i<w->nl;i++)
1327  {
1328  l=GetMechanismLink(i,&(w->m));
1329  GetTransform2Link(p,&(w->kinCS),sol,IsGroundLink(i),
1330  &(r[3*i]),&((*tl)[i]),&((*def)[i]),l);
1331  }
1332  free(r);
1333  }
1334 }
1335 
1337 {
1338  unsigned int i;
1339 
1340  for(i=0;i<w->nl;i++)
1341  {
1342  HTransformDelete(&(tl[i]));
1343  DeleteLinkConf(&(def[i]));
1344  }
1345 
1346  free(tl);
1347  free(def);
1348 }
1349 
1350 
1351 
1352 /*********************************************************************/
1353 /*********************************************************************/
1354 /*********************************************************************/
1355 /*********************************************************************/
1356 /*********************************************************************/
1357 
1359 {
1360  InitEnvironment(&(w->e));
1361  InitMechanism(&(w->m));
1362 
1363  w->nl=0;
1364  w->nj=0;
1365  w->no=0;
1366  w->nb=0;
1367  w->np=0;
1368  w->nvars=0;
1369  w->nsvars=0;
1370  w->systemVars=NULL;
1371  w->checkCollisionsLL=NULL;
1372  w->checkCollisionsLO=NULL;
1373 
1374  w->endEffector=NO_UINT;
1375 
1376  w->branch2Link=NULL;
1377 
1379 
1380  w->wcd=NULL;
1381 }
1382 
1383 unsigned int AddLink2World(Tlink *l,boolean endEffector,Tworld *w)
1384 {
1385  unsigned int j,k;
1386 
1387  if (w->stage!=WORLD_IN_DEFINITION)
1388  Error("Links can only be added during the world definition");
1389 
1390  if (GetWorldLinkID(GetLinkName(l),w)!=NO_UINT)
1391  Error("Duplicated link identifier");
1392 
1393  if (endEffector)
1394  w->endEffector=w->nl;
1395 
1396  w->nl++;
1397  k=LinkNBodies(l);
1398  w->nb+=k;
1399  w->np+=k;
1400 
1401  /* define a new row and column in he LL table */
1402 
1403  /* expand the previous rows, if any */
1404  for(j=0;j<w->nl-1;j++)
1405  {
1406  MEM_EXPAND(w->checkCollisionsLL[j],w->nl,boolean); /* enlarge */
1407  w->checkCollisionsLL[j][w->nl-1]=FALSE;
1408  }
1409 
1410  /* create a new row */
1411  if (w->checkCollisionsLL==NULL)
1412  { NEW(w->checkCollisionsLL,w->nl,boolean*); } /* create */
1413  else
1414  { MEM_EXPAND(w->checkCollisionsLL,w->nl,boolean*); } /* enlarge */
1415 
1416  /* allocate and initialize the new row */
1417  NEW(w->checkCollisionsLL[w->nl-1],w->nl,boolean);
1418  for(j=0;j<w->nl;j++)
1419  w->checkCollisionsLL[w->nl-1][j]=FALSE;
1420 
1421  if (w->no>0)
1422  {
1423  /* define a new row in he LO table */
1424  if (w->checkCollisionsLO==NULL)
1425  { NEW(w->checkCollisionsLO,w->nl,boolean*); } /* create */
1426  else
1427  { MEM_EXPAND(w->checkCollisionsLO,w->nl,boolean*); } /* enlarge */
1428  NEW(w->checkCollisionsLO[w->nl-1],w->no,boolean);
1429  for(j=0;j<w->no;j++)
1430  w->checkCollisionsLO[w->nl-1][j]=FALSE;
1431  }
1432 
1433  return(AddLink2Mechanism(l,&(w->m)));
1434 }
1435 
1436 unsigned int AddJoint2World(Tjoint *j,Tworld *w)
1437 {
1438  if (w->stage!=WORLD_IN_DEFINITION)
1439  Error("Joints can only be added during the world definition");
1440 
1441  w->nj++;
1442 
1443  return(AddJoint2Mechanism(j,&(w->m)));
1444 }
1445 
1446 void AddLeg2World(char *name,
1447  boolean planar,
1448  unsigned int type,
1449  unsigned int lID1,unsigned int lID2,
1450  double **points,
1451  Tinterval *length,double stiffness,
1452  Tinterval *rest,Tinterval *force,
1453  double radius,unsigned int gr,
1454  Tcolor *color,unsigned int bs,
1455  Tworld *w)
1456 {
1457  Tlink l;
1458  Tjoint j;
1459  Tpolyhedron b;
1460  double **p;
1461  unsigned int lID,k;
1462  double nominalLength;
1463  Tinterval negative,positive;
1464  Tcolor colorUp;
1465 
1466  NewInterval(-INF,0.0,&negative);
1467  NewInterval(0.0,+INF,&positive);
1468 
1469  /* for extensible elements the nominalLength is set to 1
1470  (i.e., no deformation) */
1471  if (((type==CABLE)&&(IntervalSize(length)>ZERO))||
1472  (type==SPRING)||(type==PRISMATIC_BAR)||
1473  (type==PRISMATIC_LEG))
1474  nominalLength=1.0; /* override the input parameter */
1475  else
1476  nominalLength=IntervalCenter(length); /* normal legs, struts, bars,
1477  or non-extensible cables (ct length) */
1478 
1479  NEW(p,2,double*);
1480  for(k=0;k<2;k++)
1481  {
1482  NEW(p[k],3,double);
1483  p[k][0]=p[k][1]=p[k][2]=0.0;
1484  }
1485 
1486  /* Tests common to all kind of legs */
1487  if (!Intersection(length,&positive,length))
1488  Error("Leg length must be positive");
1489 
1490  if (!Intersection(rest,&positive,rest))
1491  Error("Leg rest length must be positive");
1492 
1493  if (LowerLimit(rest)>UpperLimit(length))
1494  Error("Rest length must be lower than the leg length");
1495 
1496  if (radius<ZERO)
1497  Error("The leg radius must be possitive");
1498 
1499  /* Tests and representation specific for each type of leg */
1500  switch(type)
1501  {
1502  case LEG:
1503  if (IntervalSize(length)>ZERO)
1504  Error("Leg length must be constant");
1505 
1506  if ((fabs(stiffness)>ZERO)||(!ZeroInterval(rest))||(!ZeroInterval(force)))
1507  Error("Legs do not carry force information");
1508 
1509  InitAxisXLink(name,NO_FORCE,length,force,&l);
1510 
1511  p[0][0]=0.0;
1512  p[1][0]=nominalLength;
1513 
1514  NewSphere(1.5*radius,p[0],color,gr,bs,&b);
1515  AddBody2Link(&b,&l);
1516  DeletePolyhedron(&b);
1517 
1518  NewCylinder(radius,p[0],p[1],color,2*gr,bs,&b);
1519  AddBody2Link(&b,&l);
1520  DeletePolyhedron(&b);
1521 
1522  NewSphere(1.5*radius,p[1],color,gr,bs,&b);
1523  AddBody2Link(&b,&l);
1524  DeletePolyhedron(&b);
1525 
1526  break;
1527  case STRUT:
1528  /* force must be negative */
1529  if ((stiffness>0)||
1530  (!Intersection(force,&negative,force)))
1531  Error("Struts can only support compressions (negative forces)");
1532  /* do not break */
1533  case BAR:
1534  if (IntervalSize(length)>ZERO)
1535  Error("Strut/bar length must be constant");
1536 
1537  /* A link fixed length.... */
1538  if (ZeroInterval(force))
1539  InitAxisXLink(name,NO_FORCE,length,force,&l);
1540  else
1541  InitAxisXLink(name,VAR_FORCE,length,force,&l);
1542 
1543  p[0][0]=radius;
1544  p[1][0]=nominalLength-radius;
1545 
1546  NewSphere(radius,p[0],color,gr,bs,&b);
1547  AddBody2Link(&b,&l);
1548  DeletePolyhedron(&b);
1549 
1550  NewCylinder(radius,p[0],p[1],color,2*gr,bs,&b);
1551  AddBody2Link(&b,&l);
1552  DeletePolyhedron(&b);
1553 
1554  NewSphere(radius,p[1],color,gr,bs,&b);
1555  AddBody2Link(&b,&l);
1556  DeletePolyhedron(&b);
1557 
1558  break;
1559  case PRISMATIC_LEG:
1560  if ((fabs(stiffness)>ZERO)||(!ZeroInterval(rest))||(!ZeroInterval(force)))
1561  Error("Legs do not carry force information");
1562 
1563  if (IntervalSize(length)>LowerLimit(length))
1564  Error("Wrong range in prismatic leg");
1565 
1566  InitPrismaticXLink(name,NO_FORCE,length,stiffness,rest,force,&l);
1567 
1568  p[0][0]=0.0;
1569  p[1][0]=LowerLimit(length)-2*radius;
1570 
1571  NewSphere(1.5*radius,p[0],color,gr,bs,&b);
1572  AddBody2Link(&b,&l);
1573  DeletePolyhedron(&b);
1574 
1575  NewCylinder(radius,p[0],p[1],color,2*gr,bs,&b);
1576  AddBody2Link(&b,&l);
1577  DeletePolyhedron(&b);
1578 
1579  NewSphere(radius,p[1],color,gr,bs,&b);
1580  AddBody2Link(&b,&l);
1581  DeletePolyhedron(&b);
1582 
1583  CopyColor(&colorUp,color);
1584  Add2Color(1.0,&colorUp);
1585 
1586  /* Add the upper part of the prs leg */
1587  p[0][0]= 0.0;
1588  p[1][0]=-IntervalSize(length);
1589 
1590  NewSphere(1.5*radius,p[0],&colorUp,gr,bs,&b);
1591  AddBody2Link(&b,&l);
1592  DeletePolyhedron(&b);
1593 
1594  NewCylinder(0.5*radius,p[0],p[1],&colorUp,2*gr,bs,&b);
1595  AddBody2Link(&b,&l);
1596  DeletePolyhedron(&b);
1597 
1598  NewSphere(0.5*radius,p[0],&colorUp,gr,bs,&b);
1599  AddBody2Link(&b,&l);
1600  DeletePolyhedron(&b);
1601 
1602  DeleteColor(&colorUp);
1603 
1604  break;
1605  case PRISMATIC_BAR:
1606  if (IntervalSize(length)>LowerLimit(length))
1607  Error("Wrong range in prismatic leg");
1608 
1609  if ((fabs(stiffness)<ZERO)&&(ZeroInterval(rest))&&(ZeroInterval(force)))
1610  Error("Prismatic bars must carry force information");
1611 
1612  InitPrismaticXLink(name,LINEAR_FORCE,length,stiffness,rest,force,&l);
1613 
1614  p[0][0]=radius;
1615  p[1][0]=LowerLimit(length)-radius;
1616 
1617  NewSphere(radius,p[0],color,gr,bs,&b);
1618  AddBody2Link(&b,&l);
1619  DeletePolyhedron(&b);
1620 
1621  NewCylinder(radius,p[0],p[1],color,2*gr,bs,&b);
1622  AddBody2Link(&b,&l);
1623  DeletePolyhedron(&b);
1624 
1625  NewSphere(radius,p[1],color,gr,bs,&b);
1626  AddBody2Link(&b,&l);
1627  DeletePolyhedron(&b);
1628 
1629  CopyColor(&colorUp,color);
1630  Add2Color(1.0,&colorUp);
1631 
1632  /* Add the upper part of the prs leg */
1633  p[0][0]=-radius;
1634  p[1][0]=-IntervalSize(length);
1635 
1636  NewSphere(radius,p[0],&colorUp,gr,bs,&b);
1637  AddBody2Link(&b,&l);
1638  DeletePolyhedron(&b);
1639 
1640  NewCylinder(0.5*radius,p[0],p[1],&colorUp,2*gr,bs,&b);
1641  AddBody2Link(&b,&l);
1642  DeletePolyhedron(&b);
1643 
1644  NewSphere(0.5*radius,p[0],&colorUp,gr,bs,&b);
1645  AddBody2Link(&b,&l);
1646  DeletePolyhedron(&b);
1647 
1648  DeleteColor(&colorUp);
1649 
1650  break;
1651  case CABLE:
1652  /* force must be possitive */
1653  if ((stiffness<0)||
1654  (!Intersection(force,&positive,force)))
1655  Error("Cables can only support tension (postive forces)");
1656 
1657  if ((fabs(stiffness)<ZERO)&&(ZeroInterval(rest))&&(ZeroInterval(force)))
1658  Error("Cables must carry force information");
1659 
1660  if (IntervalSize(length)<ZERO)
1661  {
1662  if ((fabs(stiffness)>ZERO)||(!ZeroInterval(rest)))
1663  Error("Can not give linear force model for constant length cables");
1664  InitAxisXLink(name,VAR_FORCE,length,force,&l);
1665  }
1666  else
1668  length,stiffness,rest,force,&l);
1669 
1670  p[0][0]=radius;
1671  p[1][0]=nominalLength-radius;
1672 
1673  NewSphere(radius,p[0],color,gr,(bs==HIDDEN_SHAPE?bs:DECOR_SHAPE),&b);
1674  AddBody2Link(&b,&l);
1675  DeletePolyhedron(&b);
1676 
1677  NewCylinder(radius,p[0],p[1],color,2*gr,(bs==HIDDEN_SHAPE?bs:DECOR_SHAPE),&b);
1678  AddBody2Link(&b,&l);
1679  DeletePolyhedron(&b);
1680 
1681  NewSphere(radius,p[1],color,gr,(bs==HIDDEN_SHAPE?bs:DECOR_SHAPE),&b);
1682  AddBody2Link(&b,&l);
1683  DeletePolyhedron(&b);
1684 
1685  if (bs!=DECOR_SHAPE)
1686  {
1687  /* the object used for collision detection has
1688  some gap at the extremes to avoid collisions
1689  with the adjacent links */
1690  p[0][0]=0.1*nominalLength;
1691  p[1][0]=0.9*nominalLength;
1692 
1693  NewCylinder(radius,p[0],p[1],color,gr,HIDDEN_SHAPE,&b);
1694  AddBody2Link(&b,&l);
1695  DeletePolyhedron(&b);
1696  }
1697 
1698  break;
1699  case SPRING:
1700  if (IntervalSize(length)<ZERO)
1701  Error("Spring length can not be constant");
1702 
1703  /* force must be possitive */
1704  if ((stiffness<0)||
1705  (!Intersection(force,&positive,force)))
1706  Error("Springs can only support tension (postive forces)");
1707 
1708  if ((fabs(stiffness)<ZERO)&&(ZeroInterval(rest))&&(ZeroInterval(force)))
1709  Error("Springs must carry force information");
1710 
1711  if (IntervalSize(rest)>ZERO)
1712  Error("Springs must have constant rest length");
1713 
1714  InitDeformXLink(name,LINEAR_FORCE,length,stiffness,rest,force,&l);
1715 
1716  if (bs!=HIDDEN_SHAPE)
1717  NewSpring(1.0,0.1,radius,gr,color,&b); /* for springs parameter 'gr'
1718  is actually the number of loops*/
1719  AddBody2Link(&b,&l);
1720  DeletePolyhedron(&b);
1721 
1722  if (bs!=DECOR_SHAPE)
1723  {
1724  /* the object used for collision detection has
1725  some gap at the extremes to avoid collisions
1726  with the adjacent links */
1727  p[0][0]=0.1;
1728  p[1][0]=1.0-0.1;
1729 
1730  NewCylinder(radius,p[0],p[1],color,gr,HIDDEN_SHAPE,&b);
1731  AddBody2Link(&b,&l);
1732  DeletePolyhedron(&b);
1733  }
1734 
1735  break;
1736  default:
1737  Error("Unknown type of leg");
1738  }
1739 
1740  lID=AddLink2World(&l,FALSE,w);
1741  DeleteLink(&l);
1742 
1743  /* Now define the joints between the links */
1744  /* First the lower joint */
1745  for(k=0;k<3;k++)
1746  {
1747  p[0][k]=points[0][k];
1748  p[1][k]=0.0;
1749  }
1750  if (planar)
1752  lID1,GetWorldLink(lID1,w),
1753  lID,GetWorldLink(lID,w),
1754  p,&j);
1755  else
1757  lID1,GetWorldLink(lID1,w),
1758  lID,GetWorldLink(lID,w),
1759  p,&j);
1760  AddJoint2World(&j,w);
1761  DeleteJoint(&j);
1762 
1763  /* Now the upper joint */
1764  for(k=0;k<3;k++)
1765  {
1766  p[0][k]=0.0;
1767  p[1][k]=points[1][k];
1768  }
1769  p[0][0]=nominalLength;
1770  if (planar)
1772  lID,GetWorldLink(lID,w),
1773  lID2,GetWorldLink(lID2,w),
1774  p,&j);
1775  else
1777  lID,GetWorldLink(lID,w),
1778  lID2,GetWorldLink(lID2,w),
1779  p,&j);
1780  AddJoint2World(&j,w);
1781  DeleteJoint(&j);
1782 
1783  for(k=0;k<2;k++)
1784  free(p[k]);
1785  free(p);
1786 }
1787 
1788 void AddObstacle2World(char *name,Tpolyhedron *o,Tworld *w)
1789 {
1790  unsigned int j;
1791 
1792  if (w->stage!=WORLD_IN_DEFINITION)
1793  Error("Obstacles can only be added during the world definition");
1794 
1795  w->no++;
1796  w->np++;
1797 
1798  if (w->nl>0)
1799  {
1800  /* the first obstacle, we create the LO table */
1801  if (w->no==1)
1802  NEW(w->checkCollisionsLO,w->nl,boolean*);
1803 
1804  /* Add a column to the LO table */
1805  for(j=0;j<w->nl;j++)
1806  {
1807  if (w->no==1)
1808  { NEW(w->checkCollisionsLO[j],w->no,boolean); /*create*/}
1809  else
1810  { MEM_EXPAND(w->checkCollisionsLO[j],w->no,boolean); /* enlarge */}
1811  w->checkCollisionsLO[j][w->no-1]=FALSE;
1812  }
1813 
1814  /* initialize the new row */
1815  for(j=0;j<w->no;j++)
1816  w->checkCollisionsLO[w->nl-1][j]=FALSE;
1817  }
1818 
1819  AddShape2Environment(name,o,&(w->e));
1820 }
1821 
1822 unsigned int GetWorldMobility(Tworld *w)
1823 {
1824  return(GetMechanismMobility(&(w->m)));
1825 }
1826 
1828 {
1829  return(IsMechanismAllSpheres(&(w->m)));
1830 }
1831 
1832 unsigned int GetWorldLinkID(char *linkName,Tworld *w)
1833 {
1834  return(GetLinkID(linkName,&(w->m)));
1835 }
1836 
1837 unsigned int GetWorldObstacleID(char *obsName,Tworld *w)
1838 {
1839  return(GetObstacleID(obsName,&(w->e)));
1840 }
1841 
1842 Tlink *GetWorldLink(unsigned int linkID,Tworld *w)
1843 {
1844  return(GetMechanismLink(linkID,&(w->m)));
1845 }
1846 
1847 Tjoint *GetWorldJoint(unsigned int jointID,Tworld *w)
1848 {
1849  return(GetMechanismJoint(jointID,&(w->m)));
1850 }
1851 
1852 unsigned int GetWorldNLinks(Tworld *w)
1853 {
1854  return(w->nl);
1855 }
1856 
1857 unsigned int GetWorldNJoints(Tworld *w)
1858 {
1859  return(w->nj);
1860 }
1861 
1862 unsigned int GetWorldNObstacles(Tworld *w)
1863 {
1864  return(w->no);
1865 }
1866 
1868 {
1869  return(w->nb);
1870 }
1871 
1873 {
1874  return(w->np);
1875 }
1876 
1878 {
1879  unsigned int i;
1880 
1881  if (w->checkCollisionsLL!=NULL)
1882  {
1883  for(i=0;i<w->nl;i++)
1884  free(w->checkCollisionsLL[i]);
1885  free(w->checkCollisionsLL);
1886  }
1887 
1888  if (w->checkCollisionsLO!=NULL)
1889  {
1890  for(i=0;i<w->nl;i++)
1891  free(w->checkCollisionsLO[i]);
1892  free(w->checkCollisionsLO);
1893  }
1894 }
1895 
1897 {
1898  unsigned int i,s,k,n,p,dl,dj;
1899  Tjoint *j;
1900  Tlink *l;
1901 
1902  if (w->stage!=WORLD_DEFINED)
1903  Error("The DOF info is to be defined at the end of the world definition");
1904 
1905  dl=0;
1906  for(i=0;i<w->nl;i++)
1907  {
1908  l=GetMechanismLink(i,&(w->m));
1909  dl+=NumLinkDOF(l);
1910  }
1911  dj=0;
1912  for(i=0;i<w->nj;i++)
1913  {
1914  j=GetMechanismJoint(i,&(w->m));
1915  dj+=GetJointDOF(j);
1916  }
1917 
1918  w->ndof=dl+dj;
1919 
1920  NEW(w->link2dof,w->nl,unsigned int);
1921  NEW(w->dof2link,w->ndof,unsigned int); /* only used for link-dofs */
1922  NEW(w->dof2param,w->ndof,unsigned int); /* only used for link-dofs */
1923 
1924  NEW(w->joint2dof,w->nj,unsigned int);
1925  NEW(w->dof2joint,w->ndof,unsigned int); /* only used for joint-dofs */
1926  NEW(w->dof2range,w->ndof,unsigned int); /* only used for joint-dofs */
1927 
1928  for(i=0;i<w->ndof;i++)
1929  {
1930  w->dof2link[i]=NO_UINT;
1931  w->dof2param[i]=NO_UINT;
1932 
1933  w->dof2joint[i]=NO_UINT;
1934  w->dof2range[i]=NO_UINT;
1935  }
1936 
1937  n=0;
1938  for(i=0;i<w->nl;i++)
1939  {
1940  l=GetMechanismLink(i,&(w->m));
1941  k=NumLinkDOF(l);
1942  if (k>0)
1943  {
1944  w->link2dof[i]=n;
1945 
1946  for(s=0;s<k;s++)
1947  {
1948  w->dof2link[n]=i;
1949  w->dof2param[n]=s;
1950  n++;
1951  }
1952  }
1953  }
1954 
1955  for(i=0;i<w->nj;i++)
1956  {
1957  j=GetMechanismJoint(i,&(w->m));
1958  p=CoupledWith(j);
1959  if (p==NO_UINT)
1960  {
1961  /* Note that if Num_DOF==0 (FIX_JOINT) nothing is done
1962  and the dof2joint and joint2dof mappings are not
1963  actually used. */
1964 
1965  w->joint2dof[i]=n;
1966 
1967  k=GetJointDOF(j);
1968  for(s=0;s<k;s++)
1969  {
1970  w->dof2joint[n]=i;
1971  w->dof2range[n]=s;
1972  n++;
1973  }
1974  }
1975  else
1976  {
1977  if (p>=i)
1978  Error("Joints can only be coupled with preceeding joints");
1979  /* We will re-use the values from the original joint. */
1980  w->joint2dof[i]=w->joint2dof[p];
1981  }
1982  }
1983 }
1984 
1985 
1987 {
1988  free(w->link2dof);
1989  free(w->dof2link);
1990  free(w->dof2param);
1991 
1992  free(w->joint2dof);
1993  free(w->dof2joint);
1994  free(w->dof2range);
1995 }
1996 
1997 unsigned int GetWorldNDOF(Tworld *w)
1998 {
1999  return(w->ndof);
2000 }
2001 
2002 void GetWorldRangeDOF(unsigned int ndof,Tinterval *r,Tworld *w)
2003 {
2004  Tlink *l;
2005  Tjoint *j;
2006 
2007  if (ndof>=w->ndof)
2008  Error("DOF out of range in GetWorldRangeDOF");
2009 
2010  if (w->dof2joint[ndof]==NO_UINT)
2011  {
2012  /* A link dof */
2013  l=GetMechanismLink(w->dof2link[ndof],&(w->m));
2014  CopyInterval(r,GetLinkDOFRange(w->dof2param[ndof],l));
2015  }
2016  else
2017  {
2018  /* A joint dof */
2019  j=GetMechanismJoint(w->dof2joint[ndof],&(w->m));
2020  GetJointRangeN(w->dof2range[ndof],w->maxCoord,r,j);
2021  }
2022 }
2023 
2024 void GetWorldJointLabel(unsigned int ndof,char **string,Tworld *w)
2025 {
2026  if (w->dof2joint==NULL)
2027  Error("Uninitialized dof info in GetWorkdJointLabel");
2028 
2029  /* Only return a string for the first dof of each joint */
2030  if (w->dof2range[ndof]==0)
2031  GetJointName(string,GetMechanismJoint(w->dof2joint[ndof],&(w->m)));
2032  else
2033  *string=NULL;
2034 }
2035 
2036 void GetWorldDOFLabel(unsigned int ndof,char **string,Tworld *w)
2037 {
2038 
2039  if (w->dof2joint==NULL)
2040  Error("Uninitialized dof info in GetWorkdDOFLabel");
2041 
2042  if (w->dof2joint[ndof]==NO_UINT)
2043  {
2044  Tlink *l;
2045  char *linkName,*paramName;
2046 
2047  /* Link dof */
2048 
2049  l=GetMechanismLink(w->dof2link[ndof],&(w->m));
2050 
2051  linkName=GetLinkName(l);
2052  GetLinkDOFLabel(w->dof2param[ndof],&paramName,l);
2053 
2054  NEW(*string,strlen(linkName)+strlen(paramName)+10,char);
2055  sprintf(*string,"%s %s",paramName,linkName);
2056 
2057  free(paramName);
2058  }
2059  else
2060  {
2061  Tjoint *j;
2062 
2063  /* Joint motion dof */
2064  j=GetMechanismJoint(w->dof2joint[ndof],&(w->m));
2065  if (GetJointDOF(j)==1)
2066  GetJointName(string,j);
2067  else
2068  {
2069  Tjoint *j;
2070  char *jointName,*dofName;
2071 
2072  j=GetMechanismJoint(w->dof2joint[ndof],&(w->m));
2073 
2074  GetJointName(&jointName,j);
2075  GetJointDOFName(w->dof2range[ndof],&dofName,j);
2076 
2077  NEW(*string,strlen(jointName)+strlen(dofName)+10,char);
2078  if (dofName!=NULL)
2079  sprintf(*string,"%s %s",jointName,dofName);
2080  else
2081  sprintf(*string,"%s",jointName);
2082 
2083  free(jointName);
2084  free(dofName);
2085  }
2086  }
2087 }
2088 
2089 inline boolean IsWorldPolynomial(Tworld *w)
2090 {
2091  if (w->stage!=WORLD_DEFINED)
2092  Error("The equations and variables are not yet defined");
2093 
2094  return(IsCSPolynomial(&(w->kinCS)));
2095 }
2096 
2097 void CheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w)
2098 {
2099  unsigned int i,j;
2100 
2101  if (w->stage!=WORLD_IN_DEFINITION)
2102  Error("The geometry information must be added while defining the world");
2103 
2104  CheckSelfCollisions(fl,w);
2105 
2106  if (w->no>fo)
2107  {
2108  /* All link-obstacles are checked for collision except
2109  the ground link with any obstacle */
2110  for(i=fl;i<w->nl;i++)
2111  {
2112  for(j=fo;j<w->no;j++)
2113  w->checkCollisionsLO[i][j]=((!IsGroundLink(i))&&
2114  (GetObstacleShapeStatus(j,&(w->e))!=DECOR_SHAPE));
2115  }
2116  }
2117 }
2118 
2119 void NoCheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w)
2120 {
2121  unsigned int i,j;
2122 
2123  if (w->stage!=WORLD_IN_DEFINITION)
2124  Error("The geometry information must be added while defining the world");
2125 
2126  NoCheckSelfCollisions(fl,w);
2127 
2128  if (w->no>fo)
2129  {
2130  for(i=fl;i<w->nl;i++)
2131  {
2132  for(j=fo;j<w->no;j++)
2133  w->checkCollisionsLO[i][j]=FALSE;
2134  }
2135  }
2136 }
2137 
2138 void CheckSelfCollisions(unsigned int fl,Tworld *w)
2139 {
2140  unsigned int i,j;
2141 
2142  if (w->stage!=WORLD_IN_DEFINITION)
2143  Error("The geometry information must be added while defining the world");
2144 
2145  /* Collisions of a link with itself are not checked */
2146  for(i=fl;i<w->nl;i++)
2147  {
2148  for(j=fl;j<w->nl;j++)
2149  w->checkCollisionsLL[i][j]=(i!=j);
2150  }
2151 }
2152 
2153 void NoCheckSelfCollisions(unsigned int fl,Tworld *w)
2154 {
2155  unsigned int i,j;
2156 
2157  if (w->stage!=WORLD_IN_DEFINITION)
2158  Error("The geometry information must be added while defining the world");
2159 
2160  for(i=fl;i<w->nl;i++)
2161  {
2162  for(j=fl;j<w->nl;j++)
2163  w->checkCollisionsLL[i][j]=FALSE;
2164  }
2165 }
2166 
2167 void CheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w)
2168 {
2169  if (w->stage!=WORLD_IN_DEFINITION)
2170  Error("The geometry information must be added while defining the world");
2171 
2172  if (a==b)
2173  Error("Repeated link identifier in NoCheckLinkLinkCollision");
2174 
2175  if ((a<w->nl)&&(b<w->nl))
2176  w->checkCollisionsLL[a][b]=w->checkCollisionsLL[b][a]=TRUE;
2177 }
2178 
2179 void NoCheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w)
2180 {
2181  if (w->stage!=WORLD_IN_DEFINITION)
2182  Error("The geometry information must be added while defining the world");
2183 
2184  if (a==b)
2185  Error("Repeated link identifier in NoCheckLinkLinkCollision");
2186 
2187  if ((a<w->nl)&&(b<w->nl))
2188  w->checkCollisionsLL[a][b]=w->checkCollisionsLL[b][a]=FALSE;
2189 }
2190 
2191 void CheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w)
2192 {
2193  if (w->stage!=WORLD_IN_DEFINITION)
2194  Error("The geometry information must be added while defining the world");
2195 
2196  if ((a<w->nl)&&(!IsGroundLink(a))&&(b<w->no))
2197  w->checkCollisionsLO[a][b]=TRUE;
2198 }
2199 
2200 void NoCheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w)
2201 {
2202  if (w->stage!=WORLD_IN_DEFINITION)
2203  Error("The geometry information must be added while defining the world");
2204 
2205  if ((a<w->nl)&&(b<w->no))
2206  w->checkCollisionsLO[a][b]=FALSE;
2207 }
2208 
2209 void CheckObstacleCollision(unsigned int fl,unsigned int b,Tworld *w)
2210 {
2211  unsigned int i;
2212 
2213  for(i=fl;i<w->nl;i++)
2215 }
2216 
2217 void NoCheckObstacleCollision(unsigned int fl,unsigned int b,Tworld *w)
2218 {
2219  unsigned int i;
2220 
2221  for(i=fl;i<w->nl;i++)
2223 }
2224 
2225 void NoCheckConnectedCollisions(unsigned int fl,Tworld *w)
2226 {
2227  unsigned int i,j,k,l1,l2;
2228  Tjoint *jo,*jo1,*jo2;
2229  Tlink *lk;
2230 
2231  /* skip the collision detection between links directly connected
2232  by a joint */
2233  for(i=0;i<w->nj;i++)
2234  {
2235  jo=GetMechanismJoint(i,&(w->m));
2236  l1=JointFromID(jo);
2237  l2=JointToID(jo);
2238  if ((l1>=fl)&&(l2>=fl))
2239  NoCheckLinkLinkCollision(l1,l2,w);
2240  }
2241  /* Skip the collision detection between links coincident at at
2242  NoRot link (punctual link connecting tensegrity elements) */
2243  for(i=fl;i<w->nl;i++)
2244  {
2245  lk=GetMechanismLink(i,&(w->m));
2246  if (GetLinkType(lk)==LINK_NoRot)
2247  {
2248  for(j=0;j<w->nj;j++)
2249  {
2250  jo1=GetMechanismJoint(j,&(w->m));
2251  if ((JointFromID(jo1)==i)||(JointToID(jo1)==i))
2252  {
2253  for(k=j+1;k<w->nj;k++)
2254  {
2255  jo2=GetMechanismJoint(k,&(w->m));
2256  if ((JointFromID(jo2)==i)||(JointToID(jo2)==i))
2257  {
2258  /* Determine the links not common beetween jo1 and jo2 */
2259  if (JointFromID(jo1)==JointFromID(jo2))
2260  {
2261  l1=JointToID(jo1);
2262  l2=JointToID(jo2);
2263  }
2264  else
2265  {
2266  if (JointFromID(jo1)==JointToID(jo2))
2267  {
2268  l1=JointToID(jo1);
2269  l2=JointFromID(jo2);
2270  }
2271  else
2272  {
2273  if (JointToID(jo1)==JointFromID(jo2))
2274  {
2275  l1=JointFromID(jo1);
2276  l2=JointToID(jo2);
2277  }
2278  else
2279  {
2280  l1=JointFromID(jo1);
2281  l2=JointFromID(jo2);
2282  }
2283  }
2284  }
2285  NoCheckLinkLinkCollision(l1,l2,w);
2286  }
2287  }
2288  }
2289  }
2290  }
2291  }
2292 }
2293 
2295 {
2296  unsigned int i;
2297  boolean any;
2298 
2299  any=FALSE;
2300  for(i=0;((i<w->nl)&&(!any));i++)
2302 
2303  return(any);
2304 }
2305 
2306 void PrintCollisions(FILE *f,Tworld *w)
2307 {
2308  if (AnyCollision(w))
2309  {
2310  unsigned int i,j;
2311  char *l;
2312 
2313  fprintf(f,"[COLLISIONS]\n\n");
2314  fprintf(f," do not check: all\n");
2315  for(i=0;i<w->nl;i++)
2316  {
2317  l=GetLinkName(GetMechanismLink(i,&(w->m)));
2318  for(j=i+1;j<w->nl;j++)
2319  {
2320  if ((w->checkCollisionsLL[i][j])||(w->checkCollisionsLL[j][i]))
2321  fprintf(f," check: %s,%s\n",l,GetLinkName(GetMechanismLink(j,&(w->m))));
2322  }
2323  }
2324  for(i=0;i<w->nl;i++)
2325  {
2326  l=GetLinkName(GetMechanismLink(i,&(w->m)));
2327  for(j=0;j<w->no;j++)
2328  if (w->checkCollisionsLO[i][j])
2329  fprintf(f," check: %s,%s\n",l,GetObstacleName(j,&(w->e)));
2330  }
2331  fprintf(f,"\n");
2332  }
2333 }
2334 
2335 boolean NewtonInWorld(Tparameters *p,double *v,Tbox *b_sol,Tworld *w)
2336 {
2337  double *vA;
2338  Tbox b_solA;
2339  boolean converged;
2340  unsigned int i,k,nv,nvs;
2341  boolean *systemVarsKin;
2342 
2343  if (w->stage!=WORLD_DEFINED)
2344  Error("The equations and variables are not yet defined");
2345 
2346  nv=GetCSSystemVars(&systemVarsKin,&(w->kinCS));
2347  NEW(vA,nv,double);
2348 
2349  converged=CuikNewton(p,vA,&b_solA,&(w->kinCS));
2350 
2351  nvs=0;
2352  for(i=0;i<nv;i++)
2353  {
2354  if (systemVarsKin[i])
2355  nvs++;
2356  }
2357 
2358  InitBox(nvs,NULL,b_sol);
2359 
2360  k=0;
2361  for(i=0;i<nv;i++)
2362  {
2363  if (systemVarsKin[i])
2364  {
2365  v[k]=vA[i];
2366  SetBoxInterval(k,GetBoxInterval(i,&b_solA),b_sol);
2367  k++;
2368  }
2369  }
2370  DeleteBox(&b_solA);
2371  free(vA);
2372  free(systemVarsKin);
2373 
2374  #if (_DEBUG>2)
2375  printf("Newton generated box:");
2376  PrintBox(stdout,b_sol);
2377  fflush(stdout);
2378  #endif
2379 
2380  return(converged);
2381 }
2382 
2383 inline unsigned int GetWorldSystemVars(boolean **sv,Tworld *w)
2384 {
2385  if (w->stage!=WORLD_DEFINED)
2386  Error("The equations and variables are not yet defined");
2387 
2388  return(GetCSSystemVars(sv,&(w->kinCS)));
2389 }
2390 
2391 inline unsigned int GetWorldVarTopology(unsigned int vID,Tworld *w)
2392 {
2393  return(GetCSVarTopology(vID,&(w->kinCS)));
2394 }
2395 
2396 unsigned int GetWorldSimpVariableMask(Tparameters *p,boolean **sv,Tworld *w)
2397 {
2398  unsigned int i,n;
2399 
2400  if (w->stage!=WORLD_DEFINED)
2401  Error("The equations and variables are not yet defined");
2402 
2403  n=GetCSNumVariables(&(w->kinCS));
2404 
2405  NEW((*sv),n,boolean);
2406  for(i=0;i<n;i++)
2407  (*sv)[i]=FALSE;
2408 
2409  for(i=0;i<w->nl;i++)
2410  GetLinkPoseSimpVars(p,*sv,&(w->kinCS),GetMechanismLink(i,&(w->m)));
2411 
2412  return(n);
2413 }
2414 
2415 inline void GetWorldVarNames(char **vn,Tworld *w)
2416 {
2417  if (w->stage!=WORLD_DEFINED)
2418  Error("The equations and variables are not yet defined");
2419 
2420  return(GetCSVariableNames(vn,&(w->kinCS)));
2421 }
2422 
2423 inline char *GetWorldSystemVarName(unsigned int nv,Tworld *w)
2424 {
2425  if (w->stage!=WORLD_DEFINED)
2426  Error("The equations and variables are not yet defined");
2427 
2428  return(GetCSSystemVariableName(nv,&(w->kinCS)));
2429 }
2430 
2432 {
2433  unsigned int i,rep;
2434  Tvariables vs;
2435 
2436  if (w->stage!=WORLD_IN_DEFINITION)
2437  Error("The equations are alrady defined");
2438 
2439  w->stage=WORLD_DEFINED;
2440 
2441  InitWorldCS(w);
2442 
2443  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2444 
2447 
2448  /*Generate the kinematic equations: this includes link reference variables
2449  and equations, joint variables and equations, joint limit variables and
2450  equations and loop equations. Note that all links and joints are used in the kinCS
2451  and thus, after generating it, we can assume that the variables associated
2452  to links and joints are all already defined.
2453  This already initializes the branch2link, the sortedLinks and the jointTo
2454  arrays.
2455  */
2456  InitWorldKinCS(p,w);
2457 
2458 
2459  /* We generate the equations defining the translation
2460  from the ground link to all other links. These equations are the
2461  same as those generated by \ref GenerateEquationsFromBranch but for
2462  open branches we do not add the variables defining the X-Y-Z
2463  of the link reference.
2464  */
2465 
2466  InitEquations(&(w->refEqs));
2467  if (rep==REP_JOINTS)
2468  {
2469  TMequation ref;
2470 
2471  for(i=0;i<w->nl;i++)
2472  {
2473  GenerateMEquationFromBranch(p,&(w->branch2Link[i]),&ref,w);
2474  AddMatrixEquation(&ref,&(w->refEqs));
2475  DeleteMEquation(&ref);
2476  }
2477  }
2478  else
2479  {
2480  Tequation ref[3];
2481  unsigned int j;
2482 
2483  for(i=0;i<w->nl;i++)
2484  {
2486  for(j=0;j<3;j++)
2487  {
2488  AddEquationNoSimp(&(ref[j]),&(w->refEqs));
2489  DeleteEquation(&(ref[j]));
2490  }
2491  }
2492  }
2493  GetCSVariables(&vs,&(w->kinCS));
2494  InitJacobian(&vs,&(w->refEqs),&(w->refEqsJ));
2495  DeleteVariables(&vs);
2496 
2497  /*copy of the system variables to use later on*/
2498  w->nvars=GetCSSystemVars(&(w->systemVars),&(w->kinCS));
2499  w->nsvars=0;
2500  for(i=0;i<w->nvars;i++)
2501  {
2502  if (w->systemVars[i])
2503  w->nsvars++;
2504  }
2505 
2506  /* Define the dof information */
2507  WorldInitDOFInfo(w);
2508 }
2509 
2511 {
2512  unsigned int rep;
2513 
2514  if (w->stage<WORLD_DEFINED)
2515  Error("GenerateForceEquilibriumEquations assumes that the equations are already generated (GenerateWorldEquations)");
2516 
2517  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2518 
2519  if (rep!=REP_JOINTS)
2520  {
2521  unsigned int i,j;
2522  Tequation eq[3],eqn;
2523  Tmonomial f;
2524  unsigned int x,y,z,le,re,fr;
2525  Tlink *l;
2526  double pt;
2527 
2528  for(i=0;i<w->nl;i++)
2529  {
2530  l=GetMechanismLink(i,&(w->m));
2531  if (IsForceEquilibriumLink(l))
2532  {
2533  for(j=0;j<3;j++)
2534  {
2535  InitEquation(&(eq[j]));
2536  SetEquationType(SYSTEM_EQ,&(eq[j]));
2537  SetEquationCmp(EQU,&(eq[j]));
2538  SetEquationValue(-GetForceOnLink(j,l),&(eq[j]));
2539  }
2540 
2541  for(j=0;j<w->nj;j++)
2542  JointForceEquation(p,i,&(w->kinCS),eq,GetMechanismJoint(j,&(w->m)));
2543 
2544  for(j=0;j<3;j++)
2545  {
2546  AddEquation2CS(p,&(eq[j]),&(w->kinCS));
2547  DeleteEquation(&(eq[j]));
2548  }
2549  }
2550  }
2551 
2553  if (pt<-ZERO)
2554  Error("Pretension paramter can not be negative");
2555 
2556  if (pt>ZERO)
2557  {
2558  /* Normalize tension to compensate for one pretension (we always have one at least).
2559  If more pre-tensions are possible, fix some tension to a constant to avoid it */
2560  InitEquation(&(eqn));
2561  SetEquationType(SYSTEM_EQ,&(eqn));
2562  SetEquationCmp(EQU,&(eqn));
2563  SetEquationValue(pt*pt,&(eqn));
2564  InitMonomial(&f);
2565 
2566  for(i=0;i<w->nl;i++)
2567  {
2568  l=GetMechanismLink(i,&(w->m));
2569  if (GetLinkForceModel(l)!=NO_FORCE)
2570  {
2571  GetLinkForceVars(rep,&x,&y,&z,&le,&re,&fr,&(w->kinCS),l);
2572  AddVariable2Monomial(NFUN,fr,2,&f);
2573  AddMonomial(&f,&eqn);
2574  ResetMonomial(&f);
2575  }
2576  }
2577  DeleteMonomial(&f);
2578 
2579  AddEquation2CS(p,&(eqn),&(w->kinCS));
2580  DeleteEquation(&(eqn));
2581  }
2582  }
2583 }
2584 
2585 unsigned int WorldForceVars(Tparameters *p,boolean **fv,Tworld *w)
2586 {
2587  unsigned int i,k,n,r;
2588 
2589  if (w->stage<WORLD_DEFINED)
2590  Error("WorldForceVars assumes that the equations are already generated (GenerateWorldEquations)");
2591 
2592  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2593 
2594  /* Identify the force variables in the system */
2595  NEW(*fv,w->nvars,boolean);
2596  for(i=0;i<w->nvars;i++)
2597  (*fv)[i]=FALSE;
2598 
2599  n=0;
2600  for(i=0;i<w->nl;i++)
2601  n+=SetForceVars(r,*fv,&(w->kinCS),GetMechanismLink(i,&(w->m)));
2602 
2603  if (n>0)
2604  {
2605  /* Compact so that only system variables are returned */
2606  k=0;
2607  for(i=0;i<w->nvars;i++)
2608  {
2609  (*fv)[k]=(*fv)[i];
2610  if (w->systemVars[k]) k++;
2611  }
2612  }
2613  return(n);
2614 }
2615 
2616 unsigned int WorldSimpKinematicVars(Tparameters *p,boolean **kv,Tworld *w)
2617 {
2618  boolean *fv;
2619  unsigned int i,n,r;
2620 
2621  if (w->stage<WORLD_DEFINED)
2622  Error("WorldSimpKinematicVars assumes that the equations are already generated (GenerateWorldEquations)");
2623 
2624  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2625 
2626  /* Identify the force variables in the system */
2627  NEW(fv,w->nvars,boolean);
2628  for(i=0;i<w->nvars;i++)
2629  fv[i]=FALSE;
2630 
2631  n=0;
2632  for(i=0;i<w->nl;i++)
2633  n+=SetForceRelatedVars(r,fv,&(w->kinCS),GetMechanismLink(i,&(w->m)));
2634  if (n==0)
2635  *kv=NULL;
2636  else
2637  {
2638  /* The kinematic vars are the non-force related vars */
2639  for(i=0;i<w->nvars;i++)
2640  fv[i]=!fv[i];
2641 
2642  n=SimplifyBooleanArray(p,fv,kv,&(w->kinCS)); /* Now simplify fv */
2643  }
2644  free(fv);
2645 
2646  return(n);
2647 }
2648 
2649 unsigned int WorldForceVarsIndices(Tparameters *p,unsigned int **iv,Tworld *w)
2650 {
2651  unsigned int n;
2652  boolean *fv;
2653 
2654  n=WorldForceVars(p,&fv,w);
2655  if (n==0)
2656  iv=NULL;
2657  else
2658  {
2659  unsigned int i,k,ns;
2660 
2661  NEW(*iv,n,unsigned int);
2663  k=0;
2664  for(i=0;i<ns;i++)
2665  {
2666  if (fv[i])
2667  {
2668  (*iv)[k]=i;
2669  k++;
2670  }
2671  }
2672  if (k!=n)
2673  Error("Wrong number of force variables in WorldForceVarsIndices");
2674  }
2675  free(fv);
2676 
2677  return(n);
2678 }
2679 
2680 double WorldPotentialEnergy(Tparameters *p,boolean simp,double *sol,void *w)
2681 {
2682  unsigned int rep;
2683  double e;
2684  Tworld *wp;
2685 
2686  wp=(Tworld *)w;
2687 
2688  if (wp->stage<WORLD_DEFINED)
2689  Error("WorldPotentialEnergy assumes that the equations are already generated (GenerateWorldEquations)");
2690 
2691  e=0.0;
2692  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2693 
2694  if (rep!=REP_JOINTS)
2695  {
2696  unsigned int i;
2697  double *fullSol;
2698  Tlink *l;
2699 
2700  if (simp)
2701  {
2702  /* unsimplify */
2703  RegenerateOriginalPoint(p,sol,&fullSol,&(wp->kinCS));
2704  /* fill the gaps (dummy/force) vars not appearing in simplification */
2705  RegenerateMechanismSolution(p,&(wp->kinCS),fullSol,&(wp->m));
2706  }
2707  else
2708  /* get dummy/force vars (i.e. non-system vars) not in the input */
2709  RegenerateWorldSolutionPoint(p,sol,&fullSol,wp);
2710 
2711  for(i=0;i<wp->nl;i++)
2712  {
2713  l=GetMechanismLink(i,&(wp->m));
2714  e+=LinkPotentialEnergy(rep,&(wp->kinCS),fullSol,l);
2715  }
2716  free(fullSol);
2717  }
2718 
2719  return(e);
2720 }
2721 
2722 void WorldForceField(Tparameters *p,boolean simp,double *sol,
2723  double **g,void *w)
2724 {
2725  unsigned int rep;
2726  Tworld *wp;
2727  double *gFull;
2728 
2729  wp=(Tworld *)w;
2730 
2731  if (wp->stage<WORLD_DEFINED)
2732  Error("WorldPotentialEnergyGradient assumes that the equations are already generated (GenerateWorldEquations)");
2733 
2734  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2735 
2736  /* gradient in original system (initially zero) */
2737  NEWZ(gFull,wp->nvars,double);
2738 
2739  if (rep!=REP_JOINTS)
2740  {
2741  unsigned int i;
2742  double *fullSol;
2743  Tlink *l;
2744 
2745  if (simp)
2746  {
2747  /* unsimplify */
2748  if (RegenerateOriginalPoint(p,sol,&fullSol,&(wp->kinCS))!=wp->nvars)
2749  Error("Missmatch in the number of variables");
2750  /* fill the gaps (dummy/force) vars not appearing in simplification */
2751  RegenerateMechanismSolution(p,&(wp->kinCS),fullSol,&(wp->m));
2752  }
2753  else
2754  /* get dummy/force vars (i.e. non-system vars) not in the input */
2755  RegenerateWorldSolutionPoint(p,sol,&fullSol,w);
2756 
2757  for(i=0;i<wp->nl;i++)
2758  {
2759  l=GetMechanismLink(i,&(wp->m));
2760  LinkForceField(rep,&(wp->kinCS),fullSol,gFull,l);
2761  }
2762 
2763  free(fullSol);
2764  }
2765 
2766  if (simp)
2767  {
2768  /* If the input is in the simplified system, so is the output. */
2769  GenerateSimplifiedPoint(p,gFull,g,&(wp->kinCS));
2770  free(gFull);
2771  }
2772  else
2773  *g=gFull;
2774 }
2775 
2777  double *proj,double *sol,
2778  double **g,void *w)
2779 {
2780  WorldForceField(p,simp,proj,g,w);
2781 }
2782 
2784 {
2785  unsigned int rep;
2786 
2787  if (w->stage<WORLD_DEFINED)
2788  Error("FixLinks assumes that the equations are already generated (GenerateWorldEquations)");
2789 
2790  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2791 
2792  if (rep!=REP_JOINTS)
2793  {
2794  Tequation ref[3],eq;
2795  unsigned int i,j;
2796  double v;
2797  Tlink *l;
2798 
2799  for(i=0;i<w->nl;i++)
2800  {
2801  l=GetMechanismLink(i,&(w->m));
2803  for(j=0;j<3;j++)
2804  {
2805  v=GetLinkTrans(j,l);
2806  if (v!=INF)
2807  {
2808  CopyEquation(&eq,&(ref[j]));
2809  SetEquationValue(GetEquationValue(&eq)+v,&eq);
2810  AddEquation2CS(p,&eq,&(w->kinCS));
2811  DeleteEquation(&eq);
2812  }
2813  DeleteEquation(&(ref[j]));
2814  }
2815  }
2816  }
2817 }
2818 
2819 void WorldFixTensegrityAddon(Tparameters *p,unsigned int linkID,
2820  double **point,unsigned int *n,Tworld *w)
2821 {
2822  unsigned int i,j;
2823  Tequation eq[3];
2824  char *lname,*vname;
2825  Tlink *l;
2826  Tmonomial f;
2827  unsigned int vID;
2828 
2829  l=GetMechanismLink(linkID,&(w->m));
2830  lname=GetLinkName(l);
2831 
2832  NEW(vname,strlen(lname)+100,char);
2833 
2834  InitMonomial(&f);
2835 
2836  for(i=0;i<3;i++)
2837  {
2838  for(j=0;j<3;j++)
2840 
2841  for(j=0;j<3;j++)
2842  {
2843  LINK_TRANS(vname,lname,j);
2844 
2845  vID=GetCSVariableID(vname,&(w->kinCS));
2846  if (vID==NO_UINT)
2847  Error("Undefined variable in WorldFixTensegrityAddon");
2848 
2849  AddCt2Monomial(-1.0,&f);
2850  AddVariable2Monomial(NFUN,vID,1,&f);
2851  AddMonomial(&f,&(eq[j]));
2852  ResetMonomial(&f);
2853  }
2854 
2855  ApplyLinkRot(p,-1,NO_UINT,point[i],eq,&(w->kinCS),FALSE,l);
2856 
2857  for(j=0;j<3;j++)
2858  {
2859  AddEquation2CS(p,&(eq[j]),&(w->kinCS));
2860  DeleteEquation(&(eq[j]));
2861  }
2862  }
2863  free(vname);
2864  DeleteMonomial(&f);
2865 }
2866 
2868 {
2869  unsigned int rep;
2870 
2871  if (w->stage<WORLD_DEFINED)
2872  Error("FixZToZero assumes that the equations are already generated (GenerateWorldEquations)");
2873 
2874  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2875 
2876  if (rep!=REP_JOINTS)
2877  {
2878  unsigned int i;
2879  Tlink *l;
2880 
2881  for(i=0;i<w->nl;i++)
2882  {
2883  l=GetMechanismLink(i,&(w->m));
2884  FixLinkZToZero(p,i,&(w->kinCS),l);
2885  }
2886  }
2887 }
2888 
2890  unsigned int t,
2891  unsigned int lID1,unsigned int lID2,
2892  double scale,THTransform *r,
2893  Tworld *w)
2894 {
2895  unsigned int rep;
2896 
2897  if (w->stage<WORLD_DEFINED)
2898  Error("WorldCoupleTensegrityVariable assumes that the equations are already generated (GenerateWorldEquations)");
2899 
2900  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2901 
2902  if (rep!=REP_JOINTS)
2903  {
2904  Tlink *l1,*l2;
2905  unsigned int vID[3];
2906  unsigned int le1,re1,f1;
2907  unsigned int le2,re2,f2;
2908 
2909 
2910  l1=GetWorldLink(lID1,w);
2911  l2=GetWorldLink(lID2,w);
2912 
2913  if (t==COUPLE_ORIENTATION)
2914  {
2915  Tequation eq[3];
2916  unsigned int i,j;
2917  Tmonomial f;
2918  InitMonomial(&f);
2919 
2920  for(i=0;i<3;i++)
2921  InitEquation(&(eq[i]));
2922 
2923  GetLinkForceVars(rep,&(vID[0]),&(vID[1]),&(vID[2]),
2924  &le1,&re1,&f1,&(w->kinCS),l1);
2925 
2926  /* director vector of link 1 = */
2927  for(i=0;i<3;i++)
2928  {
2929  AddVariable2Monomial(NFUN,vID[i],1,&f);
2930  AddMonomial(&f,&(eq[i]));
2931  ResetMonomial(&f);
2932  }
2933 
2934  GetLinkForceVars(rep,&(vID[0]),&(vID[1]),&(vID[2]),
2935  &le2,&re2,&f2,&(w->kinCS),l2);
2936 
2937  /* R * director vector of link 2 */
2938  for(i=0;i<3;i++)
2939  {
2940  for(j=0;j<3;j++)
2941  {
2943  AddVariable2Monomial(NFUN,vID[j],1,&f);
2944  AddMonomial(&f,&(eq[i]));
2945  ResetMonomial(&f);
2946  }
2947  }
2948 
2949  for(i=0;i<3;i++)
2950  {
2951  SetEquationCmp(EQU,&(eq[i]));
2952  SetEquationValue(0,&(eq[i]));
2953  SetEquationType(SYSTEM_EQ,&(eq[i]));
2954 
2955  AddEquation2CS(p,&(eq[i]),&(w->kinCS));
2956  DeleteEquation(&(eq[i]));
2957  }
2958 
2959  DeleteMonomial(&f);
2960  }
2961  else
2962  {
2963  Tequation eq;
2964  Tmonomial f;
2965 
2966  if (GetLinkForceModel(l1)==NO_FORCE)
2967  Error("Defining a force couple over a non-tensegrity link");
2968 
2969  if (GetLinkForceModel(l2)==NO_FORCE)
2970  Error("Defining a force couple over a non-tensegrity link");
2971 
2972  GetLinkForceVars(rep,&(vID[0]),&(vID[1]),&(vID[2]),
2973  &le1,&re1,&f1,&(w->kinCS),l1);
2974  GetLinkForceVars(rep,&(vID[0]),&(vID[1]),&(vID[2]),
2975  &le2,&re2,&f2,&(w->kinCS),l2);
2976 
2977  InitEquation(&eq);
2978  InitMonomial(&f);
2979 
2980  switch(t)
2981  {
2982  case COUPLE_LENGTH:
2983  if ((!IsVariableLengthLink(l1))||
2984  (!IsVariableLengthLink(l2)))
2985  Error("Can not couple the length of fixed-length links");
2986  AddVariable2Monomial(NFUN,le1,1,&f);
2987  AddMonomial(&f,&eq);
2988  ResetMonomial(&f);
2989 
2990  AddCt2Monomial(-scale,&f);
2991  AddVariable2Monomial(NFUN,le2,1,&f);
2992  AddMonomial(&f,&eq);
2993  ResetMonomial(&f);
2994  break;
2995 
2996  case COUPLE_REST:
2997  if ((GetLinkForceModel(l1)!=LINEAR_FORCE)||
2999  Error("Can not couple the rest length of links without linear force model");
3000 
3001  AddVariable2Monomial(NFUN,re1,1,&f);
3002  AddMonomial(&f,&eq);
3003  ResetMonomial(&f);
3004 
3005  AddCt2Monomial(-scale,&f);
3006  AddVariable2Monomial(NFUN,re2,1,&f);
3007  AddMonomial(&f,&eq);
3008  ResetMonomial(&f);
3009  break;
3010 
3011  case COUPLE_FORCE:
3012  if ((GetLinkForceModel(l1)==NO_FORCE)||
3013  (GetLinkForceModel(l2)==NO_FORCE))
3014  Error("Can not couple the forces of links without force model");
3015 
3016  AddVariable2Monomial(NFUN,f1,1,&f);
3017  AddMonomial(&f,&eq);
3018  ResetMonomial(&f);
3019 
3020  AddCt2Monomial(-scale,&f);
3021  AddVariable2Monomial(NFUN,f2,1,&f);
3022  AddMonomial(&f,&eq);
3023  ResetMonomial(&f);
3024  break;
3025  default:
3026  Error("Unknow type of couple in WorldCoupleTensegrityVariable");
3027  }
3028 
3029  SetEquationCmp(EQU,&eq);
3030  SetEquationValue(0,&eq);
3032 
3033  AddEquation2CS(p,&eq,&(w->kinCS));
3034  DeleteEquation(&eq);
3035  }
3036  }
3037 }
3038 
3040 {
3041  unsigned int i,rep;
3042 
3043  if (w->stage!=WORLD_DEFINED)
3044  GenerateWorldEquations(p,w); /* Generate kin equations only */
3045 
3046  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3047  if (rep==REP_JOINTS)
3048  Error("GenerateWorldSingularityEquations is not defined for DOF-based representations");
3049 
3050  CopyCuikSystem(cs,&(w->kinCS));
3051 
3052  for(i=0;i<w->nj;i++)
3054 
3055  if (ln!=NULL)
3056  {
3057  unsigned int lk;
3058 
3059  lk=GetMechanismLinkID(ln,&(w->m));
3060  if (lk!=NO_UINT)
3061  {
3062  Tequation eq;
3063  char *vname;
3064  Tlink *l;
3065  char *lname;
3066  Tinterval rangeInf;
3067  Tvariable var;
3068  Tmonomial f;
3069  unsigned int vID;
3070 
3071  l=GetMechanismLink(lk,&(w->m));
3072  lname=GetLinkName(l);
3073  NEW(vname,strlen(lname)+100,char);
3074  NewInterval(-w->maxCoord,w->maxCoord,&rangeInf);
3075  InitMonomial(&f);
3076 
3077  for(i=0;i<3;i++)
3078  {
3079  CopyEquation(&eq,GetEquation(lk*3+i,&(w->refEqs)));
3080 
3081  LINK_TRANS(vname,lname,i);
3082 
3083  NewVariable(CARTESIAN_VAR,vname,&var);
3084  SetVariableInterval(&rangeInf,&var);
3085  vID=AddVariable2CS(&var,cs);
3086  DeleteVariable(&var);
3087 
3088  AddCt2Monomial(-1.0,&f);
3089  AddVariable2Monomial(NFUN,vID,1,&f);
3090  AddMonomial(&f,&eq);
3091  ResetMonomial(&f);
3092 
3094 
3095  AddEquation2CS(p,&eq,cs);
3096 
3097  DeleteEquation(&eq);
3098  }
3099  free(vname);
3100  DeleteMonomial(&f);
3101  }
3102  }
3103 }
3105 {
3106  unsigned int i,nv,vID;
3107  boolean *selectedVars;
3108  char *vname;
3109 
3111 
3112  nv=GetCSNumVariables(cs);
3113  NEW(selectedVars,nv,boolean);
3114 
3115  /*in principle all variables are to be used in the Jacobian*/
3116  for(i=0;i<nv;i++)
3117  selectedVars[i]=TRUE;
3118 
3119  /*but we exclude those for translation*/
3120  NEW(vname,strlen(ln)+100,char);
3121  for(i=0;i<3;i++)
3122  {
3123  LINK_TRANS(vname,ln,i);
3124 
3125  vID=GetCSVariableID(vname,cs);
3126  if (vID==NO_UINT)
3127  Error("Translation variable is not included in the cuiksystem (in GenerateWorldTWSEquations)");
3128  selectedVars[vID]=FALSE;
3129  }
3130 
3131  AddSimplifiedJacobianEquations(p,selectedVars,cs);
3132 
3133  free(selectedVars);
3134  free(vname);
3135 }
3136 
3138 {
3139  if (w->stage!=WORLD_DEFINED)
3140  Error("World without equations in GetWorldKinJacobian");
3141 
3142  GetCSJacobian(J,&(w->kinCS));
3143 }
3144 
3146 {
3147  if (w->stage!=WORLD_DEFINED)
3148  Error("World without equations in GetWorldKinJacobian");
3149 
3150  GetSimpCSJacobian(p,J,&(w->kinCS));
3151 }
3152 
3153 inline unsigned int GetWorldSimpTopology(Tparameters *p,
3154  unsigned int **t,Tworld *w)
3155 {
3156  if (w->stage!=WORLD_DEFINED)
3157  Error("World without equations in GetWorldSimpTopology");
3158 
3159  return(GetSimpCSTopology(p,t,&(w->kinCS)));
3160 }
3161 
3162 inline unsigned int GetWorldTopology(Tparameters *p,
3163  unsigned int **t,Tworld *w)
3164 {
3165  if (w->stage!=WORLD_DEFINED)
3166  Error("World without equations in GetWorldTopology");
3167 
3168  return(GetCSTopology(p,t,&(w->kinCS)));
3169 }
3170 
3171 unsigned int WorldDOFTopology(unsigned int **t,Tworld *w)
3172 {
3173  unsigned int i;
3174  Tjoint *j;
3175 
3176  NEW(*t,w->ndof,unsigned int);
3177  for(i=0;i<w->ndof;i++)
3178  {
3179  if (w->dof2joint[i]==NO_UINT)
3180  {
3181  /* Link parameters are all real */
3182  (*t)[i]=TOPOLOGY_R;
3183  }
3184  else
3185  {
3186  j=GetMechanismJoint(w->dof2joint[i],&(w->m));
3187  (*t)[i]=GetJointRangeTopology(w->dof2range[i],j);
3188  }
3189  }
3190  return(w->ndof);
3191 }
3192 
3193 inline unsigned int WorldSimpCuikNewton(Tparameters *p,double *pt,Tworld *w)
3194 {
3195  return(CuikNewtonSimp(p,pt,&(w->kinCS)));
3196 }
3197 
3198 inline void EvaluateWorldJacobian(double *pt,double ***J,Tworld *w)
3199 {
3200  if (w->stage!=WORLD_DEFINED)
3201  Error("World without equations in EvaluateWorldKinJacobian");
3202 
3203  EvaluateCSJacobian(pt,J,&(w->kinCS));
3204 }
3205 
3206 inline void WorldEvaluateEquations(double *pt,double *r,Tworld *w)
3207 {
3208  if (w->stage!=WORLD_DEFINED)
3209  Error("World without equations in EvaluateWorldKinJacobian");
3210 
3211  EvaluateCSEquations(pt,r,&(w->kinCS));
3212 }
3213 
3214 inline boolean WorldInequalitiesHold(double *pt,Tworld *w)
3215 {
3216  if (w->stage!=WORLD_DEFINED)
3217  Error("World without equations in WorldInequalitiesHold");
3218 
3219  return(InequalitiesHoldOnPoint(pt,&(w->kinCS)));
3220 }
3221 
3222 inline boolean WorldSimpInequalitiesHold(Tparameters *p,double *pt,Tworld *w)
3223 {
3224  if (w->stage!=WORLD_DEFINED)
3225  Error("World without equations in WorldInequalitiesHold");
3226 
3227  return(SimpInequalitiesHoldOnPoint(p,pt,&(w->kinCS)));
3228 }
3229 
3230 inline double WorldErrorInSimpInequalities(Tparameters *p,double *pt,Tworld *w)
3231 {
3232  if (w->stage!=WORLD_DEFINED)
3233  Error("World without equations in WorldErrorInSimpInequalities");
3234 
3235  return(ErrorInSimpInequalitiesOnPoint(p,pt,&(w->kinCS)));
3236 }
3237 
3238 inline void WorldEvaluateSimpEquations(Tparameters *p,double *pt,double *r,Tworld *w)
3239 {
3240  if (w->stage!=WORLD_DEFINED)
3241  Error("World without equations in WorldEvaluateSimpEquations");
3242 
3243  EvaluateSimpCSEquations(p,pt,r,&(w->kinCS));
3244 }
3245 
3246 inline void WorldEvaluateSubSetSimpEquations(Tparameters *p,boolean *se,double *pt,double *r,Tworld *w)
3247 {
3248  if (w->stage!=WORLD_DEFINED)
3249  Error("World without equations in WorldEvaluateSubSetSimpEquations");
3250 
3251  EvaluateSubSetSimpCSEquations(p,se,pt,r,&(w->kinCS));
3252 }
3253 
3254 double WorldErrorInEquations(double *pt,Tworld *w)
3255 {
3256  if (w->stage!=WORLD_DEFINED)
3257  Error("World without equations in WorldErrorInEquations");
3258 
3259  return(ErrorInCSEquations(pt,&(w->kinCS)));
3260 }
3261 
3262 inline double WorldErrorInSimpEquations(Tparameters *p,double *pt,Tworld *w)
3263 {
3264  if (w->stage!=WORLD_DEFINED)
3265  Error("World without equations in WorldErrorInSimpEquations");
3266 
3267  return(ErrorInSimpCSEquations(p,pt,&(w->kinCS)));
3268 }
3269 
3270 double WorldErrorFromDOFs(Tparameters *p,double *dof,Tworld *w)
3271 {
3272  double e,*v;
3273 
3274  WorldDOF2Sol(p,dof,&v,NULL,w);
3275  e=WorldErrorInEquations(v,w);
3276  free(v);
3277 
3278  return(e);
3279 }
3280 
3281 inline double EvaluateWorldCost(Tparameters *p,boolean simp,double *pt,void *w)
3282 {
3283  if (((Tworld *)w)->stage!=WORLD_DEFINED)
3284  Error("World without equations in EvaluateWorldCost");
3285 
3286  return(EvaluateCSCost(p,simp,pt,(void *)&(((Tworld *)w)->kinCS)));
3287 }
3288 
3290 {
3291  if (w->stage!=WORLD_DEFINED)
3292  Error("World without equations in EvaluateWorldKinJacobian");
3293 
3294  return(GetCSNumVariables(&(w->kinCS)));
3295 }
3296 
3297 inline unsigned int GetWorldNumSystemVariables(Tworld *w)
3298 {
3299  if (w->stage!=WORLD_DEFINED)
3300  Error("World without equations in EvaluateWorldKinJacobian");
3301 
3302  return(GetCSNumSystemVariables(&(w->kinCS)));
3303 }
3304 
3305 inline unsigned int RegenerateWorldOriginalPoint(Tparameters *p,double *s,
3306  double **o,Tworld *w)
3307 {
3308  unsigned int nv,r;
3309 
3310  if (w->stage!=WORLD_DEFINED)
3311  Error("World without equations in EvaluateWorldKinJacobian");
3312 
3313  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3314 
3315  if (r==REP_JOINTS)
3316  {
3317  /* in JOINTS all variables are system variables. Just unsimplify */
3318  nv=RegenerateOriginalPoint(p,s,o,&(w->kinCS));
3319  }
3320  else
3321  {
3322  /* Revover the original system variables and some of the dummies */
3323  nv=RegenerateOriginalPoint(p,s,o,&(w->kinCS));
3324 
3325  /* Some dummy variables that only appear in dummy equations are
3326  not present in the simplified system and their value can not be
3327  determined using 'RegenerateOriginalPoint'. We have to use the
3328  semantic of the variables to recover their value. */
3329  RegenerateMechanismSolution(p,&(w->kinCS),*o,&(w->m));
3330  }
3331  return(nv);
3332 }
3333 
3335  double **o,Tworld *w)
3336 {
3337  double *oAll;
3338  unsigned int ns,nAll,i,k;
3339 
3340  /* Solution point with all the variables */
3341  nAll=RegenerateWorldOriginalPoint(p,s,&oAll,w);
3342 
3343  /* Now select the system variables */
3344  ns=GetCSNumSystemVariables(&(w->kinCS));
3345  NEW(*o,ns,double);
3346 
3347  k=0;
3348  for(i=0;i<nAll;i++)
3349  {
3350  /*The input box only has values for the input variables*/
3351  if (w->systemVars[i])
3352  {
3353  (*o)[k]=oAll[i];
3354  k++;
3355  }
3356  }
3357 
3358  free(oAll);
3359  return(ns);
3360 }
3361 
3362 inline unsigned int WorldGenerateSimplifiedPoint(Tparameters *p,double *o,double **s,Tworld *w)
3363 {
3364  if (w->stage!=WORLD_DEFINED)
3365  Error("World without equations in EvaluateWorldKinJacobian");
3366 
3367  return(GenerateSimplifiedPoint(p,o,s,&(w->kinCS)));
3368 }
3369 
3370 unsigned int WorldGenerateSimplifiedPointFromSystem(Tparameters *p,double *o,double **s,Tworld *w)
3371 {
3372  unsigned int ns;
3373  double *oAll;
3374 
3375  if (w->stage!=WORLD_DEFINED)
3376  Error("World without equations in EvaluateWorldKinJacobian");
3377 
3378  /* get dummy/force vars (i.e. non-system vars) not in the input */
3379  RegenerateWorldSolutionPoint(p,o,&oAll,w);
3380 
3381  ns=GenerateSimplifiedPoint(p,oAll,s,&(w->kinCS));
3382 
3383  free(oAll);
3384 
3385  return(ns);
3386 }
3387 
3388 inline void GetWorldInitialBox(Tbox *b,Tworld *w)
3389 {
3390  if (w->stage!=WORLD_DEFINED)
3391  Error("The equations and variables are not yet defined");
3392 
3393  GenerateInitialBox(b,&(w->kinCS));
3394 }
3395 
3397 {
3398  if (w->stage!=WORLD_DEFINED)
3399  Error("The equations and variables are not yet defined");
3400 
3401  GenerateSimpInitialBox(p,b,&(w->kinCS));
3402 }
3403 
3404 unsigned int WorldManifoldDimension(Tparameters *p,double *point,Tworld *w)
3405 {
3406  if (w->stage!=WORLD_DEFINED)
3407  Error("The equations and variables are not yet defined");
3408 
3409  return(ManifoldDimension(p,point,&(w->kinCS)));
3410 }
3411 
3412 unsigned int MaxWorldReduction(Tparameters *p,Tbox *b,double *reduction,Tworld *w)
3413 {
3414  unsigned int s;
3415 
3416  if (w->stage!=WORLD_DEFINED)
3417  Error("The equations and variables are not yet defined");
3418 
3419  #if ((!_USE_MPI)&&(_DEBUG==1))
3420  {
3421  double sizeIn;
3422 
3423  sizeIn=GetBoxSize(w->systemVars,b);
3424  printf(" Kin <s:%g,l:%u>-> ",sizeIn,GetBoxLevel(b));
3425  }
3426  #endif
3427 
3428  s=MaxReduction(p,SYSTEM_VAR,reduction,b,&(w->kinCS));
3429  /*
3430  s can be EMPTY_BOX
3431  REDUCED_BOX
3432  REDUCED_BOX_WITH_SOLUTION
3433  ERROR_IN_PROCESS
3434  */
3435 
3436  #if ((!_USE_MPI)&&(_DEBUG==1))
3437  if (s==EMPTY_BOX)
3438  printf("e\n");
3439  else
3440  {
3441  double sizeOut;
3442 
3443  sizeOut=GetBoxSize(w->systemVars,b);
3444  printf("%g (s:%g)\n",*reduction,sizeOut);
3445  }
3446  fflush(stdout);
3447  #endif
3448 
3449  return(s);
3450 }
3451 
3452 void PlotWorld(Tparameters *pr,Tplot3d *pt,double axesLength,Tworld *w)
3453 {
3454  if (w->stage!=WORLD_DEFINED)
3455  Error("The equations and variables are not yet defined");
3456 
3457  /* We init the collision detection to be able to print
3458  collision information when moving the world */
3459  if (w->wcd==NULL)
3460  InitWorldCD(pr,1,w);
3461 
3462  Start3dBlock(pt);
3463  PlotEnvironment(pt,&(w->e));
3464  PlotMechanism(pt,axesLength,&(w->m));
3465  Close3dBlock(pt);
3466 }
3467 
3468 unsigned int RegenerateWorldSolutionPoint(Tparameters*pr,double *p,double **v,Tworld *w)
3469 {
3470  unsigned int i,n,k;
3471 
3472  if (w->stage!=WORLD_DEFINED)
3473  Error("The equations and variables are not yet defined");
3474 
3475  n=GetCSNumVariables(&(w->kinCS));
3476  NEW(*v,n,double);
3477 
3478  k=0;
3479  for(i=0;i<n;i++)
3480  {
3481  /*The input box only has values for the input variables*/
3482  if (w->systemVars[i])
3483  {
3484  (*v)[i]=p[k];
3485  k++;
3486  }
3487  else
3488  (*v)[i]=0.0; /*Non system variables are set to 0. See below! */
3489  }
3490 
3491  RegenerateMechanismSolution(pr,&(w->kinCS),*v,&(w->m));
3492 
3493  return(n);
3494 }
3495 
3496 void WorldPrintAtoms(Tparameters *pr,FILE *f,double *pt,Tworld *w)
3497 {
3498  THTransform *tl;
3499  TLinkConf *def;
3500 
3501  if (GetEnvironmentNObstacles(&(w->e)))
3502  Warning("The bodies in the environment would not be considered");
3503 
3504  GetLinkTransformsFromSolutionPoint(pr,FALSE,pt,&tl,&def,w);
3505 
3506  MechanismPrintAtoms(f,tl,&(w->m));
3507 
3508  DeleteLinkTransforms(tl,def,w);
3509 }
3510 
3511 void WorldStoreRigidGroups(Tparameters *pr,FILE *f,double *pt,Tworld *w)
3512 {
3513  THTransform *tl;
3514  TLinkConf *def;
3515 
3516  if (GetEnvironmentNObstacles(&(w->e)))
3517  Warning("The bodies in the environment would not be considered");
3518 
3519  GetLinkTransformsFromSolutionPoint(pr,FALSE,pt,&tl,&def,w);
3520 
3521  MechanismStoreRigidAtoms(f,tl,&(w->m));
3522 
3523  DeleteLinkTransforms(tl,def,w);
3524 }
3525 
3526 void WorldAtomJacobian(Tparameters *pr,double *sol,unsigned int *nr,unsigned int *nc,double ***J,Tworld *w)
3527 {
3528  double *a;
3529  unsigned int ta,*na,nb;
3530  unsigned int i,j,k,r;
3531  Tlink *l;
3532  Tpolyhedron *b;
3533 
3534  if (!IsMechanismAllSpheres(&(w->m)))
3535  Error("The world is not sphere-only (WorldAtomJacobian)");
3536 
3537  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
3538  if (r!=REP_JOINTS)
3539  Error("WorldAtomJacobian is only defined for JOINT-based representations");
3540 
3541  /* Count the number of atoms (total and per link) */
3542  NEW(na,w->nl,unsigned int); /* number of atoms per link */
3543  ta=0;
3544  for(i=0;i<w->nl;i++)
3545  {
3546  l=GetMechanismLink(i,&(w->m));
3547  nb=LinkNBodies(l);
3548  na[i]=0;
3549  for(j=0;j<nb;j++)
3550  {
3551  b=GetLinkBody(j,l);
3552  if (GetPolyhedronType(b)==SPHERE)
3553  na[i]++;
3554  }
3555  ta+=na[i];
3556  }
3557 
3558  /* Get the coordinates for each atom */
3559  NEW(a,3*ta,double); /* coordinates for each atom */
3560  k=0;
3561  for(i=0;i<w->nl;i++)
3562  {
3563  l=GetMechanismLink(i,&(w->m));
3564  nb=LinkNBodies(l);
3565  for(j=0;j<nb;j++)
3566  {
3567  b=GetLinkBody(j,l);
3568  if (GetPolyhedronType(b)==SPHERE)
3569  {
3570  GetPolyhedronCenter(&(a[k]),b);
3571  k+=3;
3572  }
3573  }
3574  }
3575 
3576  EvaluateJacobianXVectors(sol,ta,w->nl,na,a,nr,nc,J,&(w->refEqsJ));
3577 
3578  free(a);
3579 }
3580 
3582 {
3583  unsigned int n,r;
3584  double *v;
3585 
3586  if (w->stage!=WORLD_DEFINED)
3587  Error("The equations and variables are not yet defined");
3588 
3590  Warning("System-solution dimension missmatch in MoveWorld (using only the first values in the solution)");
3591 
3593  Error("Not enough values in the solution (using a different REPRESENTATION parameter?)");
3594 
3595 
3596  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
3597  if (r==REP_JOINTS)
3598  {
3599  /* In dof repre. all variables are system variables */
3600  n=GetCSNumVariables(&(w->kinCS));
3601  NEW(v,n,double);
3602 
3603  GetBoxCenter(NULL,v,b);
3604  MoveWorldDOF(pr,pt,v,w);
3605  }
3606  else
3607  {
3608  THTransform *tl;
3609  TLinkConf *def;
3610  double *vSystem;
3611 
3612  n=GetBoxNIntervals(b);
3613  NEW(vSystem,n,double);
3614  GetBoxCenter(NULL,vSystem,b);
3615 
3616  /* obtain the non-system variables form the system ones */
3617  RegenerateWorldSolutionPoint(pr,vSystem,&v,w);
3618 
3619  GetLinkTransformsFromSolution(pr,v,&tl,&def,w);
3620 
3621  if (MoveAndCheckCDFromTransforms(TRUE,0,tl,def,NULL,NULL,w))
3622  fprintf(stderr," Configuration is in collision\n");
3623  else
3624  fprintf(stderr," Configuration is NOT in collision\n");
3625 
3626  fprintf(stderr," Configuration error: %g\n",WorldErrorInEquations(v,w));
3627 
3628  MoveMechanismFromTransforms(pr,pt,tl,def,&(w->m));
3629 
3630  DeleteLinkTransforms(tl,def,w);
3631  free(vSystem);
3632  }
3633  free(v);
3634 
3635 }
3636 
3637 void PrintWorldAxes(Tparameters *pr,FILE *f,Tbox *b,Tworld *w)
3638 {
3639  unsigned int i,n;
3640  double *r;
3641  double *v,*vSystem;
3642  Tjoint *j;
3643  unsigned int rep;
3644 
3645  if (w->stage!=WORLD_DEFINED)
3646  Error("The equations and variables are not yet defined");
3647 
3648  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
3649  if (rep==REP_JOINTS)
3650  Error("PrintWorldAxes is not defined for JOINT-based representations");
3651 
3652  n=GetBoxNIntervals(b);
3653  NEW(vSystem,n,double);
3654  GetBoxCenter(NULL,vSystem,b);
3655 
3656  /* obtain the non-system variables form the system ones */
3657  RegenerateWorldSolutionPoint(pr,vSystem,&v,w);
3658 
3659  NEW(r,w->nl*3,double);
3661 
3662  for(i=0;i<w->nj;i++)
3663  {
3664  j=GetMechanismJoint(i,&(w->m));
3665  PrintJointAxes(pr,f,&(w->kinCS),v,r,j);
3666  }
3667  fprintf(f,"\n");
3668 
3669  free(r);
3670 
3671  free(v);
3672 }
3673 
3674 void PrintWorldCollisionInfo(FILE *f,char *fname,Tworld *w)
3675 {
3676  if (w->wcd!=NULL)
3677  StoreCollisionInfo(f,fname,w->endEffector,&(w->m),w->wcd);
3678 }
3679 
3680 void MoveWorldDOF(Tparameters *pr,Tplot3d *pt,double *dof,Tworld *w)
3681 {
3682  THTransform *tl;
3683  TLinkConf *def;
3684  double *sample;
3685 
3686  if (w->stage!=WORLD_DEFINED)
3687  Error("The equations and variables are not yet defined");
3688 
3689  GetLinkTransformsFromDOF(dof,&tl,&def,w);
3690 
3691  if (MoveAndCheckCDFromTransforms(TRUE,0,tl,def,NULL,NULL,w))
3692  {
3693  fprintf(stderr," Configuration is in collision\n");
3694  //PrintCollisionInfo(tl,&(w->m),&(w->e),w->wcd);
3695  }
3696  else
3697  fprintf(stderr," Configuration is NOT in collision\n");
3698 
3699  WorldDOF2Sol(pr,dof,&sample,NULL,w);
3700  fprintf(stderr," Configuration error: %g\n",WorldErrorInEquations(sample,w));
3701  free(sample);
3702 
3703  MoveMechanismFromTransforms(pr,pt,tl,def,&(w->m));
3704 
3705  DeleteLinkTransforms(tl,def,w);
3706 }
3707 
3708 unsigned int WorldDOF2Sol(Tparameters *p,double *dof,double **sol,Tbox *b,Tworld *w)
3709 {
3710  unsigned int r,i,n;
3711  Tjoint *j;
3712 
3713  if (w->stage!=WORLD_DEFINED)
3714  Error("The equations and variables are not yet defined");
3715 
3716  /* Ensure that the input dofs are in correct range */
3717  for(i=0;i<w->ndof;i++)
3718  {
3719  /* only joint dof can have topology_s */
3720  if (w->dof2joint[i]!=NO_UINT)
3721  {
3722  j=GetMechanismJoint(w->dof2joint[i],&(w->m));
3724  PI2PI(dof[i]);
3725  }
3726  }
3727 
3728  n=GetCSNumVariables(&(w->kinCS));
3729  NEW(*sol,n,double);
3730  for(i=0;i<n;i++)
3731  (*sol)[i]=INF;
3732 
3733  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3734 
3735  if (r==REP_JOINTS)
3736  /* in dof all variables are system variables */
3737  memcpy(*sol,dof,n*sizeof(double));
3738  else
3739  {
3740  unsigned int k,l1,l2;
3741  THTransform *tl;
3742  TLinkConf *def;
3743  Tjoint *j;
3744  Tlink *l;
3745 
3746  GetLinkTransformsFromDOF(dof,&tl,&def,w);
3747 
3748  for(i=0;i<w->nl;i++)
3749  {
3750  l=GetMechanismLink(i,&(w->m));
3751  /* no need to pass 'dof' to GenerateLinkSolution since the relevant dof
3752  are already stored in 'def' */
3753  GenerateLinkSolution(p,&(tl[i]),&(def[i]),&(w->kinCS),*sol,IsGroundLink(i),l);
3754  if (w->openLink[i])
3755  {
3756  /* Some links are at the end of an open chain of links (i.e., the
3757  mechanism is not fully composed by closed chains). In this case
3758  the system includes variables to represent the end position (x,y,z)
3759  of those links. Here we define the value for these variables from
3760  the translation part of the corresponding link transform. */
3761  char *vname,*ln;
3762  unsigned int vID;
3763 
3764  ln=GetLinkName(l);
3765  NEW(vname,strlen(ln)+100,char);
3766 
3767  for(k=0;k<3;k++)
3768  {
3769  LINK_TRANS(vname,ln,k);
3770 
3771  vID=GetCSVariableID(vname,&(w->kinCS));
3772  if (vID==NO_UINT)
3773  Error("Undefined reference variable in WorldDOF2Sol");
3774 
3775  (*sol)[vID]=HTransformGetElement(k,AXIS_H,&(tl[i]));
3776  }
3777  free(vname);
3778  }
3779  }
3780 
3781  for(i=0;i<w->nj;i++)
3782  {
3783  j=GetMechanismJoint(i,&(w->m));
3784  l1=JointFromID(j);
3785  l2=JointToID(j);
3786  GenerateJointSolution(p,&(dof[w->joint2dof[i]]),
3787  &(tl[l1]),&(tl[l2]),
3788  &(w->kinCS),*sol,j);
3789  }
3790 
3791  DeleteLinkTransforms(tl,def,w);
3792  }
3793 
3794  if (b!=NULL)
3795  InitBoxFromPoint(n,*sol,b);
3796 
3797  return(n);
3798 }
3799 
3800 void WorldSample2DOF(Tparameters *p,double *sample,double *dof,Tworld *w)
3801 {
3802  unsigned int r ;
3803 
3804  if (w->stage!=WORLD_DEFINED)
3805  Error("The equations and variables are not yet defined");
3806 
3807  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
3808 
3809  if (r==REP_JOINTS)
3810  {
3811  /* In JOINTS all variables are system variables */
3812  memcpy(dof,sample,sizeof(double)*w->ndof);
3813  }
3814  else
3815  {
3816  THTransform *tl;
3817  TLinkConf *def;
3818  double *v;
3819 
3820  /* Wee need a complete solution, not only one with system variables */
3821  RegenerateWorldSolutionPoint(p,sample,&v,w);
3822 
3823  /* Get a transform giving the absolute pose for each link using
3824  the full solution point. */
3825  GetLinkTransformsFromSolution(p,v,&tl,&def,w);
3826 
3827  free(v);
3828 
3829  /* Obtain the joint variables from the link absolute poses and the
3830  link configuration */
3831  GetMechanismDOFsFromTransforms(p,tl,def,dof,&(w->m));
3832 
3833  DeleteLinkTransforms(tl,def,w);
3834  }
3835 }
3836 
3837 void AnimateWorld(Tparameters *pr,char *pname,double axesLength,
3838  double frameDelay,Tlist *p,Tworld *w)
3839 {
3840  Tplot3d p3d;
3841  boolean start;
3842  Titerator it;
3843 
3844  if (w->stage!=WORLD_DEFINED)
3845  Error("The equations and variables are not yet defined");
3846 
3847  if (axesLength<0) axesLength=0;
3848 
3849  if (frameDelay<0) frameDelay=0;
3850 
3851  InitPlot3d(pname,FALSE,0,NULL,&p3d);
3852 
3853  Start3dBlock(&p3d);
3855 
3856  PlotWorld(pr,&p3d,axesLength,w);
3857 
3858  InitIterator(&it,p);
3859  First(&it);
3860 
3862  Error("The input boxes does not have enough number of variables");
3863 
3864  start=TRUE;
3865  while(!EndOfList(&it))
3866  {
3867  MoveWorld(pr,&p3d,(Tbox *)GetCurrent(&it),w);
3868  if (start)
3869  {
3870  Close3dBlock(&p3d);
3871  start=FALSE;
3872  }
3873  Delay3dObject(FRAME_RATE+frameDelay,&p3d);
3874 
3875  Advance(&it);
3876  }
3878  ClosePlot3d(TRUE,0,0,0,&p3d); /*TRUE -> quit geomview after the animation*/
3879 }
3880 
3881 void PrintWorldCS(Tparameters *p,char *fname,Tworld *w)
3882 {
3883  Tfilename fout;
3884  FILE *f;
3885 
3886  if (w->stage!=WORLD_DEFINED)
3887  Error("The equations and variables are not yet defined");
3888 
3889  CreateFileName(NULL,fname,NULL,CUIK_EXT,&fout);
3890  f=fopen(GetFileFullName(&fout),"w");
3891  if (!f) Error("Could not open file in PrintWorldCS");
3892  #if (_DEBUG>0)
3893  printf("Generating cuik file : %s\n",GetFileFullName(&fout));
3894  #endif
3895 
3896  fprintf(f,"%% Kinematic equations\n");
3897  PrintCuikSystem(p,f,&(w->kinCS));
3898  fclose(f);
3899  DeleteFileName(&fout);
3900 }
3901 
3902 void PrintWorld(char *fname,int argc,char **arg,Tworld *w)
3903 {
3904  Tfilename fworld;
3905  FILE *f;
3906  time_t t;
3907  char hostname[50];
3908 
3909  CreateFileName(NULL,fname,NULL,WORLD_EXT,&fworld);
3910  fprintf(stderr,"Writing world to : %s\n",GetFileFullName(&fworld));
3911 
3912  f=fopen(GetFileFullName(&fworld),"w");
3913  if (!f)
3914  Error("Could not open the file to store the world");
3915 
3916  /* Do not document the link/joint/obstacle definitions. */
3917  fprintf(f,"/** \\cond */\n\n");
3918 
3919  PrintMechanism(f,GetFilePath(&fworld),GetFileName(&fworld),&(w->m));
3920 
3921  PrintEnvironment(f,GetFilePath(&fworld),&(w->e));
3922 
3923  /* Print the collision information */
3924  PrintCollisions(f,w);
3925 
3926  /* Autognerated documentation to avoid errors when generating the documentation. */
3927  fprintf(f,"/** \\endcond */\n");
3928  fprintf(f,"/**\n");
3929  fprintf(f," \\file %s.%s\n\n",GetFileName(&fworld),GetFileExtension(&fworld));
3930  fprintf(f," \\brief Automatically generated world file.\n\n");
3931  if ((argc>0)&&(arg!=NULL))
3932  {
3933  unsigned int i;
3934 
3935  fprintf(f," Automatically generated world file with the command:\n\n - ");
3936  for(i=0;i<(unsigned int)argc;i++)
3937  fprintf(f,"%s ",arg[i]);
3938  fprintf(f,"\n .\n\n");
3939  }
3940  else
3941  fprintf(f," Automatically generated world file.\n");
3942  t=time(NULL);
3943  gethostname(hostname,50);
3944  fprintf(f," Created on: %s Executed at: %s\n",ctime(&t),hostname);
3945  fprintf(f,"*/\n");
3946 
3947  fclose(f);
3948 
3949  DeleteFileName(&fworld);
3950 }
3951 
3953 {
3954  DeleteEnvironment(&(w->e));
3955  DeleteMechanism(&(w->m));
3957  DeleteWorldCD(w);
3958 
3959  /* if GenerateWorldEquationSystems was called */
3960  if (w->stage==WORLD_DEFINED)
3961  DeleteWorldCS(w);
3962 }
void AddLeg2World(char *name, boolean planar, unsigned int type, unsigned int lID1, unsigned int lID2, double **points, Tinterval *length, double stiffness, Tinterval *rest, Tinterval *force, double radius, unsigned int gr, Tcolor *color, unsigned int bs, Tworld *w)
Adds a sph-sph joint to the world.
Definition: world.c:1446
boolean NewtonInWorld(Tparameters *p, double *v, Tbox *b_sol, Tworld *w)
Generates a sample from a the kinematic cuiksystem in the world using the Newton algorithm.
Definition: world.c:2335
void GetJointDOFName(unsigned int ndof, char **name, Tjoint *j)
Label for a specific dof of a joint.
Definition: joint.c:1006
void First(Titerator *i)
Moves an iterator to the first position of its associated list.
Definition: list.c:356
void GetWorldDOFLabel(unsigned int ndof, char **string, Tworld *w)
Returns a label identifying each degree of freedom.
Definition: world.c:2036
unsigned int * dof2param
Definition: world.h:326
void GetJointTransform(double *dof, THTransform *t, Tjoint *j)
Computes the transform induced by the joint.
Definition: joint.c:2537
unsigned int GetWorldObstacleID(char *obsName, Tworld *w)
Gets the identifier of an obstacle from its name.
Definition: world.c:1837
boolean Intersection(Tinterval *i1, Tinterval *i2, Tinterval *i_out)
Computes the intersection of two intervals.
Definition: interval.c:285
Set of variables of a cuiksystem.
Definition: variables.h:38
#define COUPLE_LENGTH
One of the possible variables to couple in tensegrities.
Definition: world.h:77
void RegenerateMechanismSolution(Tparameters *p, TCuikSystem *cs, double *sol, Tmechanism *m)
Computes the values for the non-system variables used to represent the rotation matrices for all link...
Definition: mechanism.c:230
unsigned int WorldManifoldDimension(Tparameters *p, double *point, Tworld *w)
Dimensionality of the solution manifold.+.
Definition: world.c:3404
void DeleteVector(void *vector)
Destructor.
Definition: vector.c:389
#define INITIAL_FRAME_DELAY
When generating 3d animations, delay before starting the animation.
Definition: world.h:39
unsigned int WorldGenerateSimplifiedPoint(Tparameters *p, double *o, double **s, Tworld *w)
Reconstruct a point in simplified kinematic system from an original point.
Definition: world.c:3362
void CheckLinkLinkCollision(unsigned int a, unsigned int b, Tworld *w)
Activates the possible collision between a particular pair of links.
Definition: world.c:2167
#define CARTESIAN_VAR
One of the possible type of variables.
Definition: variable.h:62
Tinterval * GetBoxInterval(unsigned int n, Tbox *b)
Returns a pointer to one of the intervals defining the box.
Definition: box.c:270
signed int GetMechanismMobility(Tmechanism *m)
Computes the mobility of a given mechanism.
Definition: mechanism.c:74
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:1102
void EvaluateSubSetSimpCSEquations(Tparameters *pr, boolean *se, double *p, double *r, TCuikSystem *cs)
Evaluates a subset of the simplified equation set on a point.
Definition: cuiksystem.c:5100
char * GetFileName(Tfilename *fn)
Gets the file name.
Definition: filename.c:161
#define SYSTEM_EQ
One of the possible type of equations.
Definition: equation.h:146
void WorldInitDOFInfo(Tworld *w)
Collects information about the DOF of the mechanism.
Definition: world.c:1896
unsigned int nl
Definition: world.h:238
Tequation * GetEquation(unsigned int n, Tequations *eqs)
Gets an equation from the set.
Definition: equations.c:1799
#define REP_JOINTS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:60
void NoCheckLinkObstacleCollision(unsigned int a, unsigned int b, Tworld *w)
Desactivates the possible collision between a particular link and an object in the environment...
Definition: world.c:2200
boolean IsCSPolynomial(TCuikSystem *cs)
Identifies polynomial cuiksystems.
Definition: cuiksystem.c:2696
void InitJacobian(Tvariables *vs, Tequations *eqs, TJacobian *j)
Constructor.
Definition: jacobian.c:16
unsigned int VectorSize(Tvector *vector)
Gets the number of elements in a vector.
Definition: vector.c:173
boolean LinkCanCollide(unsigned int l, unsigned int nl, unsigned int no, boolean **checkCollisionsLL, boolean **checkCollisionsLO)
Identifies links than can collide.
Definition: cd.c:2218
void AddObstacle2World(char *name, Tpolyhedron *o, Tworld *w)
Adds an obstacle to the environment in the world.
Definition: world.c:1788
#define FALSE
FALSE.
Definition: boolean.h:30
unsigned int ManifoldDimension(Tparameters *pr, double *p, TCuikSystem *cs)
Computes the dimension of the solution space.
Definition: cuiksystem.c:5312
void DeleteLinkTransforms(THTransform *tl, TLinkConf *def, Tworld *w)
Deletes transforms for each link.
Definition: world.c:1336
unsigned int SimplifyBooleanArray(Tparameters *p, boolean *bo, boolean **bs, TCuikSystem *cs)
Transforms arrays of booleans from the original to the simplified system.
Definition: cuiksystem.c:5002
void GenerateMEquationFromBranch(Tparameters *p, Tbranch *b, TMequation *meq, Tworld *w)
Generates a matrix equation from a branch.
Definition: world.c:725
unsigned int AddLink2World(Tlink *l, boolean endEffector, Tworld *w)
Adds a link to the mechanism in the world.
Definition: world.c:1383
void DeleteBranch(Tbranch *b)
Destructor.
Definition: world.c:293
Tvector steps
Definition: world.h:214
void InitWorldCD(Tparameters *pr, unsigned int mt, Tworld *w)
Initializes the collision detector.
Definition: world.c:1030
Relation between two links.
Definition: joint.h:168
void PlotMechanism(Tplot3d *pt, double axesLength, Tmechanism *m)
Adds a mechanism to a 3d scene.
Definition: mechanism.c:212
boolean WorldSimpInequalitiesHold(Tparameters *p, double *pt, Tworld *w)
Check if the inequalities hold for the simplified system.
Definition: world.c:3222
void GetPolyhedronCenter(double *c, Tpolyhedron *p)
Gets the center of the spheres.
Definition: polyhedron.c:1353
void SetEquationType(unsigned int type, Tequation *eq)
Changes the type of the equation (SYSTEM_EQ, CARTESIAN_EQ, DUMMY_EQ, DERIVED_EQ). ...
Definition: equation.c:1076
boolean ** checkCollisionsLL
Definition: world.h:250
void DeleteWorldCS(Tworld *w)
Deletes the world cuik system.
Definition: world.c:1003
unsigned int WorldGenerateSimplifiedPointFromSystem(Tparameters *p, double *o, double **s, Tworld *w)
Reconstruct a point in simplified kinematic system from an original point.
Definition: world.c:3370
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
void GetWorldInitialBox(Tbox *b, Tworld *w)
Gets the kinematic search space for a given problem.
Definition: world.c:3388
double GetBoxSize(boolean *used, Tbox *b)
Computes the size of the box.
Definition: box.c:631
TCuikSystem kinCS
Definition: world.h:270
Data structure to hold the information about the name of a file.
Definition: filename.h:271
#define BAR
One of the possible types of legs.
Definition: world.h:142
Auxiliary data for the collision detection.
Definition: cd.h:258
void PlotWorld(Tparameters *pr, Tplot3d *pt, double axesLength, Tworld *w)
Adds a world (environment plus mechanism) in a 3D scene.
Definition: world.c:3452
void InitMEquation(TMequation *me)
Construtor.
Definition: mequation.c:99
void GetWorldJointLabel(unsigned int ndof, char **string, Tworld *w)
Returns a label identifying each joint.
Definition: world.c:2024
void WorldForceFieldProjectedGradient(Tparameters *p, boolean simp, double *proj, double *sol, double **g, void *w)
The projected gradient of the force field.
Definition: world.c:2776
double GetMechanismMaxCoordinate(Tmechanism *m)
Returns the sum of the maximum coordinate value for all the links and joints in the mechanism...
Definition: mechanism.c:46
boolean * systemVars
Definition: world.h:274
void DeleteEquation(Tequation *eq)
Destructor.
Definition: equation.c:1859
A step in a kinematic branch.
Definition: world.h:185
void GetWorldSimpInitialBox(Tparameters *p, Tbox *b, Tworld *w)
Gets the kinematic simplified search space for a given problem.
Definition: world.c:3396
unsigned int * link2dof
Definition: world.h:324
void GenerateInitialBox(Tbox *box, TCuikSystem *cs)
Gives the search space in the form of a box.
Definition: cuiksystem.c:4849
unsigned int GetJointDOF(Tjoint *j)
Computes the degrees of freedom allowed by a given joint.
Definition: joint.c:1293
boolean GenerateEquationsFromBranch(Tparameters *p, unsigned int eq_type, TCuikSystem *cs, Tbranch *b, Tworld *w)
Generate equations from a branch.
Definition: world.c:794
unsigned int MaxWorldReduction(Tparameters *p, Tbox *b, double *reduction, Tworld *w)
Reduces the system variables as much as possible using the kinematic constraints. ...
Definition: world.c:3412
void GetLinkTransformsFromSolution(Tparameters *p, double *sol, THTransform **tl, TLinkConf **def, Tworld *w)
Define transforms for the links from the a solution point.
Definition: world.c:1305
void * GetVectorElement(unsigned int i, Tvector *vector)
Returns a pointer to a vector element.
Definition: vector.c:270
A homgeneous transform in R^3.
void AddMatrixEquation(TMequation *equation, Tequations *eqs)
Adds a matrix equation to the set.
Definition: equations.c:1759
void SetBoxInterval(unsigned int n, Tinterval *is, Tbox *b)
Replaces a particular interval in a box.
Definition: box.c:259
double EvaluateWorldCost(Tparameters *p, boolean simp, double *pt, void *w)
Evaluates the functions cost defined in a world.
Definition: world.c:3281
Tmechanism m
Definition: world.h:236
#define NEWZ(_var, _n, _type)
Allocates and cleans memory space.
Definition: defines.h:394
void GenerateTransEquationsFromBranch(Tparameters *p, unsigned int eq_type, Tequation *eqs, Tbranch *b, Tworld *w)
Generate equations from a branch.
Definition: world.c:756
#define STRUT
One of the possible types of legs.
Definition: world.h:134
#define FINAL_FRAME_DELAY
When generating 3d animations, delay between the end of the animation and the exit of the animation b...
Definition: world.h:53
#define SYSTEM_VAR
One of the possible type of variables.
Definition: variable.h:24
void AddShape2Environment(char *name, Tpolyhedron *o, Tenvironment *e)
Adds an obstacle (i.e., a convex polyhedron) to the environment.
Definition: environment.c:20
unsigned int AddVariable2CS(Tvariable *v, TCuikSystem *cs)
Adds a variable to the system.
Definition: cuiksystem.c:2532
void WorldForceField(Tparameters *p, boolean simp, double *sol, double **g, void *w)
Evaluates the gradient of the potential energy of a configuration.
Definition: world.c:2722
Definition of variable names.
void CopyEquation(Tequation *eq_dst, Tequation *eq_orig)
Copy constructor.
Definition: equation.c:216
void GetBranchStep(unsigned int i, double *s, Tjoint **joint, Tbranch *b)
Retrives a particular step form a kinematic chain.
Definition: world.c:185
unsigned int WorldDOFTopology(unsigned int **t, Tworld *w)
Gets the topology of the degrees of freedom.
Definition: world.c:3171
#define PI2PI(a)
Forces an angle go be in [-pi,pi].
Definition: defines.h:205
void DeleteJacobian(TJacobian *j)
Destructor.
Definition: jacobian.c:255
double ErrorInSimpCSEquations(Tparameters *pr, double *p, TCuikSystem *cs)
Evaluates the norm of the error in a point for the simplified equations.
Definition: cuiksystem.c:5140
Tlink * JointFrom(Tjoint *j)
Gets a pointer to the first link involved in the joint.
Definition: joint.c:951
#define EQU
In a Tequation, the equation relational operator is equal.
Definition: equation.h:202
unsigned int GetWorldSimpVariableMask(Tparameters *p, boolean **sv, Tworld *w)
Identifies pose related variable that survied in the simplified system.
Definition: world.c:2396
void InitCD(unsigned int engine, boolean parallel, Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TworldCD *cd)
Initializes the collision detector.
Definition: cd.c:2135
void AddSimplifiedJacobianEquations(Tparameters *p, boolean *selectedVars, TCuikSystem *cs)
Adds a linear combination of the Jacobian to the system.
Definition: cuiksystem.c:2835
void DeleteCuikSystem(TCuikSystem *cs)
Destructor.
Definition: cuiksystem.c:5450
unsigned int GetCSSystemVars(boolean **sv, TCuikSystem *cs)
Identifies the system variables.
Definition: cuiksystem.c:2659
void DeleteMechanism(Tmechanism *m)
Destructor.
Definition: mechanism.c:513
Tenvironment e
Definition: world.h:235
#define EMPTY_BOX
One of the possible results of reducing a box.
Definition: box.h:25
void PrintWorldCS(Tparameters *p, char *fname, Tworld *w)
Prints the cuiksystems derived from a world.
Definition: world.c:3881
boolean CheckCollision(boolean all, THTransform *tl, TLinkConf *def, THTransform *tlPrev, TLinkConf *defPrev, TworldCD *cd)
Determines if there is a collision.
Definition: cd.c:2260
void CreateBranch(Tbranch *b)
Constructor.
Definition: world.c:148
#define COORD_EQ
One of the possible type of equations.
Definition: equation.h:155
unsigned int CoupledWith(Tjoint *j)
Returns the identifier of the joint coupled with the query joint.
Definition: joint.c:1285
unsigned int GetWorldTopology(Tparameters *p, unsigned int **t, Tworld *w)
Gets the topology of the variables.
Definition: world.c:3162
void InitPlot3d(char *name, boolean axes, int argc, char **arg, Tplot3d *p)
Constructor.
Definition: plot3d.c:41
#define WORLD_IN_DEFINITION
One of the stages of the Tworld structure definition.
Definition: world.h:61
boolean ** checkCollisionsLO
Definition: world.h:259
boolean AllRevolute(Tmechanism *m)
TRUE if all joints are revolute joints.
Definition: mechanism.c:196
void SetVariableInterval(Tinterval *i, Tvariable *v)
Sets the new range for the variable.
Definition: variable.c:68
#define TRUE
TRUE.
Definition: boolean.h:21
double WorldErrorInEquations(double *pt, Tworld *w)
Evalates the norm of the error in the kinematic equations for a given point.
Definition: world.c:3254
void InitBox(unsigned int dim, Tinterval *is, Tbox *b)
Initializes a box.
Definition: box.c:23
void NewSphere(double r, double *center, Tcolor *c, unsigned int gr, unsigned int bs, Tpolyhedron *p)
Constructor.
Definition: polyhedron.c:1020
void Close3dBlock(Tplot3d *p)
Ends a block of commands.
Definition: plot3d.c:146
unsigned int np
Definition: world.h:242
void DeleteWorldCollisionInfo(Tworld *w)
Removes the collision information stored in a Tworld.
Definition: world.c:1877
void InitEquation(Tequation *eq)
Constructor.
Definition: equation.c:86
unsigned int GetWorldLinkID(char *linkName, Tworld *w)
Gets the identifier of a link from its name.
Definition: world.c:1832
char * GetFilePath(Tfilename *fn)
Gets the file path.
Definition: filename.c:156
void Error(const char *s)
General error function.
Definition: error.c:80
void MoveWorldDOF(Tparameters *pr, Tplot3d *pt, double *dof, Tworld *w)
Moves a mechanisms to a configuration given by the degrees of freedom.
Definition: world.c:3680
void AddMatrixEquation2CS(Tparameters *p, TMequation *eq, TCuikSystem *cs)
Adds a matrix equation to the system.
Definition: cuiksystem.c:2511
boolean WorldCanCollide(Tworld *w)
Determines if any collision is potentially possible.
Definition: world.c:1063
#define NFUN
No trigonometric function for the variable.
Definition: variable_set.h:36
#define TOPOLOGY_R
One of the possible topologies.
Definition: defines.h:122
void InitMechanism(Tmechanism *m)
Constructor.
Definition: mechanism.c:16
All the necessary information to generate equations for mechanisms.
Definition: world.h:229
void CopyColor(Tcolor *c_dst, Tcolor *c_src)
Copy constructor.
Definition: color.c:57
#define CUIK_EXT
File extension for equation files.
Definition: filename.h:71
A color.
Definition: color.h:86
#define PRISMATIC_BAR
One of the possible types of legs.
Definition: world.h:171
void SimplifyMEquation(TMequation *me)
Tries to reduce the complexity of the matrix equation.
Definition: mequation.c:358
void EvaluateCSEquations(double *p, double *r, TCuikSystem *cs)
Evaluates the equation set on a point.
Definition: cuiksystem.c:5087
#define AXIS_H
The homogeneous dimension in R^3.
Definition: htransform.h:108
void PrintWorld(char *fname, int argc, char **arg, Tworld *w)
Prints the world.
Definition: world.c:3902
unsigned int * dof2range
Definition: world.h:331
CBLAS_INLINE void HTransformProduct(THTransform *t1, THTransform *t2, THTransform *t3)
Product of two homogeneous transforms.
Definition: htransform.c:410
#define ZERO
Floating point operations giving a value below this constant (in absolute value) are considered 0...
Definition: defines.h:37
void InitWorldKinCS(Tparameters *p, Tworld *w)
Initializes the kinematic sub-problem of a Tworld structure.
Definition: world.c:916
void CopyInterval(Tinterval *i_dst, Tinterval *i_org)
Copy constructor.
Definition: interval.c:59
unsigned int GetWorldSimpTopology(Tparameters *p, unsigned int **t, Tworld *w)
Gets the topology of the variables.
Definition: world.c:3153
unsigned int nStepsBranch(Tbranch *b)
Gets the number of steps in a kinematic chain.
Definition: world.c:180
Matrix equation.
Definition: mequation.h:42
void SetEquationValue(double v, Tequation *eq)
Changes the right-hand value of the equation.
Definition: equation.c:1089
Tlink * GetWorldLink(unsigned int linkID, Tworld *w)
Gets a link from its identifier.
Definition: world.c:1842
Tjoint * GetWorldJoint(unsigned int jointID, Tworld *w)
Gets a joint from its identifier.
Definition: world.c:1847
unsigned int Branches2Links(unsigned int from, unsigned int *links, unsigned int *jointTo, boolean *isLeaf, Tbranch *b, Tworld *w)
Determines kinematic branch from a link to all other links.
Definition: world.c:560
void CopyVoidPtr(void *a, void *b)
Copy constructor for void pointers.
Definition: vector.c:87
unsigned int GetSimpCSTopology(Tparameters *p, unsigned int **t, TCuikSystem *cs)
Topology of the variables in the simplified system.
Definition: cuiksystem.c:2719
void AddMonomial(Tmonomial *f, Tequation *eq)
Adds a new monomial to the equation.
Definition: equation.c:1419
unsigned int GetObstacleShapeStatus(unsigned int i, Tenvironment *e)
Gets the status (NORMAL, HIDDEN, DECOR) of an obstacle given its identifier.
Definition: environment.c:96
boolean AnyCollision(Tworld *w)
Determines if we want to avoid any collision.
Definition: world.c:2294
Definition of the Tworld type and the associated functions.
unsigned int no
Definition: world.h:241
unsigned int GetBoxNIntervals(Tbox *b)
Box dimensionality.
Definition: box.c:1016
void NewSpring(double length, double start, double rad, unsigned int lps, Tcolor *c, Tpolyhedron *p)
Defines a spring.
Definition: polyhedron.c:1145
void CopyCuikSystem(TCuikSystem *cs_dst, TCuikSystem *cs_src)
Copy constructor.
Definition: cuiksystem.c:2216
void NewRevUpJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
Constructor.
Definition: joint.c:668
void WorldEvaluateSimpEquations(Tparameters *p, double *pt, double *r, Tworld *w)
Evaluates the simplified kinematic equations.
Definition: world.c:3238
void PrintEnvironment(FILE *f, char *path, Tenvironment *e)
Stores the environment information into a file.
Definition: environment.c:123
void WorldEvaluateEquations(double *pt, double *r, Tworld *w)
Evaluates the kinematic equations.
Definition: world.c:3206
void Add2Color(double s, Tcolor *c)
Adds to a color.
Definition: color.c:69
A polyhedron.
Definition: polyhedron.h:134
unsigned int * joint2dof
Definition: world.h:329
unsigned int GetBoxLevel(Tbox *b)
Returns the box level.
Definition: box.c:700
void GenerateJointEquationsInBranch(Tparameters *p, double s, TCuikSystem *cs, Tequation *eq, Tjoint *j)
Generate the constraints of a joint in a sequence.
Definition: joint.c:1812
void GetJointName(char **name, Tjoint *j)
Returns a string identifying the joint.
Definition: joint.c:966
Error and warning functions.
void EvaluateJacobianXVectors(double *p, unsigned int n, unsigned int ng, unsigned int *g, double *v, unsigned int *nr, unsigned int *nc, double ***m, TJacobian *j)
Evaluates the Jacobian multiplied by some given vectors.
Definition: jacobian.c:212
void DeleteFileName(Tfilename *fn)
Destructor.
Definition: filename.c:205
unsigned int GetJointID(Tjoint *j)
Gets the joint identifier.
Definition: joint.c:941
A 3D plot.
Definition: plot3d.h:54
boolean MoveAndCheckCD(Tparameters *p, boolean all, unsigned int tID, double *sol, double *solPrev, Tworld *w)
Checks a point for collision.
Definition: world.c:1102
unsigned int MaxReduction(Tparameters *p, unsigned int varMask, double *reduction, Tbox *b, TCuikSystem *cs)
Reduces a box as much as possible.
Definition: cuiksystem.c:2955
double ErrorInCSEquations(double *p, TCuikSystem *cs)
Evalates the norm of the error in a point.
Definition: cuiksystem.c:5120
unsigned int GetBranchDestination(Tbranch *b)
Returns the identifier of the link where the kinematic chain ends.
Definition: world.c:214
Tjoint * joint
Definition: world.h:186
boolean WorldInequalitiesHold(double *pt, Tworld *w)
Check if the inequalities hold.
Definition: world.c:3214
double HTransformGetElement(unsigned int i, unsigned int j, THTransform *t)
Gets an element in a homogeneous transform.
Definition: htransform.c:329
void AddStepToBranch(double s, Tjoint *joint, Tbranch *b)
Adds a step to a kinematic chain.
Definition: world.c:168
void MergeBranches(Tbranch *b1, Tbranch *b2, Tbranch *b)
Merge two kinematic branches.
Definition: world.c:229
#define INIT_NUM_JOINTS
Initial room for joints in a mechanism.
Definition: mechanism.h:36
void NewFreeJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
Constructor.
Definition: joint.c:93
void DeleteTransSeq(TTransSeq *ts)
Destructor.
Definition: trans_seq.c:1288
void Delay3dObject(double t, Tplot3d *p)
Introduces a delay in the generation of the geometry.
Definition: plot3d.c:211
void SetEquationCmp(unsigned int cmp, Tequation *eq)
Changes the relational operator (LEQ, GEQ, EQU) of the equation.
Definition: equation.c:1081
void WorldDeleteDOFInfo(Tworld *w)
Deletes the information collected at WorldInitDOFInfo.
Definition: world.c:1986
boolean IsWorldPolynomial(Tworld *w)
Checks if the system of equations is polynomial.
Definition: world.c:2089
void ResetMonomial(Tmonomial *f)
Reset the monomial information.
Definition: monomial.c:24
unsigned int nj
Definition: world.h:240
double LowerLimit(Tinterval *i)
Gets the lower limit.
Definition: interval.c:79
void HTransformInverse(THTransform *t, THTransform *ti)
Inverse of a homogeneous transform.
Definition: htransform.c:503
void GenerateForceEquilibriumEquations(Tparameters *p, Tworld *w)
Adds force equilibrium equations.
Definition: world.c:2510
void GetWorldVarNames(char **vn, Tworld *w)
Return the variable names.
Definition: world.c:2415
unsigned int GetWorldNDOF(Tworld *w)
Gets the number of degrees of freedom in the world.
Definition: world.c:1997
#define LEG
One of the possible types of legs.
Definition: world.h:115
void GetTransformFromBranch(THTransform *tj, THTransform *t, Tbranch *b)
Defines the transform taking to the end of the branch.
Definition: world.c:272
void GetMechanismDOFsFromTransforms(Tparameters *p, THTransform *tl, TLinkConf *def, double *dof, Tmechanism *m)
Extract the joint DOF values form the poses of all links.
Definition: mechanism.c:286
void WorldPrintAtoms(Tparameters *pr, FILE *f, double *pt, Tworld *w)
Generates a file with the atom centers in global coordiantes.
Definition: world.c:3496
boolean EndOfList(Titerator *i)
Checks if an iterator is pointing at the end of the list.
Definition: list.c:445
unsigned int GetBranchOrigin(Tbranch *b)
Returns the identifier of the link where the kinematic chain starts.
Definition: world.c:199
An equation.
Definition: equation.h:237
void DeleteVariable(Tvariable *v)
Destructor.
Definition: variable.c:93
unsigned int GetWorldNConvexBodiesInLinks(Tworld *w)
Gets the number of convex polyhedrons that define the mechanism included in the world.
Definition: world.c:1867
unsigned int WorldForceVarsIndices(Tparameters *p, unsigned int **iv, Tworld *w)
Creates an array with the indices of the force variables.
Definition: world.c:2649
Definitions of constants and macros used in several parts of the cuik library.
A generic list.
Definition: list.h:46
TworldCD * wcd
Definition: world.h:320
void MechanismStoreRigidAtoms(FILE *f, THTransform *tl, Tmechanism *m)
Auxiliary function for WorldStoreRigidGroups.
Definition: mechanism.c:354
void InitVector(unsigned int ele_size, void(*Copy)(void *, void *), void(*Delete)(void *), unsigned int max_ele, Tvector *vector)
Constructor.
Definition: vector.c:100
void GetWorldRangeDOF(unsigned int ndof, Tinterval *r, Tworld *w)
Gets the range for a given degree of freedom.
Definition: world.c:2002
void NewSphSphLowJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
Constructor.
Definition: joint.c:601
void InitIterator(Titerator *i, Tlist *list)
Constructor.
Definition: list.c:284
unsigned int nsvars
Definition: world.h:273
unsigned int GetWorldSystemVars(boolean **sv, Tworld *w)
Gets the system vars of the kinematic cuiksystem.
Definition: world.c:2383
void InitEnvironment(Tenvironment *e)
Constructor.
Definition: environment.c:13
void GetCSJacobian(TJacobian *J, TCuikSystem *cs)
Defines the Jacobian of a CuikSystem.
Definition: cuiksystem.c:2714
Definition of the Tlist type and the associated functions.
unsigned int GetWorldMobility(Tworld *w)
Returns the number of degrees of freedom of the mechanism in the world.
Definition: world.c:1822
unsigned int GetJointRangeTopology(unsigned int nr, Tjoint *j)
Returns the topology of one of the ranges of the joint.
Definition: joint.c:1214
void CheckLinkObstacleCollision(unsigned int a, unsigned int b, Tworld *w)
Activates the possible collision between a particular link and an object in the environment.
Definition: world.c:2191
double WorldErrorInSimpEquations(Tparameters *p, double *pt, Tworld *w)
Evalates the norm of the error in the kinematic equations for a given point.
Definition: world.c:3262
void GenerateKinTree(Tvector *bOut, Tworld *w)
Builds a kinematic tree departing from the ground link.
Definition: world.c:632
void PrintWorldCollisionInfo(FILE *f, char *fname, Tworld *w)
Prints information collected durint last collision check.
Definition: world.c:3674
void NewVariable(unsigned int type, char *name, Tvariable *v)
Constructor.
Definition: variable.c:20
void EvaluateEqualityEquations(boolean systemOnly, double *v, double *r, Tequations *eqs)
Evaluates all equality equations in the set.
Definition: equations.c:2579
void InitEquations(Tequations *eqs)
Constructor.
Definition: equations.c:720
void PrintJointAxes(Tparameters *p, FILE *f, TCuikSystem *cs, double *sol, double *r, Tjoint *j)
Prints the joint axis in world coordinates.
Definition: joint.c:3014
unsigned int * dof2joint
Definition: world.h:330
unsigned int nvars
Definition: world.h:272
void DeleteEnvironment(Tenvironment *e)
Destructor.
Definition: environment.c:169
#define C_FCL
One of the possible collison detection engines.
Definition: parameters.h:105
#define CT_CD_ENGINE
Collision detection engine.
Definition: parameters.h:542
void DeleteInterval(Tinterval *i)
Destructor.
Definition: interval.c:1021
void InitWorld(Tworld *w)
Constructor.
Definition: world.c:1358
void NoCheckAllCollisions(unsigned int fl, unsigned int fo, Tworld *w)
Desactivates all the possible collision between links and links and obstacles.
Definition: world.c:2119
void NoCheckSelfCollisions(unsigned int fl, Tworld *w)
Desactivates all the possible collision between links.
Definition: world.c:2153
void InitCuikSystem(TCuikSystem *cs)
Constructor.
Definition: cuiksystem.c:2167
boolean MoveAndCheckCDFromTransforms(boolean all, unsigned int tID, THTransform *tl, TLinkConf *def, THTransform *tlPrev, TLinkConf *defPrev, Tworld *w)
Checks a point for collision.
Definition: world.c:1077
boolean CuikNewton(Tparameters *p, double *sol, Tbox *b_sol, TCuikSystem *cs)
Applies Newton-Rhapson to a set of equations.
Definition: cuiksystem.c:3947
unsigned int GetJointType(Tjoint *j)
Gets the joint type.
Definition: joint.c:931
void AddEquation2CS(Tparameters *p, Tequation *eq, TCuikSystem *cs)
Adds an equation to the system.
Definition: cuiksystem.c:2502
unsigned int WorldDOF2Sol(Tparameters *p, double *dof, double **sol, Tbox *b, Tworld *w)
Transforms from degrees of freedom to cuik variables.
Definition: world.c:3708
unsigned int nb
Definition: world.h:239
A scaled product of powers of variables.
Definition: monomial.h:32
void Warning(const char *s)
General warning function.
Definition: error.c:116
A table of parameters.
void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn)
Constructor.
Definition: filename.c:22
unsigned int CuikNewtonSimp(Tparameters *p, double *x, TCuikSystem *cs)
CuikNewton on the simplified system.
Definition: cuiksystem.c:3618
void NewCylinder(double r, double *p1, double *p2, Tcolor *c, unsigned int gr, unsigned int bs, Tpolyhedron *p)
Constructor.
Definition: polyhedron.c:1044
boolean * openLink
Definition: world.h:309
double UpperLimit(Tinterval *i)
Gets the uppser limit.
Definition: interval.c:87
void GetWorldSimpJacobian(Tparameters *p, TJacobian *J, Tworld *w)
Gets the simplified kinematic Jacobian.
Definition: world.c:3145
Tequations refEqs
Definition: world.h:291
boolean InequalitiesHoldOnPoint(double *p, TCuikSystem *cs)
Tests if all inqualities hold for a given point.
Definition: cuiksystem.c:5248
void PrintCollisionInfo(THTransform *tl, Tmechanism *m, Tenvironment *e, TworldCD *cd)
Prints some information collected during last collision check.
Definition: cd.c:2374
A sequence of transforms.
Definition: trans_seq.h:319
unsigned int RegenerateWorldOriginalSystemPoint(Tparameters *p, double *s, double **o, Tworld *w)
Reconstruct a point in original kinematic system from a simplified point.
Definition: world.c:3334
char * GetCSSystemVariableName(unsigned int id, TCuikSystem *cs)
Gets a system variable name.
Definition: cuiksystem.c:2617
Tjoint * GetMechanismJoint(unsigned int i, Tmechanism *m)
Gets a joint given its identifier.
Definition: mechanism.c:185
unsigned int GetCSNumSystemVariables(TCuikSystem *cs)
Gets the number of system variables already in the cuiksystem.
Definition: cuiksystem.c:2570
A generic vector.
Definition: vector.h:227
void GetJointTransSeq(Tparameters *p, TCuikSystem *cs, TTransSeq *ts, Tjoint *j)
Build the sequence of transforms passing through the joint.
Definition: joint.c:2829
char * GetFileFullName(Tfilename *fn)
Gets the file full name (paht+name+extension).
Definition: filename.c:151
#define PRISMATIC_LEG
One of the possible types of legs.
Definition: world.h:126
A kinematic branch.
Definition: world.h:213
TJacobian refEqsJ
Definition: world.h:301
boolean IsMechanismAllSpheres(Tmechanism *m)
TRUE if the mechanism is composed by spheres only.
Definition: mechanism.c:41
void GetWorldJacobian(TJacobian *J, Tworld *w)
Gets the kinematic Jacobian.
Definition: world.c:3137
#define CT_REPRESENTATION
Representation.
Definition: parameters.h:215
double WorldErrorInSimpInequalities(Tparameters *p, double *pt, Tworld *w)
Determines the maximum error in the inequalites for the simplified system.
Definition: world.c:3230
void NoCheckConnectedCollisions(unsigned int fl, Tworld *w)
Desactivates the collision detection between connected links.
Definition: world.c:2225
void DeleteCD(TworldCD *cd)
Collision information destructor.
Definition: cd.c:2403
#define REV_JOINT
One of the possible type of joints.
Definition: joint.h:59
void GetCSVariables(Tvariables *vs, TCuikSystem *cs)
Gets the cuiksystem variables.
Definition: cuiksystem.c:2550
void EvaluateCSJacobian(double *p, double ***J, TCuikSystem *cs)
Evaluates the Jacobian of the system in a given point.
Definition: cuiksystem.c:5108
void StoreCollisionInfo(FILE *f, char *fname, unsigned int objectID, Tmechanism *m, TworldCD *cd)
Stores the information collected during last collision check into a file.
Definition: cd.c:2346
unsigned int WorldForceVars(Tparameters *p, boolean **fv, Tworld *w)
Creates a boolean array identifying force variables.
Definition: world.c:2585
boolean ZeroInterval(Tinterval *i)
Checks if a given interval is zero.
Definition: interval.c:340
unsigned int stage
Definition: world.h:230
void PrintCollisions(FILE *f, Tworld *w)
Stores the collision information into a file.
Definition: world.c:2306
#define SPHERE
One of the possible type of polyhedrons.
Definition: polyhedron.h:70
A box.
Definition: box.h:83
double WorldErrorFromDOFs(Tparameters *p, double *dof, Tworld *w)
Error in equations from DOFs.
Definition: world.c:3270
double GetEquationValue(Tequation *eq)
Gets the right-hand value of the equation.
Definition: equation.c:1212
Data associated with each variable in the problem.
Definition: variable.h:84
void PrintMechanism(FILE *f, char *path, char *prefix, Tmechanism *m)
Stores the mechanisms information into a file.
Definition: mechanism.c:411
unsigned int GetWorldNumVariables(Tworld *w)
Number of variables in the kinematic subsystem.
Definition: world.c:3289
unsigned int WorldSimpKinematicVars(Tparameters *p, boolean **kv, Tworld *w)
Creates a boolean array to identify kinematic variables.
Definition: world.c:2616
#define WORLD_EXT
File extension for problem files.
Definition: filename.h:162
A cuiksystem, i.e., a set of variables and equations defining a position analysis problem...
Definition: cuiksystem.h:181
void WorldEvaluateSubSetSimpEquations(Tparameters *p, boolean *se, double *pt, double *r, Tworld *w)
Evaluates a subset of the simplified kinematic equations.
Definition: world.c:3246
void WorldStoreRigidGroups(Tparameters *pr, FILE *f, double *pt, Tworld *w)
Generates a file with the atoms grouped in rigid clusters.
Definition: world.c:3511
void * GetCurrent(Titerator *i)
Gets the element pointed by the iterator.
Definition: list.c:299
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
void InitBoxFromPoint(unsigned int dim, double *p, Tbox *b)
Initializes a box from a point.
Definition: box.c:43
void CopyBranch(Tbranch *b_dst, Tbranch *b_src)
Copy constructor.
Definition: world.c:153
boolean IsRevoluteBinaryLink(unsigned int nl, double ***p, Tworld *w)
Identifies links articulated with only two revolute joints.
Definition: world.c:879
unsigned int WorldSimpCuikNewton(Tparameters *p, double *pt, Tworld *w)
Tries to reach the kinematic manifold.
Definition: world.c:3193
double WorldPotentialEnergy(Tparameters *p, boolean simp, double *sol, void *w)
Evaluates the potential energy of a configuration.
Definition: world.c:2680
void PrintWorldAxes(Tparameters *pr, FILE *f, Tbox *b, Tworld *w)
Prints the axes of the mechanism.
Definition: world.c:3637
void AddVariable2Monomial(unsigned int fn, unsigned int varid, unsigned int p, Tmonomial *f)
Adds a power variable to the monomial.
Definition: monomial.c:171
unsigned int JointFromID(Tjoint *j)
Gets the identifier of the first link involved in the joint.
Definition: joint.c:946
double IntervalCenter(Tinterval *i)
Gets the interval center.
Definition: interval.c:129
void CheckSelfCollisions(unsigned int fl, Tworld *w)
Activates all the possible collision between links.
Definition: world.c:2138
Definition of the TSHTransform type and the associated functions.
unsigned int AddLink2Mechanism(Tlink *l, Tmechanism *m)
Adds a link to a mechanism.
Definition: mechanism.c:95
void JointForceEquation(Tparameters *pr, unsigned int linkID, TCuikSystem *cs, Tequation *eq, Tjoint *j)
Add the force terms derived from a given joint.
Definition: joint.c:1925
unsigned int GetCDEngine(TworldCD *cd)
Determines the collision engine.
Definition: cd.c:2255
void PrintCuikSystem(Tparameters *p, FILE *f, TCuikSystem *cs)
Prints a cuiksystem.
Definition: cuiksystem.c:5359
char * GetWorldSystemVarName(unsigned int nv, Tworld *w)
Returns the name of a given system variable.
Definition: world.c:2423
unsigned int GetWorldNObstacles(Tworld *w)
Gets the number of obstacles in the environment included in the world.
Definition: world.c:1862
unsigned int GetMechanismLinkID(char *ln, Tmechanism *m)
Gets a link identifier given its name.
Definition: mechanism.c:160
#define CT_PRETENSION
Pre-tension.
Definition: parameters.h:579
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:2365
char * GetObstacleName(unsigned int i, Tenvironment *e)
Gets the name of an obstacle given its identifier.
Definition: environment.c:74
void GetCSVariableNames(char **varNames, TCuikSystem *cs)
Gets points to the variable names.
Definition: cuiksystem.c:2555
void DeleteBox(void *b)
Destructor.
Definition: box.c:1283
unsigned int GetCSTopology(Tparameters *p, unsigned int **t, TCuikSystem *cs)
Topology of the variables in the system.
Definition: cuiksystem.c:2728
boolean SimpInequalitiesHoldOnPoint(Tparameters *pr, double *p, TCuikSystem *cs)
Tests if all simplified inqualities hold for a given point.
Definition: cuiksystem.c:5268
void NoCheckObstacleCollision(unsigned int fl, unsigned int b, Tworld *w)
Deactivates the possible collision between a particular obstacle and all the links.
Definition: world.c:2217
void GenerateWorldSingularityEquations(Tparameters *p, char *ln, TCuikSystem *cs, Tworld *w)
Adds variables/equations to detect singularities.
Definition: world.c:3039
void DeleteVariables(Tvariables *vs)
Destructor.
Definition: variables.c:354
void WorldFixTensegrityAddon(Tparameters *p, unsigned int linkID, double **point, unsigned int *n, Tworld *w)
Fixes a tensegrity addon.
Definition: world.c:2819
The Jacobian of a set of equations.
Definition: jacobian.h:23
unsigned int ndof
Definition: world.h:322
void NewRevLowJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
Constructor.
Definition: joint.c:627
Tbranch * branch2Link
Definition: world.h:283
void DeleteVoidPtr(void *a)
Destructor for void pointers.
Definition: vector.c:92
void HTransformDelete(THTransform *t)
Destructor.
Definition: htransform.c:887
void DeleteWorld(Tworld *w)
Destructor.
Definition: world.c:3952
boolean WorldContinuousCD(Tworld *w)
Determines the type of collision library used.
Definition: world.c:1068
unsigned int GetWorldVarTopology(unsigned int vID, Tworld *w)
Get the topology of a given variable.
Definition: world.c:2391
void FixLinks(Tparameters *p, Tworld *w)
Generate equations to fix some links.
Definition: world.c:2783
#define MEM_EXPAND(_var, _n, _type)
Expands a previously allocated memory space.
Definition: defines.h:404
#define CABLE
One of the possible types of legs.
Definition: world.h:150
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
unsigned int RegenerateWorldSolutionPoint(Tparameters *pr, double *p, double **v, Tworld *w)
Computes the missing values in a kinematic solution.
Definition: world.c:3468
unsigned int GetWorldNLinks(Tworld *w)
Gets the number of links in the mechanism included in the world.
Definition: world.c:1852
void DeletePolyhedron(Tpolyhedron *p)
Destructor.
Definition: polyhedron.c:1692
void EvaluateWorldJacobian(double *pt, double ***J, Tworld *w)
Evaluates the kinematic Jacobian.
Definition: world.c:3198
void DeleteColor(Tcolor *c)
Destructor.
Definition: color.c:143
void NewInterval(double lower, double upper, Tinterval *i)
Constructor.
Definition: interval.c:47
unsigned int AddJoint2World(Tjoint *j, Tworld *w)
Adds a joint to the mechanism in the world.
Definition: world.c:1436
Tlink * GetMechanismLink(unsigned int i, Tmechanism *m)
Gets a link given its identifier.
Definition: mechanism.c:149
void EvaluateSimpCSEquations(Tparameters *pr, double *p, double *r, TCuikSystem *cs)
Evaluates the simplified equation set on a point.
Definition: cuiksystem.c:5092
void FixZToZero(Tparameters *p, Tworld *w)
Generate equations to fix the Z components of the links.
Definition: world.c:2867
boolean IsMechanismInWorldAllSpheres(Tworld *w)
TRUE if the mechanisms in the world is based on spheres.
Definition: world.c:1827
#define INF
Infinite.
Definition: defines.h:70
unsigned int * sortedLinks
Definition: world.h:279
unsigned int endEffector
Definition: world.h:245
void AddCt2Monomial(double k, Tmonomial *f)
Scales a monomial.
Definition: monomial.c:158
unsigned int GetSolutionPointFromLinkTransforms(Tparameters *p, THTransform *tl, TLinkConf *def, double **sol, Tworld *w)
Determines the mechanisms configuration from the pose of all links.
Definition: world.c:1261
#define COUPLE_REST
One of the possible variables to couple in tensegrities.
Definition: world.h:85
List iterator.
Definition: list.h:61
void WorldCoupleTensegrityVariable(Tparameters *p, unsigned int t, unsigned int lID1, unsigned int lID2, double scale, THTransform *r, Tworld *w)
Couple variables from different elements of a tensegrity.
Definition: world.c:2889
void MoveWorld(Tparameters *pr, Tplot3d *pt, Tbox *b, Tworld *w)
Moves the mechanisms defined in a world information to a given configuration.
Definition: world.c:3581
void GetBoxCenter(boolean *used, double *c, Tbox *b)
Returns the box center along the selected dimensions.
Definition: box.c:721
double EvaluateCSCost(Tparameters *p, boolean simp, double *s, void *cs)
Evalutes the equation to minimize in a given point.
Definition: cuiksystem.c:5162
#define COUPLE_FORCE
One of the possible variables to couple in tensegrities.
Definition: world.h:93
unsigned int GetWorldNConvexBodies(Tworld *w)
Gets the number of convex polyhedrons that define the mechanism and the environment included in the w...
Definition: world.c:1872
unsigned int * dof2link
Definition: world.h:325
double GetEnvironmentMaxCoordinate(Tenvironment *e)
Returns the sum of the maximum coordinate value for all the convex polyhedrons in the environment...
Definition: environment.c:118
void DeleteEquations(Tequations *eqs)
Destructor.
Definition: equations.c:2862
unsigned int GetCSVariableID(char *name, TCuikSystem *cs)
Gets the numerical identifier of a variable given its name.
Definition: cuiksystem.c:2607
Definition of basic randomization functions.
void PrintBox(FILE *f, Tbox *b)
Prints a box.
Definition: box.c:1142
unsigned int GetCSNumNonDummyVariables(TCuikSystem *cs)
Gets the number of non-dummy variables already in the cuiksystem.
Definition: cuiksystem.c:2579
void GenerateWorldEquations(Tparameters *p, Tworld *w)
Generates all the cuiksystems derived from the world information.
Definition: world.c:2431
void GenerateJointRangeSingularityEquations(Tparameters *p, TCuikSystem *cs, Tjoint *j)
Modifies the cuik system to detect end range singularities.
Definition: joint.c:1333
unsigned int GetCSVarTopology(unsigned int vID, TCuikSystem *cs)
Determines the topology of a given variable.
Definition: cuiksystem.c:2672
unsigned int GetObstacleID(char *name, Tenvironment *e)
Gets the idetifier of an obstacles given its name.
Definition: environment.c:48
char * GetFileExtension(Tfilename *fn)
Gets the file extension.
Definition: filename.c:171
void AnimateWorld(Tparameters *pr, char *pname, double axesLength, double frameDelay, Tlist *p, Tworld *w)
Produces an animation along a path.
Definition: world.c:3837
unsigned int GetWorldNumSystemVariables(Tworld *w)
Number of system variables in the kinematic subsystem.
Definition: world.c:3297
void GetLinkTransformsFromSolutionPoint(Tparameters *p, boolean simp, double *sol, THTransform **tl, TLinkConf **def, Tworld *w)
Define transforms for the links from the a solution point.
Definition: world.c:1215
Defines a interval.
Definition: interval.h:33
void GenerateWorldTWSEquations(Tparameters *p, char *ln, TCuikSystem *cs, Tworld *w)
Adds variables/equations to detect translational workspace boundaries.
Definition: world.c:3104
unsigned int RegenerateOriginalPoint(Tparameters *p, double *s, double **o, TCuikSystem *cs)
Generates an original point from a simplified point.
Definition: cuiksystem.c:4978
void HTransformIdentity(THTransform *t)
Constructor.
Definition: htransform.c:69
void MoveMechanismFromTransforms(Tparameters *pr, Tplot3d *pt, THTransform *tl, TLinkConf *def, Tmechanism *m)
Displaces a mechanism in a 3d scene.
Definition: mechanism.c:391
double sign
Definition: world.h:187
void CheckObstacleCollision(unsigned int fl, unsigned int b, Tworld *w)
Activates the possible collision between a particular obstacle and all the links. ...
Definition: world.c:2209
void InitMonomial(Tmonomial *f)
Constructor.
Definition: monomial.c:17
double IntervalSize(Tinterval *i)
Gets the uppser limit.
Definition: interval.c:96
void WorldAtomJacobian(Tparameters *pr, double *sol, unsigned int *nr, unsigned int *nc, double ***J, Tworld *w)
Jacobian of the atom position w.r.t. the variables.
Definition: world.c:3526
void AddTransSeq2MEquation(int s, TTransSeq *ts, TMequation *me)
Concatenates a transform sequence to the matrix equation.
Definition: mequation.c:250
void GetSimpCSJacobian(Tparameters *p, TJacobian *J, TCuikSystem *cs)
Defines the Jacobian of a simplified CuikSystem.
Definition: cuiksystem.c:2734
unsigned int GetCSNumVariables(TCuikSystem *cs)
Gets the number of variables already in the cuiksystem.
Definition: cuiksystem.c:2565
unsigned int nwcd
Definition: world.h:316
double ErrorInSimpInequalitiesOnPoint(Tparameters *pr, double *p, TCuikSystem *cs)
Computes the maximum error in all the simplified inqualities for a given point.
Definition: cuiksystem.c:5291
unsigned int RegenerateWorldOriginalPoint(Tparameters *p, double *s, double **o, Tworld *w)
Reconstruct a point in original kinematic system from a simplified point.
Definition: world.c:3305
#define SPRING
One of the possible types of legs.
Definition: world.h:159
void DeleteJoint(Tjoint *j)
Destructor.
Definition: joint.c:3190
void GenerateJointRangeEquations(Tparameters *p, TCuikSystem *cs, Tjoint *j)
Generate the constraints related with the joint limits.
Definition: joint.c:1521
#define COUPLE_ORIENTATION
One of the possible variables to couple in tensegrities.
Definition: world.h:104
void GetLinkTransformsFromDOF(double *dof, THTransform **tl, TLinkConf **def, Tworld *w)
Define transforms for the links from the DOF.
Definition: world.c:1145
boolean Advance(Titerator *i)
Moves an iterator to the next position of its associated list.
Definition: list.c:373
unsigned int GetLinkID(char *name, Tmechanism *m)
Gets the identifier of a link given its name.
Definition: mechanism.c:51
#define LINK_TRANS(vname, linkName, cn)
Translation part of the homogeneous transform defining the position of a link in global coordinates...
Definition: varnames.h:168
unsigned int * jointTo
Definition: world.h:281
unsigned int AddJoint2Mechanism(Tjoint *j, Tmechanism *m)
Adds a joint to a mechanism.
Definition: mechanism.c:113
void MechanismPrintAtoms(FILE *f, THTransform *tl, Tmechanism *m)
Prints the center of the atoms in a mechanism.
Definition: mechanism.c:339
double maxCoord
Definition: world.h:247
unsigned int GenerateSimplifiedPoint(Tparameters *p, double *o, double **s, TCuikSystem *cs)
Generates a simplified point.
Definition: cuiksystem.c:4992
void GenerateJointEquations(Tparameters *p, double maxCoord, TCuikSystem *cs, Tjoint *j)
Generate the constraints related with the joint.
Definition: joint.c:1967
void NewSphSphUpJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
Constructor.
Definition: joint.c:612
void DeleteMEquation(TMequation *me)
Destructor.
Definition: mequation.c:492
void GetJointRangeN(unsigned int nr, double mt, Tinterval *r, Tjoint *j)
Returns one of the ranges of the joint.
Definition: joint.c:1127
void PlotEnvironment(Tplot3d *pt, Tenvironment *e)
Displays the obstacles in the environment in a 3D geometry.
Definition: environment.c:146
void Start3dBlock(Tplot3d *p)
Starts a block of commands.
Definition: plot3d.c:141
void DeleteMonomial(Tmonomial *f)
Destructor.
Definition: monomial.c:289
unsigned int GetPolyhedronType(Tpolyhedron *p)
Retrives the type of a polyhedron.
Definition: polyhedron.c:1363
void ClosePlot3d(boolean quit, double average_x, double average_y, double average_z, Tplot3d *p)
Destructor.
Definition: plot3d.c:473
void NoCheckLinkLinkCollision(unsigned int a, unsigned int b, Tworld *w)
Desactivates the possible collision between a particular pair of links.
Definition: world.c:2179
void GenerateSimpInitialBox(Tparameters *p, Tbox *box, TCuikSystem *cs)
Gives the search space in the form of a box for the simplified system.
Definition: cuiksystem.c:4854
unsigned int NewVectorElement(void *e, Tvector *vector)
Adds an element to the vector.
Definition: vector.c:216
#define TOPOLOGY_S
One of the possible topologies.
Definition: defines.h:139
unsigned int GetWorldNJoints(Tworld *w)
Gets the number of joints in the mechanism included in the world.
Definition: world.c:1857
#define DECOR_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:46
unsigned int GetEnvironmentNObstacles(Tenvironment *e)
Gets the number of obstacles in the environment.
Definition: environment.c:43
void AddEquationNoSimp(Tequation *equation, Tequations *eqs)
Adds an equation to the set.
Definition: equations.c:1750
unsigned int JointToID(Tjoint *j)
Gets the identifier of the second link involved in the joint.
Definition: joint.c:956
Link configuration.
Definition: link.h:276
#define WORLD_DEFINED
One of the stages of the Tworld structure definition.
Definition: world.h:67
void WorldSample2DOF(Tparameters *p, double *sample, double *dof, Tworld *w)
Transforms a sample degrees of freedom.
Definition: world.c:3800
void CheckAllCollisions(unsigned int fl, unsigned int fo, Tworld *w)
Activates all the possible collision between links and links and obstacles.
Definition: world.c:2097
#define HIDDEN_SHAPE
One of the possible type of shapes.
Definition: polyhedron.h:37
void InitWorldCS(Tworld *w)
Initializes the cuiksystems in a Tworld structure.
Definition: world.c:998
void DeleteWorldCD(Tworld *w)
Deletes the collision information stored in the world.
Definition: world.c:1127
#define FRAME_RATE
When generating 3d animations, frame rate.
Definition: world.h:45