world.h
Go to the documentation of this file.
1 #ifndef WORLDH
2 #define WORLDH
3 
4 #include "vector.h"
5 #include "environment.h"
6 #include "mechanism.h"
7 #include "cuiksystem.h"
8 #include "equations.h"
9 #include "plot3d.h"
10 #include "box.h"
11 #include "csmapping.h"
12 #include "parameters.h"
13 #include "filename.h"
14 #include "statistics.h"
15 #include "list.h"
16 #include "jacobian.h"
17 #include "cd.h"
18 
19 #include <stdlib.h>
20 
21 
32 /*****************************************************************************/
33 
39 #define INITIAL_FRAME_DELAY 5.0
40 
45 #define FRAME_RATE (1.0/10)
46 
53 #define FINAL_FRAME_DELAY 500.0
54 /*****************************************************************************/
55 /*****************************************************************************/
56 
61 #define WORLD_IN_DEFINITION 0
62 
67 #define WORLD_DEFINED 1
68 
69 /*****************************************************************************/
70 
77 #define COUPLE_LENGTH 0
78 
85 #define COUPLE_REST 1
86 
93 #define COUPLE_FORCE 2
94 
104 #define COUPLE_ORIENTATION 3
105 
106 /*****************************************************************************/
107 
115 #define LEG 0
116 
126 #define PRISMATIC_LEG 1
127 
134 #define STRUT 2
135 
142 #define BAR 3
143 
150 #define CABLE 4
151 
159 #define SPRING 5
160 
171 #define PRISMATIC_BAR 6
172 
173 /*****************************************************************************/
174 
175 
185 typedef struct {
187  double sign;
191 } TBranchStep;
192 
213 typedef struct {
215 } Tbranch;
216 
229 typedef struct {
230  unsigned int stage;
238  unsigned int nl;
239  unsigned int nb;
240  unsigned int nj;
241  unsigned int no;
242  unsigned int np;
245  unsigned int endEffector;
247  double maxCoord;
250  boolean **checkCollisionsLL;
259  boolean **checkCollisionsLO;
272  unsigned int nvars;
273  unsigned int nsvars;
274  boolean *systemVars;
279  unsigned int *sortedLinks;
281  unsigned int *jointTo;
309  boolean *openLink;
316  unsigned int nwcd;
322  unsigned int ndof;
324  unsigned int *link2dof;
325  unsigned int *dof2link;
326  unsigned int *dof2param;
329  unsigned int *joint2dof;
330  unsigned int *dof2joint;
331  unsigned int *dof2range;
333 } Tworld;
334 
335 
350 void InitWorld(Tworld *w);
351 
374 boolean InitWorldFromFile(Tparameters *p,boolean error,char *fn,
375  Tworld *w);
376 
415 boolean InitTensegrityFromFile(Tparameters *p,char *fn,
416  Tworld *w);
417 
433 unsigned int AddLink2World(Tlink *l,boolean object,Tworld *w);
434 
435 
446 unsigned int AddJoint2World(Tjoint *j,Tworld *w);
447 
489 void AddLeg2World(char *name,boolean planar,unsigned int type,
490  unsigned int lID1,unsigned int lID2,
491  double **points,
492  Tinterval *length,double stiffness,
493  Tinterval *rest,Tinterval *force,
494  double radius,unsigned int gr,
495  Tcolor *color,unsigned int bs,
496  Tworld *w);
497 
507 void AddObstacle2World(char *name,Tpolyhedron *o,Tworld *w);
508 
522 unsigned int GetWorldMobility(Tworld *w);
523 
534 
546 unsigned int GetWorldLinkID(char *linkName,Tworld *w);
547 
559 unsigned int GetWorldObstacleID(char *obsName,Tworld *w);
560 
572 Tlink *GetWorldLink(unsigned int linkID,Tworld *w);
573 
574 
586 Tjoint *GetWorldJoint(unsigned int jointID,Tworld *w);
587 
597 unsigned int GetWorldNLinks(Tworld *w);
598 
608 unsigned int GetWorldNJoints(Tworld *w);
609 
621 unsigned int GetWorldNObstacles(Tworld *w);
622 
635 unsigned int GetWorldNConvexBodiesInLinks(Tworld *w);
636 
651 unsigned int GetWorldNConvexBodies(Tworld *w);
652 
653 
667 unsigned int GetWorldNDOF(Tworld *w);
668 
669 
679 void GetWorldRangeDOF(unsigned int ndof,Tinterval *r,Tworld *w);
680 
681 
699 void GetWorldJointLabel(unsigned int ndof,char **string,Tworld *w);
700 
716 void GetWorldDOFLabel(unsigned int ndof,char **string,Tworld *w);
717 
718 
729 boolean IsWorldPolynomial(Tworld *w);
730 
745 void CheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w);
746 
762 void NoCheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w);
763 
773 boolean AnyCollision(Tworld *w);
774 
784 void PrintCollisions(FILE *f,Tworld *w);
785 
798 void CheckSelfCollisions(unsigned int fl,Tworld *w);
799 
812 void NoCheckSelfCollisions(unsigned int fl,Tworld *w);
813 
830 void CheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w);
831 
844 void NoCheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w);
845 
863 void CheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w);
864 
878 void NoCheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w);
879 
894 void CheckObstacleCollision(unsigned int fl,unsigned int b,Tworld *w);
895 
910 void NoCheckObstacleCollision(unsigned int fl,unsigned int b,Tworld *w);
911 
926 void NoCheckConnectedCollisions(unsigned int fl,Tworld *w);
927 
946 void InitWorldCD(Tparameters *pr,unsigned int mt,Tworld *w);
947 
960 boolean WorldCanCollide(Tworld *w);
961 
979 void GetLinkTransformsFromSolutionPoint(Tparameters *p,boolean simp,double *sol,
980  THTransform **tl,TLinkConf **def,Tworld *w);
981 
1001  double **sol,Tworld *w);
1002 
1014 
1027 boolean WorldContinuousCD(Tworld *w);
1028 
1053 boolean MoveAndCheckCDFromTransforms(boolean all,unsigned int tID,
1054  THTransform *tl,TLinkConf *def,
1055  THTransform *tlPrev,TLinkConf *defPrev,
1056  Tworld *w);
1057 
1086 boolean MoveAndCheckCD(Tparameters *p,boolean all,unsigned int tID,
1087  double *sol,double *solPrev,Tworld *w);
1088 
1089 
1117 boolean NewtonInWorld(Tparameters *p,double *v,Tbox *b_sol,Tworld *w);
1118 
1129 unsigned int GetWorldSystemVars(boolean **sv,Tworld *w);
1130 
1141 unsigned int GetWorldVarTopology(unsigned int vID,Tworld *w);
1142 
1161 unsigned int GetWorldSimpVariableMask(Tparameters *p,boolean **sv,Tworld *w);
1162 
1163 
1176 void GetWorldVarNames(char **vn,Tworld *w);
1177 
1190 char *GetWorldSystemVarName(unsigned int nv,Tworld *w);
1191 
1206 
1220 
1235 unsigned int WorldForceVars(Tparameters *p,boolean **fv,Tworld *w);
1236 
1237 
1257 unsigned int WorldSimpKinematicVars(Tparameters *p,boolean **kv,Tworld *w);
1258 
1274 unsigned int WorldForceVarsIndices(Tparameters *p,unsigned int **iv,Tworld *w);
1275 
1296 double WorldPotentialEnergy(Tparameters *p,boolean simp,double *sol,void *w);
1297 
1321 void WorldForceField(Tparameters *p,boolean simp,double *sol,
1322  double **g,void *w);
1323 
1369 void WorldForceFieldProjectedGradient(Tparameters *p,boolean simp,
1370  double *proj,double *sol,
1371  double **g,void *w);
1372 
1386 void FixLinks(Tparameters *p,Tworld *w);
1387 
1411 void WorldFixTensegrityAddon(Tparameters *p,unsigned int linkID,
1412  double **point,unsigned int *n,Tworld *w);
1413 
1426 void FixZToZero(Tparameters *p,Tworld *w);
1427 
1449  unsigned int t,
1450  unsigned int lID1,unsigned int lID2,
1451  double scale,THTransform *r,
1452  Tworld *w);
1482  Tworld *w);
1483 
1502  Tworld *w);
1503 
1518 void GetWorldJacobian(TJacobian *J,Tworld *w);
1519 
1536 
1554 unsigned int GetWorldSimpTopology(Tparameters *p,
1555  unsigned int **t,Tworld *w);
1556 
1574 unsigned int GetWorldTopology(Tparameters *p,
1575  unsigned int **t,Tworld *w);
1576 
1588 unsigned int WorldDOFTopology(unsigned int **t,Tworld *w);
1589 
1609 void GetWorldKinJacobian(Tparameters *p,unsigned int *nr,unsigned int *nc,
1610  Tequation ***J,Tworld *w);
1626 unsigned int WorldSimpCuikNewton(Tparameters *p,double *pt,Tworld *w);
1627 
1644 void EvaluateWorldJacobian(double *p,double ***J,Tworld *w);
1645 
1658 void WorldEvaluateEquations(double *pt,double *r,Tworld *w);
1659 
1671 boolean WorldInequalitiesHold(double *pt,Tworld *w);
1672 
1685 boolean WorldSimpInequalitiesHold(Tparameters *p,double *pt,Tworld *w);
1686 
1698 double WorldErrorInSimpInequalities(Tparameters *p,double *pt,Tworld *w);
1699 
1713 void WorldEvaluateSimpEquations(Tparameters *p,double *pt,double *r,
1714  Tworld *w);
1715 
1730 void WorldEvaluateSubSetSimpEquations(Tparameters *p,boolean *se,double *pt,double *r,
1731  Tworld *w);
1732 
1747 double WorldErrorInEquations(double *pt,Tworld *w);
1748 
1764 double WorldErrorInSimpEquations(Tparameters *p,double *pt,Tworld *w);
1765 
1779 double WorldErrorFromDOFs(Tparameters *p,double *dof,Tworld *w);
1780 
1792 double EvaluateWorldCost(Tparameters *p,boolean simp,double *pt,void *w);
1793 
1804 unsigned int GetWorldNumVariables(Tworld *w);
1805 
1816 unsigned int GetWorldNumSystemVariables(Tworld *w);
1817 
1836 unsigned int RegenerateWorldOriginalPoint(Tparameters *p,double *s,
1837  double **o,Tworld *w);
1838 
1858 unsigned int RegenerateWorldOriginalSystemPoint(Tparameters *p,double *s,
1859  double **o,Tworld *w);
1860 
1878 unsigned int WorldGenerateSimplifiedPoint(Tparameters *p,double *o,
1879  double **s,Tworld *w);
1898 unsigned int WorldGenerateSimplifiedPointFromSystem(Tparameters *p,double *o,
1899  double **s,Tworld *w);
1900 
1912 void GetWorldInitialBox(Tbox *b,Tworld *w);
1913 
1924 
1938 unsigned int WorldManifoldDimension(Tparameters *p,double *point,Tworld *w);
1939 
1957 unsigned int MaxWorldReduction(Tparameters *p,Tbox *b,
1958  double *reduction,Tworld *w);
1959 
1974 void PlotWorld(Tparameters *pr,Tplot3d *pt,double axesLength,Tworld *w);
1975 
1990 unsigned int RegenerateWorldSolutionPoint(Tparameters *pr,double *p,
1991  double **v,Tworld *w);
1992 
2023 void WorldPrintAtoms(Tparameters *pr,FILE *f,double *pt,Tworld *w);
2024 
2042 void WorldStoreRigidGroups(Tparameters *pr,FILE *f,double *pt,Tworld *w);
2043 
2064 void WorldAtomJacobian(Tparameters *pr,double *sol,unsigned int *nr,unsigned int *nc,double ***J,Tworld *w);
2065 
2088 void MoveWorld(Tparameters *pr,Tplot3d *pt,Tbox *b,Tworld *w);
2089 
2103 void PrintWorldAxes(Tparameters *pr,FILE *f,Tbox *b,Tworld *w);
2104 
2123 void PrintWorldCollisionInfo(FILE *f,char *fname,Tworld *w);
2124 
2144 void MoveWorldDOF(Tparameters *pr,Tplot3d *pt,double *dof,Tworld *w);
2145 
2164 unsigned int WorldDOF2Sol(Tparameters *p,double *dof,double **sol,Tbox *b,Tworld *w);
2165 
2182 void WorldSample2DOF(Tparameters *p,double *sample,double *dof,Tworld *w);
2183 
2208 void AnimateWorld(Tparameters *pr,char *pname,double axesLength,
2209  double frameDelay,Tlist *p,Tworld *w);
2210 
2221 void PrintWorldCS(Tparameters *p,char *fname,Tworld *w);
2222 
2237 void PrintWorld(char *fname,int argc,char **arg,Tworld *w);
2238 
2246 void DeleteWorld(Tworld *w);
2247 
2248 #endif
void PrintWorldCS(Tparameters *p, char *fname, Tworld *w)
Prints the cuiksystems derived from a world.
Definition: world.c:3881
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
unsigned int RegenerateWorldSolutionPoint(Tparameters *pr, double *p, double **v, Tworld *w)
Computes the missing values in a kinematic solution.
Definition: world.c:3468
boolean IsWorldPolynomial(Tworld *w)
Checks if the system of equations is polynomial.
Definition: world.c:2089
unsigned int * dof2param
Definition: world.h:326
double WorldErrorInSimpInequalities(Tparameters *p, double *pt, Tworld *w)
Determines the maximum error in the inequalites for the simplified system.
Definition: world.c:3230
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 GenerateForceEquilibriumEquations(Tparameters *p, Tworld *w)
Adds force equilibrium equations.
Definition: world.c:2510
unsigned int nl
Definition: world.h:238
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 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
boolean WorldCanCollide(Tworld *w)
Determines if any collision is potentially possible.
Definition: world.c:1063
unsigned int WorldForceVarsIndices(Tparameters *p, unsigned int **iv, Tworld *w)
Creates an array with the indices of the force variables.
Definition: world.c:2649
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 GetWorldRangeDOF(unsigned int ndof, Tinterval *r, Tworld *w)
Gets the range for a given degree of freedom.
Definition: world.c:2002
void PrintWorld(char *fname, int argc, char **arg, Tworld *w)
Prints the world.
Definition: world.c:3902
Tvector steps
Definition: world.h:214
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
Relation between two links.
Definition: joint.h:168
boolean ** checkCollisionsLL
Definition: world.h:250
void PrintWorldCollisionInfo(FILE *f, char *fname, Tworld *w)
Prints information collected durint last collision check.
Definition: world.c:3674
TCuikSystem kinCS
Definition: world.h:270
Auxiliary data for the collision detection.
Definition: cd.h:258
void WorldEvaluateEquations(double *pt, double *r, Tworld *w)
Evaluates the kinematic equations.
Definition: world.c:3206
boolean * systemVars
Definition: world.h:274
A step in a kinematic branch.
Definition: world.h:185
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 * link2dof
Definition: world.h:324
A homgeneous transform in R^3.
Definition of the TJacobian type and the associated functions.
boolean MoveAndCheckCD(Tparameters *p, boolean all, unsigned int tID, double *sol, double *solPrev, Tworld *w)
Checks a point for collision.
Definition: world.c:1102
Tmechanism m
Definition: world.h:236
boolean WorldInequalitiesHold(double *pt, Tworld *w)
Check if the inequalities hold.
Definition: world.c:3214
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
void EvaluateWorldJacobian(double *p, double ***J, Tworld *w)
Evaluates the kinematic Jacobian.
Definition: world.c:3198
void GetWorldKinJacobian(Tparameters *p, unsigned int *nr, unsigned int *nc, Tequation ***J, Tworld *w)
Gets the simplified kinematic Jacobian.
void GetWorldJacobian(TJacobian *J, Tworld *w)
Gets the kinematic Jacobian.
Definition: world.c:3137
void WorldSample2DOF(Tparameters *p, double *sample, double *dof, Tworld *w)
Transforms a sample degrees of freedom.
Definition: world.c:3800
void NoCheckConnectedCollisions(unsigned int fl, Tworld *w)
Desactivates the collision detection between connected links.
Definition: world.c:2225
Tenvironment e
Definition: world.h:235
void GetWorldVarNames(char **vn, Tworld *w)
Return the variable names.
Definition: world.c:2415
double WorldErrorFromDOFs(Tparameters *p, double *dof, Tworld *w)
Error in equations from DOFs.
Definition: world.c:3270
void WorldPrintAtoms(Tparameters *pr, FILE *f, double *pt, Tworld *w)
Generates a file with the atom centers in global coordiantes.
Definition: world.c:3496
void GetWorldDOFLabel(unsigned int ndof, char **string, Tworld *w)
Returns a label identifying each degree of freedom.
Definition: world.c:2036
Definition of the Tfilename type and the associated functions.
Definition of the Tplot3d type and the associated functions.
boolean ** checkCollisionsLO
Definition: world.h:259
void WorldEvaluateSubSetSimpEquations(Tparameters *p, boolean *se, double *pt, double *r, Tworld *w)
Evaluates a subset of the simplified kinematic equations.
Definition: world.c:3246
unsigned int WorldManifoldDimension(Tparameters *p, double *point, Tworld *w)
Dimensionality of the solution manifold.+.
Definition: world.c:3404
unsigned int WorldSimpCuikNewton(Tparameters *p, double *pt, Tworld *w)
Tries to reach the kinematic manifold.
Definition: world.c:3193
void PrintWorldAxes(Tparameters *pr, FILE *f, Tbox *b, Tworld *w)
Prints the axes of the mechanism.
Definition: world.c:3637
unsigned int np
Definition: world.h:242
unsigned int GetWorldNumSystemVariables(Tworld *w)
Number of system variables in the kinematic subsystem.
Definition: world.c:3297
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
All the necessary information to generate equations for mechanisms.
Definition: world.h:229
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
A color.
Definition: color.h:86
A mechanism description.
Definition: mechanism.h:51
char * GetWorldSystemVarName(unsigned int nv, Tworld *w)
Returns the name of a given system variable.
Definition: world.c:2423
void DeleteLinkTransforms(THTransform *tl, TLinkConf *def, Tworld *w)
Deletes transforms for each link.
Definition: world.c:1336
unsigned int GetWorldNObstacles(Tworld *w)
Gets the number of obstacles in the environment included in the world.
Definition: world.c:1862
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 NoCheckAllCollisions(unsigned int fl, unsigned int fo, Tworld *w)
Desactivates all the possible collision between links and links and obstacles.
Definition: world.c:2119
unsigned int * dof2range
Definition: world.h:331
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
Definition of the Tstatistics type and the associated functions.
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 GetWorldInitialBox(Tbox *b, Tworld *w)
Gets the kinematic search space for a given problem.
Definition: world.c:3388
unsigned int no
Definition: world.h:241
void DeleteWorld(Tworld *w)
Destructor.
Definition: world.c:3952
A polyhedron.
Definition: polyhedron.h:134
unsigned int * joint2dof
Definition: world.h:329
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
A 3D plot.
Definition: plot3d.h:54
double EvaluateWorldCost(Tparameters *p, boolean simp, double *pt, void *w)
Evaluates the functions cost defined in a world.
Definition: world.c:3281
boolean IsMechanismInWorldAllSpheres(Tworld *w)
TRUE if the mechanisms in the world is based on spheres.
Definition: world.c:1827
Tjoint * joint
Definition: world.h:186
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
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
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
unsigned int nj
Definition: world.h:240
unsigned int GetWorldObstacleID(char *obsName, Tworld *w)
Gets the identifier of an obstacle from its name.
Definition: world.c:1837
void GenerateWorldEquations(Tparameters *p, Tworld *w)
Generates all the cuiksystems derived from the world information.
Definition: world.c:2431
void CheckSelfCollisions(unsigned int fl, Tworld *w)
Activates all the possible collision between links.
Definition: world.c:2138
void AnimateWorld(Tparameters *pr, char *pname, double axesLength, double frameDelay, Tlist *p, Tworld *w)
Produces an animation along a path.
Definition: world.c:3837
Set of equations.
Definition: equations.h:81
An equation.
Definition: equation.h:237
A collection of obstacles (convex polyhedrons) with their names.
Definition: environment.h:39
void AddObstacle2World(char *name, Tpolyhedron *o, Tworld *w)
Adds an obstacle to the environment in the world.
Definition: world.c:1788
Definition of the Tbox type and the associated functions.
unsigned int AddLink2World(Tlink *l, boolean object, Tworld *w)
Adds a link to the mechanism in the world.
Definition: world.c:1383
A generic list.
Definition: list.h:46
TworldCD * wcd
Definition: world.h:320
unsigned int nsvars
Definition: world.h:273
Definition of the Tlist type and the associated functions.
boolean WorldSimpInequalitiesHold(Tparameters *p, double *pt, Tworld *w)
Check if the inequalities hold for the simplified system.
Definition: world.c:3222
void GenerateWorldSingularityEquations(Tparameters *p, char *ln, TCuikSystem *cs, Tworld *w)
Adds variables/equations to detect singularities.
Definition: world.c:3039
unsigned int * dof2joint
Definition: world.h:330
Definition of the Tmechanism type and the associated functions.
unsigned int nvars
Definition: world.h:272
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
unsigned int GetWorldNLinks(Tworld *w)
Gets the number of links in the mechanism included in the world.
Definition: world.c:1852
unsigned int nb
Definition: world.h:239
unsigned int GetWorldSimpTopology(Tparameters *p, unsigned int **t, Tworld *w)
Gets the topology of the variables.
Definition: world.c:3153
A table of parameters.
Definition of the TCuikSystem type and the associated functions.
unsigned int AddJoint2World(Tjoint *j, Tworld *w)
Adds a joint to the mechanism in the world.
Definition: world.c:1436
boolean * openLink
Definition: world.h:309
void FixZToZero(Tparameters *p, Tworld *w)
Generate equations to fix the Z components of the links.
Definition: world.c:2867
boolean AnyCollision(Tworld *w)
Determines if we want to avoid any collision.
Definition: world.c:2294
Tequations refEqs
Definition: world.h:291
void WorldEvaluateSimpEquations(Tparameters *p, double *pt, double *r, Tworld *w)
Evaluates the simplified kinematic equations.
Definition: world.c:3238
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
A generic vector.
Definition: vector.h:227
boolean InitTensegrityFromFile(Tparameters *p, char *fn, Tworld *w)
Constructor.
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
A kinematic branch.
Definition: world.h:213
TJacobian refEqsJ
Definition: world.h:301
void CheckLinkLinkCollision(unsigned int a, unsigned int b, Tworld *w)
Activates the possible collision between a particular pair of links.
Definition: world.c:2167
boolean InitWorldFromFile(Tparameters *p, boolean error, char *fn, Tworld *w)
Constructor.
unsigned int stage
Definition: world.h:230
A box.
Definition: box.h:83
void GenerateWorldTWSEquations(Tparameters *p, char *ln, TCuikSystem *cs, Tworld *w)
Adds variables/equations to detect translational workspace boundaries.
Definition: world.c:3104
A cuiksystem, i.e., a set of variables and equations defining a position analysis problem...
Definition: cuiksystem.h:181
unsigned int GetWorldLinkID(char *linkName, Tworld *w)
Gets the identifier of a link from its name.
Definition: world.c:1832
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
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
Definition of the Tvector type and the associated functions.
void PlotWorld(Tparameters *pr, Tplot3d *pt, double axesLength, Tworld *w)
Adds a world (environment plus mechanism) in a 3D scene.
Definition: world.c:3452
unsigned int GetWorldSystemVars(boolean **sv, Tworld *w)
Gets the system vars of the kinematic cuiksystem.
Definition: world.c:2383
Tlink * GetWorldLink(unsigned int linkID, Tworld *w)
Gets a link from its identifier.
Definition: world.c:1842
void NoCheckLinkLinkCollision(unsigned int a, unsigned int b, Tworld *w)
Desactivates the possible collision between a particular pair of links.
Definition: world.c:2179
Tjoint * GetWorldJoint(unsigned int jointID, Tworld *w)
Gets a joint from its identifier.
Definition: world.c:1847
unsigned int GetWorldNJoints(Tworld *w)
Gets the number of joints in the mechanism included in the world.
Definition: world.c:1857
Definition of the Tenvironment type and the associated functions.
The Jacobian of a set of equations.
Definition: jacobian.h:23
unsigned int ndof
Definition: world.h:322
Tbranch * branch2Link
Definition: world.h:283
void InitWorld(Tworld *w)
Constructor.
Definition: world.c:1358
void NoCheckSelfCollisions(unsigned int fl, Tworld *w)
Desactivates all the possible collision between links.
Definition: world.c:2153
Headers of the interface with the collision detection engine.
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
unsigned int GetWorldSimpVariableMask(Tparameters *p, boolean **sv, Tworld *w)
Identifies pose related variable that survied in the simplified system.
Definition: world.c:2396
unsigned int * sortedLinks
Definition: world.h:279
void GetWorldSimpJacobian(Tparameters *p, TJacobian *J, Tworld *w)
Gets the simplified kinematic Jacobian.
Definition: world.c:3145
unsigned int endEffector
Definition: world.h:245
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
unsigned int * dof2link
Definition: world.h:325
unsigned int GetWorldTopology(Tparameters *p, unsigned int **t, Tworld *w)
Gets the topology of the variables.
Definition: world.c:3162
Definition of the Tmapping type and the associated functions.
unsigned int GetWorldNDOF(Tworld *w)
Gets the number of degrees of freedom in the world.
Definition: world.c:1997
unsigned int WorldForceVars(Tparameters *p, boolean **fv, Tworld *w)
Creates a boolean array identifying force variables.
Definition: world.c:2585
void InitWorldCD(Tparameters *pr, unsigned int mt, Tworld *w)
Initializes the collision detector.
Definition: world.c:1030
void PrintCollisions(FILE *f, Tworld *w)
Stores the collision information into a file.
Definition: world.c:2306
unsigned int GetWorldNumVariables(Tworld *w)
Number of variables in the kinematic subsystem.
Definition: world.c:3289
Defines a interval.
Definition: interval.h:33
unsigned int WorldSimpKinematicVars(Tparameters *p, boolean **kv, Tworld *w)
Creates a boolean array to identify kinematic variables.
Definition: world.c:2616
unsigned int GetWorldNConvexBodiesInLinks(Tworld *w)
Gets the number of convex polyhedrons that define the mechanism included in the world.
Definition: world.c:1867
Definition of the Tparameters type and the associated functions.
double WorldPotentialEnergy(Tparameters *p, boolean simp, double *sol, void *w)
Evaluates the potential energy of a configuration.
Definition: world.c:2680
double sign
Definition: world.h:187
Definition of the Tequations type and the associated functions.
void GetWorldSimpInitialBox(Tparameters *p, Tbox *b, Tworld *w)
Gets the kinematic simplified search space for a given problem.
Definition: world.c:3396
unsigned int nwcd
Definition: world.h:316
unsigned int GetWorldMobility(Tworld *w)
Returns the number of degrees of freedom of the mechanism in the world.
Definition: world.c:1822
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
unsigned int * jointTo
Definition: world.h:281
double maxCoord
Definition: world.h:247
unsigned int WorldDOFTopology(unsigned int **t, Tworld *w)
Gets the topology of the degrees of freedom.
Definition: world.c:3171
void WorldFixTensegrityAddon(Tparameters *p, unsigned int linkID, double **point, unsigned int *n, Tworld *w)
Fixes a tensegrity addon.
Definition: world.c:2819
Link configuration.
Definition: link.h:276