Institut de Robòtica i Informàtica Industrial
KRD Group

The CuikSuite Project

world.h File Reference

Definition of the Tworld type and the associated functions. More...

#include "vector.h"
#include "mechanism.h"
#include "cuiksystem.h"
#include "box.h"
#include "link.h"
#include "joint.h"
#include "plot3d.h"
#include "color.h"
#include "equations.h"
#include "csmapping.h"
#include "parameters.h"
#include "filename.h"
#include <stdlib.h>

Go to the source code of this file.

Data Structures

struct  Tworld
 Information to generate equations from high level link descriptions. More...

Defines

#define INITIAL_FRAME_DELAY   5.0
 When generating 3d animations, delay before starting the animation.
#define FRAME_RATE   (1.0/10)
 When generating 3d animations, frame rate.
#define FINAL_FRAME_DELAY   500.0
 When generating 3d animations, delay between the end of the animation and the exit of the animation browser.
#define EMPTY_WORLD   0
 One of the stages of the Tworld structure definition.
#define WORLD_WITH_GEOMETRY   1
 One of the stages of the Tworld structure definition.
#define WORLD_WITH_EQUATIONS   2
 One of the stages of the Tworld structure definition.

Functions

void InitWorld (Tworld *w)
 Constructor.
void InitWorldFromFile (Tparameters *p, Tfilename *f, Tworld *w)
 Constructor.
unsigned int AddLink2World (Tlink *l, Tworld *w)
 Adds a link to the mechanism in the world.
unsigned int AddJoint2World (Tjoint *j, Tworld *w)
 Adds a joint to the mechanism in the world.
unsigned int GetWorldDOF (Tworld *w)
 Returns the number of degrees of freedom of the mechanism in the world.
unsigned int GetWorldLinkID (char *linkName, Tworld *w)
 Gets the identifier of a link from its name.
TlinkGetWorldLink (unsigned int linkID, Tworld *w)
 Gets a link from its identifier.
unsigned int GetWorldNLinks (Tworld *w)
 Gets the number of links in the mechanism included in the world.
unsigned int GetWorldNJoints (Tworld *w)
 Gets the number of joints in the mechanism included in the world.
unsigned int GetWorldNConvexBodiesInLinks (Tworld *w)
 Gets the number of convex polyhedrons that define the mechanism included in the world.
unsigned int GetWorldSystemVars (boolean **sv, Tworld *w)
 Gets the system vars of the global cuiksystem.
void GenerateWorldEquationSystems (Tparameters *p, Tworld *w)
 Generates all the cuiksystems derived from the world information.
void GetGlobalInitialBox (Tbox *b, Tworld *w)
 Gets the search space for a given problem.
unsigned int MaxKinematicReduction (Tparameters *p, Tbox *b, double *reduction, Tworld *w)
 Reduces the system variables as much as possible using the kinematic constraints.
unsigned int SplitGlobalBox (Tparameters *p, Tbox *box, Tbox **b1, Tbox **b2, Tworld *w)
 Split a box into two sub-boxes.
void PlotWorld (Tplot3d *pt, double axesLength, Tworld *w)
 Adds a world (environment plus mechanism) in a 3D scene.
void MoveWorld (Tplot3d *pt, Tbox *b, Tworld *w)
 Moves the mechanisms defined in a world information to a given configuration.
void AnimateWorld (char *pname, double axesLength, double frameDelay, Tlist *p, Tworld *w)
 Produces an animation along a path.
void PrintWorldCS (Tparameters *p, Tfilename *fname, Tworld *w)
 Prints the cuiksystems derived from a world.
void DeleteWorld (Tworld *w)
 Destructor.

Detailed Description

Definition of the Tworld type and the associated functions.

See also:
Tworld, world.c.

Definition in file world.h.


Define Documentation

#define INITIAL_FRAME_DELAY   5.0

When generating 3d animations, delay before starting the animation (in seconds).

Definition at line 31 of file world.h.

Referenced by AnimateWorld().

#define FRAME_RATE   (1.0/10)

When generating 3d animations, frames per second.

Definition at line 37 of file world.h.

Referenced by AnimateWorld().

#define FINAL_FRAME_DELAY   500.0

When generating 3d animations, delay between the end of the animation and the exit of the animation browser (in seconds).

Definition at line 45 of file world.h.

Referenced by AnimateWorld().

#define EMPTY_WORLD   0

The world is just initialized and holds no data.

Definition at line 52 of file world.h.

Referenced by InitWorld().

#define WORLD_WITH_GEOMETRY   1

The world is includes geometric information (links, joints).

Definition at line 58 of file world.h.

Referenced by DeleteWorld(), GenerateWorldEquationSystems(), and PlotWorld().

#define WORLD_WITH_EQUATIONS   2

Function Documentation

void InitWorld ( Tworld w  ) 

Initializes an empty world.

Parameters:
w The world to initialize.

Definition at line 964 of file world.c.

References EMPTY_WORLD, InitMechanism(), Tworld::m, Tworld::nb, Tworld::nj, Tworld::nl, Tworld::refEqs, Tworld::stage, and Tworld::systemVars.

Here is the call graph for this function:

void InitWorldFromFile ( Tparameters p,
Tfilename f,
Tworld w 
)

Initializes an world from the information stored in a .world file.

We don't have documentation yet about how to define world files. Please, have a look to the examples directory.

Parameters:
p A set of parameters. Used, for instance, when defining the equations (dummification level, etc).
f The name of the file from where to read the information.
w The world to initialize.

Referenced by main().

Here is the caller graph for this function:

unsigned int AddLink2World ( Tlink l,
Tworld w 
)

Adds a link to the mechanism in the world.

Parameters:
l The link to add.
w The world to update.
Returns:
The identifier assigned to the new link.

Definition at line 978 of file world.c.

References AddLink2Mechanism(), LinkNBodies(), Tworld::m, Tworld::nb, and Tworld::nl.

Here is the call graph for this function:

unsigned int AddJoint2World ( Tjoint j,
Tworld w 
)

Adds a joint to the mechanism in the world.

Parameters:
j The joint to add.
w The world to update.
Returns:
The identifier assigned to the new joint.

Definition at line 989 of file world.c.

References AddJoint2Mechanism(), Tworld::m, and Tworld::nj.

Here is the call graph for this function:

unsigned int GetWorldDOF ( Tworld w  ) 

Returns the number of degrees of freedom of the mechanism in the world.

Parameters:
w The world to query.
Returns:
The number of degrees of freedom of the mechanism in the world.
See also:
GetMechanismDOF.

Definition at line 996 of file world.c.

References GetMechanismDOF(), and Tworld::m.

Here is the call graph for this function:

unsigned int GetWorldLinkID ( char *  linkName,
Tworld w 
)

Gets the identifier of a link from its name (NO_UINT if there is no link with the given name).

Parameters:
linkName The name of the link.
w The world to query.
Returns:
The identifier assigned to the new link.

Definition at line 1001 of file world.c.

References GetLinkID(), and Tworld::m.

Here is the call graph for this function:

Tlink* GetWorldLink ( unsigned int  linkID,
Tworld w 
)

Gets a pointer to a link in the mechanism included in the world from its identifier (NULL if there is no link with the given identifier).

Parameters:
linkID The identifier of the link to retrive.
w The world to query.
Returns:
The pointer to the requested link, if any.

Definition at line 1006 of file world.c.

References GetMechanismLink(), and Tworld::m.

Here is the call graph for this function:

unsigned int GetWorldNLinks ( Tworld w  ) 

Gets the number of links in the mechanism included in the world.

Parameters:
w The world to query.
Returns:
The number of links in the mechanism included in the world.

Definition at line 1011 of file world.c.

References Tworld::nl.

unsigned int GetWorldNJoints ( Tworld w  ) 

Gets the number of joints in the mechanism included in the world.

Parameters:
w The world to query.
Returns:
The number of joints in the mechanism included in the world.

Definition at line 1016 of file world.c.

References Tworld::nj.

unsigned int GetWorldNConvexBodiesInLinks ( Tworld w  ) 

Each link can include more than one convex polyhedron. This function returns the sum of the number of convex polyhedrons defined over all links

Parameters:
w The world to query.
Returns:
The number of convex bodies in the mechanism included in the world.

Definition at line 1021 of file world.c.

References Tworld::nb.

unsigned int GetWorldSystemVars ( boolean **  sv,
Tworld w 
)

Gets the system vars of the global cuiksystem in the form of an array of booleans, one for each variable in the kinCS where only entries corresponding to system variables are set to TRUE.

Parameters:
sv Space where to store the output array of booleans.
w The world to query.

Definition at line 1026 of file world.c.

References Error(), GetCSSystemVars(), Tworld::kinCS, Tworld::stage, and WORLD_WITH_EQUATIONS.

Here is the call graph for this function:

void GenerateWorldEquationSystems ( Tparameters p,
Tworld w 
)

From the information stored in a .world file, we generate the environment and mechanisms structures. From this information we have to generate systems of equations necessary to solve the problem. This is the purpose of this function. Therefore, this function should be used after reading the .world file and before start to solve the problem.

Parameters:
p A set of parameters (used, for instance, to find out the dummification level).
w The world structure with the information from where to generate the cuiksystems.

Definition at line 1034 of file world.c.

References Branch2Link(), COORD_EQ, Error(), GenerateTransEquationsFromBranch(), GetCSSystemVars(), GetMechanismMaxCoordinate(), InitWorldCS(), InitWorldKinCS(), Tworld::kinCS, Tworld::m, Tworld::maxCoord, NEW, Tworld::nl, Tworld::refEqs, Tworld::stage, Tworld::systemVars, WORLD_WITH_EQUATIONS, and WORLD_WITH_GEOMETRY.

Here is the call graph for this function:

void GetGlobalInitialBox ( Tbox b,
Tworld w 
)

Gets the search space for a given problem. The search space is deduced from the ranges of the variables involved in the problem.

This can be seen as a box from world constructor.

Parameters:
b The output box with the search space.
w The world to query.

Definition at line 1077 of file world.c.

References Error(), GenerateInitialBox(), Tworld::kinCS, Tworld::stage, and WORLD_WITH_EQUATIONS.

Here is the call graph for this function:

unsigned int MaxKinematicReduction ( Tparameters p,
Tbox b,
double *  reduction,
Tworld w 
)

Reduces the system variables as much as possible taking into account only the system equations (i.e., using only the Tworld::kinCS).

Parameters:
p The set of parameters to use when solving the cuiksystems (RHO, SIGMA,...).
b The box with the ranges to reduce.
reduction The reduction ratio (size of the output box / size of the input box).
w The world to use in the reduction.
Returns:
The box status after the reduction: EMPTY_BOX, REDUCED_BOX, REDUCED_BOX_WITH_SOLUTION ERROR_IN_PROCESS.

Definition at line 1085 of file world.c.

References EMPTY_BOX, Error(), GetBoxLevel(), GetBoxSize(), Tworld::kinCS, MaxReduction(), Tworld::stage, SYSTEM_VAR, Tworld::systemVars, and WORLD_WITH_EQUATIONS.

Here is the call graph for this function:

unsigned int SplitGlobalBox ( Tparameters p,
Tbox box,
Tbox **  b1,
Tbox **  b2,
Tworld w 
)
Parameters:
p Parameters to take into account in the split (ERROR_SPLIT,...)
box The box to split.
b1 The first sub-box resulting from the split.
b2 The second sub-box resulting from the split.
w The world to which the boxes are referred.
Returns:
The dimension along which the split took place.

Definition at line 1130 of file world.c.

References ComputeSplitDim(), CUT_POINT, Error(), Tworld::kinCS, NEW, SplitBox(), Tworld::stage, and WORLD_WITH_EQUATIONS.

Here is the call graph for this function:

void PlotWorld ( Tplot3d pt,
double  axesLength,
Tworld w 
)

Adds a world (environment plus mechanism) in a 3D scene. Note that the the mechanism is not set in any valid configuration but all links are plotted with its local frame overlapped with the global one. To actually display the mechanism in a correct configuration you should use MoveWorld.

Parameters:
pt The 3d scene where we add the geometry.
axesLength Length for the axes for each link. 0 not to display them.
w The wold to plot.

Definition at line 1152 of file world.c.

References Error(), Tworld::m, PlotMechanism(), Tworld::stage, and WORLD_WITH_GEOMETRY.

Referenced by AnimateWorld(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

void MoveWorld ( Tplot3d pt,
Tbox b,
Tworld w 
)

Moves the mechanisms defined in a world information to a given configuration.

Note that the box should only include ranges for the system variables. Therefore, the box given as a parameter is a sub-box of the box associated with Tworld::kinCS.

Parameters:
pt The 3d scene where the world has been previously added (see PlotWorld).
b The box from where to get the configuration. The configuration used to plot the mechanism is defined from the center of the interval for the system variables. Note that by taking the center it is not warranteed the configuration to be valid but if boxes are small enough the committed error will be small too.
w The world with the mechanism to move.

Definition at line 1160 of file world.c.

References Error(), EvaluateEquation(), GetBoxInterval(), GetCSNumVariables(), GetEquationValue(), IntervalCenter(), Tworld::kinCS, Tworld::m, MoveMechanism(), NEW, Tworld::nl, Tworld::refEqs, RegenerateMechanismSolution(), Tworld::stage, Tworld::systemVars, and WORLD_WITH_EQUATIONS.

Referenced by AnimateWorld(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

void AnimateWorld ( char *  pname,
double  axesLength,
double  frameDelay,
Tlist p,
Tworld w 
)

Produces an animation along a path. An animation is a 3d geometry that can be browsed with geomview.

Note that the boxes in the list should only include ranges for the system variables. Therefore, the box given as a parameter is a sub-box of the box associated with Tworld::kinCS. Lists printed while solving a problem are already in this form (i.e., including only system variables)

Parameters:
pname Name for the file with the geometry to generate.
axesLength Length for the axes for each link. 0 not to display them.
frameDelay Extra time (in seconds) added in between frames. In the default mode animations show 10 frames per second (i.e., the frame delay is 0.1). This parameter can be used to slow down animations. This is very useful when animating sets of isolated solutions: we can freeze each frame time enough to inspect each solution separately.
p The list of boxes with the frames of the animation.
w The world.

Definition at line 1211 of file world.c.

References Advance(), Close3dBlock(), ClosePlot3d(), Delay3dObject(), EndOfList(), Error(), FALSE, FINAL_FRAME_DELAY, First(), FRAME_RATE, GetBoxNIntervals(), GetCSNumNonDummyVariables(), GetCurrent(), INITIAL_FRAME_DELAY, InitIterator(), InitPlot3d(), Tworld::kinCS, MoveWorld(), PlotWorld(), Tworld::stage, Start3dBlock(), TRUE, and WORLD_WITH_EQUATIONS.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

void PrintWorldCS ( Tparameters p,
Tfilename fname,
Tworld w 
)

Generate a file with the cuiksystems generated from the world information.

Parameters:
p A set of parameters.
fname The base name for the file names to create
w The world with the cuiksystems to print.

Definition at line 1254 of file world.c.

References CreateFileName(), CUIK_EXT, DeleteFileName(), Error(), GetFileFullName(), GetFileName(), GetFilePath(), Tworld::kinCS, PARAM_EXT, PrintCuikSystem(), Tworld::stage, and WORLD_WITH_EQUATIONS.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

void DeleteWorld ( Tworld w  ) 

Deletes the information stored in a world and frees the allocated memory.

Parameters:
w The world to delete.

Definition at line 1286 of file world.c.

References DeleteEquation(), DeleteMechanism(), DeleteWorldCS(), Tworld::m, Tworld::nl, Tworld::refEqs, Tworld::stage, Tworld::systemVars, WORLD_WITH_EQUATIONS, and WORLD_WITH_GEOMETRY.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function: