Detailed Description

Implementaton of the function operating on Tworld.

See Also: Tworld, world.h

Definition in file world.c.

Functions
void	CreateBranch (Tbranch *b)
	Constructor. More...

void	CopyBranch (Tbranch b_dst, Tbranch b_src)
	Copy constructor. More...

void	AddStepToBranch (double s, Tjoint joint, Tbranch b)
	Adds a step to a kinematic chain. More...

unsigned int	nStepsBranch (Tbranch *b)
	Gets the number of steps in a kinematic chain. More...

void	GetBranchStep (unsigned int i, double s, Tjoint joint, Tbranch b)
	Retrives a particular step form a kinematic chain. More...

unsigned int	GetBranchOrigin (Tbranch *b)
	Returns the identifier of the link where the kinematic chain starts. More...

unsigned int	GetBranchDestination (Tbranch *b)
	Returns the identifier of the link where the kinematic chain ends. More...

void	MergeBranches (Tbranch b1, Tbranch b2, Tbranch *b)
	Merge two kinematic branches. More...

void	GetTransformFromBranch (THTransform tj, THTransform t, Tbranch *b)
	Defines the transform taking to the end of the branch. More...

void	DeleteBranch (Tbranch *b)
	Destructor. More...

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. More...

void	GenerateKinTree (Tvector bOut, Tworld w)
	Builds a kinematic tree departing from the ground link. More...

void	GenerateMEquationFromBranch (Tparameters p, Tbranch b, TMequation meq, Tworld w)
	Generates a matrix equation from a branch. More...

void	GenerateTransEquationsFromBranch (Tparameters p, unsigned int eq_type, Tequation eqs, Tbranch b, Tworld w)
	Generate equations from a branch. More...

boolean	GenerateEquationsFromBranch (Tparameters p, unsigned int eq_type, TCuikSystem cs, Tbranch b, Tworld w)
	Generate equations from a branch. More...

void	DeleteWorldCollisionInfo (Tworld *w)
	Removes the collision information stored in a Tworld. More...

void	WorldInitDOFInfo (Tworld *w)
	Collects information about the DOF of the mechanism. More...

void	WorldDeleteDOFInfo (Tworld *w)
	Deletes the information collected at WorldInitDOFInfo. More...

void	GetLinkTransformsFromDOF (double dof, THTransform tl, Tworld w)
	Define transforms for the links from the DOF. More...

void	GetLinkTransformsFromSolution (Tparameters p, double sol, THTransform *tl, Tworld w)
	Define transforms for the links from the a solution point. More...

boolean	IsRevoluteBinaryLink (unsigned int nl, double **p, Tworld w)
	Identifies links articulated with only two revolute joints. More...

void	InitWorldKinCS (Tparameters p, Tworld w)
	Initializes the kinematic sub-problem of a Tworld structure. More...

void	InitWorldCS (Tworld *w)
	Initializes the cuiksystems in a Tworld structure. More...

void	DeleteWorldCS (Tworld *w)
	Deletes the world cuik system. More...

void	DeleteWorldCD (Tworld *w)
	Deletes the collision information stored in the world. More...

void	InitWorldCD (Tparameters pr, unsigned int mt, Tworld w)
	Initializes the collision detector. More...

boolean	WorldCanCollide (Tworld *w)
	Determines if any collision is potentially possible. More...

boolean	WorldContinuousCD (Tworld *w)
	Determines the type of collision library used. More...

boolean	MoveAndCheckCDFromTransforms (boolean all, unsigned int tID, THTransform tl, THTransform tlPrev, Tworld *w)
	Checks a point for collision. More...

boolean	MoveAndCheckCD (Tparameters p, boolean all, unsigned int tID, double sol, double solPrev, Tworld w)
	Checks a point for collision. More...

void	GetLinkTransformsFromSolutionPoint (Tparameters p, boolean simp, double sol, THTransform *tl, Tworld w)
	Define transforms for the links from the a solution point. More...

unsigned int	GetSolutionPointFromLinkTransforms (Tparameters p, THTransform tl, double *sol, Tworld w)
	Determines the mechanisms configuration from the pose of all links. More...

void	DeleteLinkTransforms (THTransform tl, Tworld w)
	Deletes transforms for each link. More...

void	InitWorld (Tworld *w)
	Constructor. More...

unsigned int	AddLink2World (Tlink l, boolean endEffector, Tworld w)
	Adds a link to the mechanism in the world. More...

unsigned int	AddJoint2World (Tjoint j, Tworld w)
	Adds a joint to the mechanism in the world. More...

void	AddObstacle2World (char name, Tpolyhedron o, Tworld *w)
	Adds an obstacle to the environment in the world. More...

unsigned int	GetWorldMobility (Tworld *w)
	Returns the number of degrees of freedom of the mechanism in the world. More...

boolean	IsMechanismInWorldAllSpheres (Tworld *w)
	TRUE if the mechanisms in the world is based on spheres. More...

unsigned int	GetWorldLinkID (char linkName, Tworld w)
	Gets the identifier of a link from its name. More...

unsigned int	GetWorldObstacleID (char obsName, Tworld w)
	Gets the identifier of an obstacle from its name. More...

Tlink *	GetWorldLink (unsigned int linkID, Tworld *w)
	Gets a link from its identifier. More...

Tjoint *	GetWorldJoint (unsigned int jointID, Tworld *w)
	Gets a joint from its identifier. More...

unsigned int	GetWorldNLinks (Tworld *w)
	Gets the number of links in the mechanism included in the world. More...

unsigned int	GetWorldNJoints (Tworld *w)
	Gets the number of joints in the mechanism included in the world. More...

unsigned int	GetWorldNObstacles (Tworld *w)
	Gets the number of obstacles in the environment included in the world. More...

unsigned int	GetWorldNConvexBodiesInLinks (Tworld *w)
	Gets the number of convex polyhedrons that define the mechanism included in the world. More...

unsigned int	GetWorldNConvexBodies (Tworld *w)
	Gets the number of convex polyhedrons that define the mechanism and the environment included in the world. More...

unsigned int	GetWorldNDOF (Tworld *w)
	Gets the number of degrees of freedom in the world. More...

void	GetWorldRangeDOF (unsigned int ndof, Tinterval r, Tworld w)
	Gets the range for a given degree of freedom. More...

void	GetWorldJointLabel (unsigned int ndof, char *string, Tworld w)
	Returns a label identifying each joint. More...

void	GetWorldDOFLabel (unsigned int ndof, char *string, Tworld w)
	Returns a label identifying each degree of freedom. More...

boolean	IsWorldPolynomial (Tworld *w)
	Checks if the system of equations is polynomial. More...

void	CheckAllCollisions (unsigned int fl, unsigned int fo, Tworld *w)
	Activates all the possible collision between links and links and obstacles. More...

void	NoCheckAllCollisions (unsigned int fl, unsigned int fo, Tworld *w)
	Desactivates all the possible collision between links and links and obstacles. More...

void	CheckSelfCollisions (unsigned int fl, Tworld *w)
	Activates all the possible collision between links. More...

void	NoCheckSelfCollisions (unsigned int fl, Tworld *w)
	Desactivates all the possible collision between links. More...

void	CheckLinkLinkCollision (unsigned int a, unsigned int b, Tworld *w)
	Activates the possible collision between a particular pair of links. More...

void	NoCheckLinkLinkCollision (unsigned int a, unsigned int b, Tworld *w)
	Desactivates the possible collision between a particular pair of links. More...

void	CheckLinkObstacleCollision (unsigned int a, unsigned int b, Tworld *w)
	Activates the possible collision between a particular link and an object in the environment. More...

void	NoCheckLinkObstacleCollision (unsigned int a, unsigned int b, Tworld *w)
	Desactivates the possible collision between a particular link and an object in the environment. More...

boolean	AnyCollision (Tworld *w)
	Determines if we want to avoid any collision. More...

void	PrintCollisions (FILE f, Tworld w)
	Stores the collision information into a file. More...

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. More...

unsigned int	GetWorldSystemVars (boolean *sv, Tworld w)
	Gets the system vars of the kinematic cuiksystem. More...

unsigned int	GetWorldVarTopology (unsigned int vID, Tworld *w)
	Get the topology of a given variable. More...

unsigned int	GetWorldSimpVariableMask (Tparameters p, boolean sv, Tworld w)
	Identifies pose related variable that survied in the simplified system. More...

void	GetWorldVarNames (char *vn, Tworld w)
	Return the variable names. More...

void	GenerateWorldEquations (Tparameters p, Tworld w)
	Generates all the cuiksystems derived from the world information. More...

void	GenerateWorldSingularityEquations (Tparameters p, char ln, TCuikSystem cs, Tworld w)
	Adds variables/equations to detect singularities. More...

void	GenerateWorldTWSEquations (Tparameters p, char ln, TCuikSystem cs, Tworld w)
	Adds variables/equations to detect translational workspace boundaries. More...

void	GetWorldJacobian (TJacobian J, Tworld w)
	Gets the kinematic Jacobian. More...

void	GetWorldSimpJacobian (Tparameters p, TJacobian J, Tworld *w)
	Gets the simplified kinematic Jacobian. More...

unsigned int	GetWorldSimpTopology (Tparameters p, unsigned int t, Tworld w)
	Gets the topology of the variables. More...

unsigned int	WorldSimpCuikNewton (Tparameters p, double pt, Tworld *w)
	Tries to reach the kinematic manifold. More...

void	EvaluateWorldJacobian (double pt, double *J, Tworld w)
	Evaluates the kinematic Jacobian. More...

void	WorldEvaluateEquations (double pt, double r, Tworld *w)
	Evaluates the kinematic equations. More...

boolean	WorldInequalitiesHold (double pt, Tworld w)
	Check if the inequalities hold. More...

boolean	WorldSimpInequalitiesHold (Tparameters p, double pt, Tworld *w)
	Check if the inequalities hold for the simplified system. More...

double	WorldErrorInSimpInequalities (Tparameters p, double pt, Tworld *w)
	Determines the maximum error in the inequalites for the simplified system. More...

void	WorldEvaluateSimpEquations (Tparameters p, double pt, double r, Tworld w)
	Evaluates the simplified kinematic equations. More...

void	WorldEvaluateSubSetSimpEquations (Tparameters p, boolean se, double pt, double r, Tworld *w)
	Evaluates a subset of the simplified kinematic equations. More...

double	WorldErrorInEquations (double pt, Tworld w)
	Evalates the norm of the error in the kinematic equations for a given point. More...

double	WorldErrorInSimpEquations (Tparameters p, double pt, Tworld *w)
	Evalates the norm of the error in the kinematic equations for a given point. More...

double	WorldErrorFromDOFs (Tparameters p, double dof, Tworld *w)
	Error in equations from DOFs. More...

double	EvaluateWorldCost (Tparameters p, boolean simp, double pt, void *w)
	Evaluates the functions cost defined in a world. More...

unsigned int	GetWorldNumVariables (Tworld *w)
	Number of variables in the kinematic subsystem. More...

unsigned int	GetWorldNumSystemVariables (Tworld *w)
	Number of system variables in the kinematic subsystem. More...

unsigned int	RegenerateWorldOriginalPoint (Tparameters p, double s, double *o, Tworld w)
	Reconstruct a point in original kinematic system from a simplified point. More...

unsigned int	WorldGenerateSimplifiedPoint (Tparameters p, double o, double *s, Tworld w)
	Reconstruct a point in simplified kinematic system from an original point. More...

void	GetWorldInitialBox (Tbox b, Tworld w)
	Gets the kinematic search space for a given problem. More...

void	GetWorldSimpInitialBox (Tparameters p, Tbox b, Tworld *w)
	Gets the kinematic simplified search space for a given problem. More...

unsigned int	MaxWorldReduction (Tparameters p, Tbox b, double reduction, Tworld w)
	Reduces the system variables as much as possible using the kinematic constraints. More...

void	PlotWorld (Tparameters pr, Tplot3d pt, double axesLength, Tworld *w)
	Adds a world (environment plus mechanism) in a 3D scene. More...

unsigned int	RegenerateWorldSolutionPoint (Tparameters pr, double p, double *v, Tworld w)
	Computes the missing values in a kinematic solution. More...

void	WorldPrintAtoms (Tparameters pr, FILE f, double pt, Tworld w)
	Generates a file with the atom centers in global coordiantes. More...

void	WorldStoreRigidGroups (Tparameters pr, FILE f, double pt, Tworld w)
	Generates a file with the atoms grouped in rigid clusters. More...

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. More...

void	MoveWorld (Tparameters pr, Tplot3d pt, Tbox b, Tworld w)
	Moves the mechanisms defined in a world information to a given configuration. More...

void	PrintWorldAxes (Tparameters pr, FILE f, Tbox b, Tworld w)
	Prints the axes of the mechanism. More...

void	PrintWorldCollisionInfo (FILE f, char fname, Tworld *w)
	Prints information collected durint last collision check. More...

void	MoveWorldDOF (Tparameters pr, Tplot3d pt, double dof, Tworld w)
	Moves a mechanisms to a configuration given by the degrees of freedom. More...

unsigned int	WorldDOF2Sol (Tparameters p, double dof, double *sol, Tbox b, Tworld *w)
	Transforms from degrees of freedom to cuik variables. More...

void	WorldSample2DOF (Tparameters p, double sample, double dof, Tworld w)
	Transforms a sample degrees of freedom. More...

void	AnimateWorld (Tparameters pr, char pname, double axesLength, double frameDelay, Tlist p, Tworld w)
	Produces an animation along a path. More...

void	PrintWorldCS (Tparameters p, Tfilename fname, Tworld *w)
	Prints the cuiksystems derived from a world. More...

void	PrintWorld (char fname, int argc, char arg, Tworld w)
	Prints the world. More...

void	DeleteWorld (Tworld *w)
	Destructor. More...

Function Documentation

void CreateBranch ( Tbranch * b )

Defines an empty kinematic chain.

Parameters

b	The kinematic chain to initialize.

Definition at line 147 of file world.c.

References CopyVoidPtr(), DeleteVoidPtr(), INIT_NUM_JOINTS, InitVector(), and Tbranch::steps.

Referenced by Branches2Links(), CopyBranch(), and MergeBranches().

void CopyBranch	(	Tbranch *	b_dst,
		Tbranch *	b_src
	)

Defines a kinematic chain from another chain.

Parameters

b_dst	The kinematic chain to initialize.
b_src	The kinematic chain from where to copy the data.

Definition at line 152 of file world.c.

References AddStepToBranch(), CreateBranch(), GetBranchStep(), and nStepsBranch().

Referenced by Branches2Links(), and GenerateKinTree().

void AddStepToBranch	(	double	s,
		Tjoint *	joint,
		Tbranch *	b
	)

Adds a step to a kinematic chain.

Parameters

s	The sign of the step (1 if the given joint has to be used as given or -1 if it has to be used reversed).
joint	The joint to use to extend the kinematic branch.
b	The kinematic chain from where to copy the data.

Definition at line 167 of file world.c.

References TBranchStep::joint, NEW, NewVectorElement(), TBranchStep::sign, and Tbranch::steps.

Referenced by Branches2Links(), CopyBranch(), GenerateKinTree(), and MergeBranches().

unsigned int nStepsBranch ( Tbranch * b )

Gets the number of steps in a kinematic chain.

Parameters

b	The kinematic branch to query.

Returns: The number of steps in a kinematic chain.

Definition at line 179 of file world.c.

References Tbranch::steps, and VectorSize().

Referenced by CopyBranch(), DeleteBranch(), GenerateEquationsFromBranch(), GenerateKinTree(), GenerateMEquationFromBranch(), GenerateTransEquationsFromBranch(), GetBranchDestination(), GetTransformFromBranch(), and MergeBranches().

void GetBranchStep	(	unsigned int	i,
		double *	s,
		Tjoint **	joint,
		Tbranch *	b
	)

Retrives a particular step form a kinematic chain.

If there is no step with the given number in the kinematic chain, this funtion triggers an error.

Parameters

i	The number of step to get.
s	Output with the sign of the requested step.
joint	Output with the joint used in the requested step.
b	The kinematic branch to query.

Definition at line 184 of file world.c.

References Error(), GetVectorElement(), and Tbranch::steps.

Referenced by CopyBranch(), GenerateMEquationFromBranch(), GenerateTransEquationsFromBranch(), GetBranchDestination(), GetBranchOrigin(), and MergeBranches().

unsigned int GetBranchOrigin ( Tbranch * b )

Returns the identifier of the link where the kinematic chain starts.

Parameters

b	The kinematic branch to query.

Returns: The identifier of the link where the kinematic chain starts.

Definition at line 198 of file world.c.

References GetBranchStep(), JointFromID(), and JointToID().

Referenced by GenerateEquationsFromBranch().

unsigned int GetBranchDestination ( Tbranch * b )

Returns the identifier of the link where the kinematic chain ends.

Parameters

b	The kinematic branch to query.

Returns: The identifier of the link where the kinematic chain ends.

Definition at line 213 of file world.c.

References GetBranchStep(), JointFromID(), JointToID(), and nStepsBranch().

Referenced by GenerateEquationsFromBranch().

void MergeBranches	(	Tbranch *	b1,
		Tbranch *	b2,
		Tbranch *	b
	)

We get two branches, b1,b2, representing two paths from, typically, the groundLink to the same link. The output, b, will be the closed branch resulting from eliminating from b1 and b2 the initial common path from the groudnLink to the point where the paths diverge.

Parameters

b1	The first kinematic branch to merge.
b2	The second kinematic branch to merge.
b	The resulting kinematic branch.

Definition at line 228 of file world.c.

References AddStepToBranch(), CreateBranch(), FALSE, GetBranchStep(), and nStepsBranch().

Referenced by GenerateKinTree().

void GetTransformFromBranch	(	THTransform *	tj,
		THTransform *	t,
		Tbranch *	b
	)

Defines te transfrom taking from the origin to the destination of the branch by accumulating the transforms for the joints involved in the branch.

Parameters

tj	The set of transforms for each individual joint.
t	The output transform.
b	The branch.

Definition at line 271 of file world.c.

References GetJointID(), GetVectorElement(), HTransformIdentity(), HTransformInverse(), HTransformProduct(), TBranchStep::joint, nStepsBranch(), TBranchStep::sign, and Tbranch::steps.

Referenced by GetLinkTransformsFromDOF().

void DeleteBranch ( Tbranch * b )

Deletes the information stored in a kinematic branch and frees the allocated memory.

Parameters

b	The branch to delete.

Definition at line 292 of file world.c.

References DeleteVector(), GetVectorElement(), nStepsBranch(), and Tbranch::steps.

Referenced by DeleteWorldCS(), GenerateKinTree(), and InitWorldKinCS().

unsigned int Branches2Links	(	unsigned int	from,
		unsigned int *	links,
		unsigned int *	jointTo,
		boolean *	isLeaf,
		Tbranch *	b,
		Tworld *	w
	)

Considering the mechanisms as a graph where the vertices are the links and the edges are defined by the joints, this function determines the shortest from link f to all the other links, if possible (some links might be disconnected form link f).

Parameters

from	The identifier of the initial link for the branch to generate.
links	Output. The list of links as they are visited during the generation of the branches. The space for this array of size 'nl' must be allocated externally.
jointTo	Ouput. The joint necessary to reach each link. The space for this array of size 'nl' must be allocated externally.
isLeaf	Output. TRUE for the links that are at the extreme of the tree (no other link is reached from them). The space for this array of size 'nl' must be allocated externally.
b	The output branches. The space for this array of size 'nl' must be allocated externally.
w	The world to query.

Returns: The number of visited links (valid elements in 'links'). Must be w->nl if we managed to visit all links (i.e., if the graph is fully connected).

Definition at line 558 of file world.c.

References AddStepToBranch(), CopyBranch(), CreateBranch(), FALSE, GetMechanismJoint(), JointFromID(), JointToID(), Tworld::m, NEW, Tworld::nl, NO_UINT, and TRUE.

Referenced by GenerateKinTree().

void GenerateKinTree	(	Tvector *	bOut,
		Tworld *	w
	)

Builds a kinematic tree departing from the ground link. From the kinematic tree we extract two types of branches

Closed loops: Branches created when, at a given point, links appear from leaves of the tree to internal three nodes. Closed branches are then build using MergeBranches.
Open branches: Branches that connect the ground link to links with open branches, i.e., without loops. These are the kind of loops that appear in tree like mechanisms.

A mechanism can have the two types of branches.

Before building the kinematic tree, we ensure the mechanism to be fully connected: Parts of the mechanism disconnected from the ground link are connected to it using free-joints.

Parameters

bOut	Vector with the open and closed branches extracted from the kinematic tree.
w	The world to query.

Definition at line 624 of file world.c.

References AddJoint2Mechanism(), AddStepToBranch(), Tworld::branch2Link, Branches2Links(), CopyBranch(), CopyVoidPtr(), DeleteBranch(), DeleteJoint(), DeleteVoidPtr(), FALSE, GetMechanismJoint(), GetMechanismLink(), InitVector(), JointFromID(), Tworld::jointTo, JointToID(), Tworld::m, MergeBranches(), NEW, NewFreeJoint(), NewVectorElement(), Tworld::nj, Tworld::nl, nStepsBranch(), Tworld::openLink, Tworld::sortedLinks, and TRUE.

Referenced by InitWorldKinCS().

void GenerateMEquationFromBranch	(	Tparameters *	p,
		Tbranch *	b,
		TMequation *	meq,
		Tworld *	w
	)

Generates a matrix equatoin from a branch by accumulating the (symbolic) transforms for the joints in the branch.

Parameters

p	The set of parameters.
b	The branch.
meq	The resulting matrix equation.
w	The world.

Definition at line 718 of file world.c.

References AddTransSeq2MEquation(), DeleteTransSeq(), GetBranchStep(), GetJointTransSeq(), InitMEquation(), Tworld::kinCS, nStepsBranch(), and SimplifyMEquation().

Referenced by GenerateEquationsFromBranch(), and GenerateWorldEquations().

void GenerateTransEquationsFromBranch	(	Tparameters *	p,
		unsigned int	eq_type,
		Tequation *	eqs,
		Tbranch *	b,
		Tworld *	w
	)

Generate equations from a branch considering the branch merely as a sum of vectors.

The difference with GenerateEquationsFromBranch is that for open branches we do not add to the resulting equations the variables representing the position of the link reached by this open branch. Thus, the output equations here only involve system equations.

Moreove, the resulting equations are not added to a cuiksystem but returned to the caller.

Parameters

p	The set of parameters.
eq_type	Type for the equations to generete (SYSTEM_EQS, COORD_EQS).
eqs	Space for 3 equations where to store the result.
b	The branch from where to generate the equations.
w	The world on which we have to operate.

Definition at line 740 of file world.c.

References CT_REPRESENTATION, EQU, Error(), GenerateJointEquationsInBranch(), GetBranchStep(), GetParameter(), InitEquation(), Tworld::kinCS, nStepsBranch(), REP_JOINTS, SetEquationCmp(), and SetEquationType().

Referenced by GenerateEquationsFromBranch(), and GenerateWorldEquations().

boolean GenerateEquationsFromBranch	(	Tparameters *	p,
		unsigned int	eq_type,
		TCuikSystem *	cs,
		Tbranch *	b,
		Tworld *	w
	)

Generate equations from a branch.

If the branch is closed, we generate 3 loop equations (X, Y and Z) that are added to the given cuiksystem.

If the branch is open we generate equations that give the 3D position in the global frame of reference of the origin of the frame of reference of the last link in the branch.

This function uses GenerateTransEquationsFromBranch and then completes equations for open branches and add the results to the given cuiksystem.

Parameters

p	The set of parameters used when generating the equations (DUMMIFY_LEVEL,...).
eq_type	Type for the equations to generete (SYSTEM_EQS, COORD_EQS).
cs	The cuiksystem where to add the equations.
b	The branch from where to generate the equations.
w	The world on which we have to operate.

Returns: TRUE if the branch is open and new reference variables are added to the CuikSystem.

Definition at line 778 of file world.c.

References AddCt2Monomial(), AddEquation2CS(), AddMatrixEquation2CS(), AddMonomial(), AddVariable2CS(), AddVariable2Monomial(), COORD_EQ, CT_REPRESENTATION, DeleteEquation(), DeleteInterval(), DeleteMEquation(), DeleteMonomial(), DeleteVariable(), FALSE, GenerateMEquationFromBranch(), GenerateTransEquationsFromBranch(), GetBranchDestination(), GetBranchOrigin(), GetLinkName(), GetMechanismLink(), GetParameter(), InitMonomial(), LINK_TRANS, Tworld::m, Tworld::maxCoord, NEW, NewInterval(), NewVariable(), NFUN, nStepsBranch(), REP_JOINTS, ResetMonomial(), SetEquationType(), SetVariableInterval(), and SYSTEM_VAR.

Referenced by InitWorldKinCS().

void DeleteWorldCollisionInfo ( Tworld * w )

Removes the collision information stored in a Tworld: the Tworld::checkCollisionsLL and Tworld::checkCollisionsLO arrays.

Parameters

w	The world whose collision information we want to delete.

Definition at line 1482 of file world.c.

References Tworld::checkCollisionsLL, Tworld::checkCollisionsLO, and Tworld::nl.

Referenced by DeleteWorld().

void WorldInitDOFInfo ( Tworld * w )

Collects information about the DOF of the mechanism from the joints.

Parameters

w	The world whose DOF we want to determine.

Definition at line 1501 of file world.c.

References CoupledWith(), Tworld::dof2joint, Tworld::dof2range, Error(), GetJointDOF(), GetMechanismJoint(), Tworld::joint2dof, Tworld::m, Tworld::ndof, NEW, Tworld::nj, NO_UINT, Tworld::stage, and WORLD_DEFINED.

Referenced by GenerateWorldEquations().

void WorldDeleteDOFInfo ( Tworld * w )

Deletes the information collected at WorldInitDOFInfo.

Parameters

w	The world with the information to release.

Definition at line 1552 of file world.c.

References Tworld::dof2joint, Tworld::dof2range, and Tworld::joint2dof.

Referenced by DeleteWorldCS().

void GetLinkTransformsFromDOF	(	double *	dof,
		THTransform **	tl,
		Tworld *	w
	)

Computes a transform giving the pose for each link from values for the different degrees of freedom of the mechanisms.

Parameters

dof	The values for the degrees of freedom.
tl	The output transforms. The space is allocated inside this function.
w	the world.

Definition at line 1118 of file world.c.

References Tworld::branch2Link, GetJointTransform(), GetMechanismJoint(), GetTransformFromBranch(), HTransformDelete(), HTransformIdentity(), HTransformInverse(), HTransformProduct(), Tworld::joint2dof, JointFromID(), Tworld::jointTo, JointToID(), Tworld::m, NEW, Tworld::nj, Tworld::nl, and Tworld::sortedLinks.

Referenced by GetLinkTransformsFromSolution(), GetLinkTransformsFromSolutionPoint(), MoveWorldDOF(), and WorldDOF2Sol().

void GetLinkTransformsFromSolution	(	Tparameters *	p,
		double *	sol,
		THTransform **	tl,
		Tworld *	w
	)

Computes a transform giving the pose for each link from values for a solution point. Note that if solutions are given as degrees of freedom this is equivalent to GetLinkTransformsFromDOF.

The difference with respect to GetLinkTransformsFromSolutionPoint is that here the point includes values for all variables and in GetLinkTransformsFromSolutionPoint only the system variables are given.

Parameters

p	The parameters.
sol	The solution point (including all variables, system, dummies...)
tl	The output transforms. The space is allocated inside this function.
w	the world.

Definition at line 1260 of file world.c.

References CT_REPRESENTATION, EvaluateEqualityEquations(), FALSE, GetLinkTransformsFromDOF(), GetMechanismLink(), GetParameter(), GetTransform2Link(), IsGroundLink, Tworld::kinCS, Tworld::m, NEW, Tworld::nl, Tworld::refEqs, and REP_JOINTS.

Referenced by GetLinkTransformsFromSolutionPoint(), MoveAndCheckCD(), MoveWorld(), and WorldSample2DOF().

boolean IsRevoluteBinaryLink	(	unsigned int	nl,
		double ***	p,
		Tworld *	w
	)

Identifies links articulated with only two revolute joints. For those links it is possible to re-define the reference frame so that simpler equations will be obtained.

Parameters

nl	The identifier of the link to be checked.
p	Space to store four points defining the rotation axis for the first revolute axis. The indexing is p[i][j][k] where i is the number of axis (0,1) j the number of point in the axis (0,1) and k the component (0,1,2 for x, y, z).
w	The world with the information of links and joints.

Returns: TRUE if the link with the given identifier is a revolute, binary link.

See Also: ChangeLinkReferenceFrame

Definition at line 865 of file world.c.

References GetJointPoint(), GetJointType(), GetMechanismJoint(), JointFromID(), JointToID(), Tworld::m, and REV_JOINT.

Referenced by InitWorldKinCS().

void InitWorldKinCS	(	Tparameters *	p,
		Tworld *	w
	)

Initializes the kinematic sub-problem of a Tworld structure.

Parameters

p	A set of parametersused when generating the equations (DUMMIFY_LEVEL,...).
w	The world on which we have to operate.

Definition at line 900 of file world.c.

References AllRevolute(), ChangeLinkReferenceFrame(), CT_REPRESENTATION, DeleteBranch(), DeleteVector(), Error(), GenerateEquationsFromBranch(), GenerateJointEquations(), GenerateJointRangeEquations(), GenerateKinTree(), GenerateLinkRot(), GetMechanismJoint(), GetMechanismLink(), GetParameter(), GetVectorElement(), IsRevoluteBinaryLink(), Tworld::kinCS, Tworld::m, Tworld::maxCoord, NEW, Tworld::nj, Tworld::nl, REP_JOINTS, SYSTEM_EQ, and VectorSize().

Referenced by GenerateWorldEquations().

void InitWorldCS ( Tworld * w )

Initializes the cuiksystems in a Tworld structure. The Tworld::kinCS is initialized as an empty cuiksystem. Equations are added to this system using InitWorldKinCS.

Parameters

w	The world structure to initialzie.

Definition at line 981 of file world.c.

References InitCuikSystem(), and Tworld::kinCS.

Referenced by GenerateWorldEquations().

void DeleteWorldCS ( Tworld * w )

Deletes the equations stored in the world.

Parameters

w	The world with the cuiksystem.

Definition at line 986 of file world.c.

References Tworld::branch2Link, DeleteBranch(), DeleteCuikSystem(), DeleteEquations(), DeleteJacobian(), Tworld::jointTo, Tworld::kinCS, Tworld::nl, Tworld::openLink, Tworld::refEqs, Tworld::refEqsJ, Tworld::sortedLinks, Tworld::systemVars, and WorldDeleteDOFInfo().

Referenced by DeleteWorld().

void DeleteWorldCD ( Tworld * w )

Deletes the collision information stored in the world, if any.

Parameters

w	The world structure with the collision information to release.

Definition at line 1100 of file world.c.

References DeleteCD(), Tworld::nwcd, and Tworld::wcd.

Referenced by DeleteWorld().

void InitWorldCD	(	Tparameters *	pr,
		unsigned int	mt,
		Tworld *	w
	)

Initializes the collision detector.

This is decoupled from the world initialization so that we can init it only when required.

In some cases (for instance when parallelizing the atlas construction using OpenMP) we need to perform collision detection in parallel for different threads. To avoid problems, in this case we initialize separate collision detection structures.

Parameters

pr	The set of parameters.
mt	Maximum number of threads in parallal that might use the collision detection.
w	The world from where to read the link/obstacle information.

Definition at line 1013 of file world.c.

References AnyCollision(), Tworld::checkCollisionsLL, Tworld::checkCollisionsLO, CT_CD_ENGINE, DeleteCD(), Tworld::e, Error(), GetParameter(), InitCD(), Tworld::m, NEW, Tworld::nwcd, Tworld::stage, Tworld::wcd, and WORLD_DEFINED.

Referenced by main(), and PlotWorld().

boolean WorldCanCollide ( Tworld * w )

Determides if, after initializing the world collision info, any collision is possible.

This function can only be used after calling InitWorldCD.

Parameters

w	The world to query.

TRUE if any collision is possible.

Definition at line 1046 of file world.c.

References Tworld::wcd.

boolean WorldContinuousCD ( Tworld * w )

Determines if the collision detection engine is discrete or continuous. Note that the return is only valid if the collision detection system has been previously initialized.

Parameters

w	The world structure to query.

Returns: TRUE if we use a continuous collision detection engine.

Definition at line 1051 of file world.c.

References C_FCL, FALSE, GetCDEngine(), and Tworld::wcd.

boolean MoveAndCheckCDFromTransforms	(	boolean	all,
		unsigned int	tID,
		THTransform *	tl,
		THTransform *	tlPrev,
		Tworld *	w
	)

Moves the links to the position given by the corresponding transforms and determines if there is any collision.

No check is done on whether the given point is actually a solution or not.

Parameters

all	TRUE if we want to detect all the collision. FALSE just to detect the first one.
tID	Identifier of the thread executing this collision check. See InitWorldCD. This parameter is only relevant if parameter 'mt' in InitWorldCD is larger than 0.
tl	One homogeneous transform for each link.
tlPrev	One homogeneous transform for each link in the previous configuration. Only used for continuous collision check (C_FCL collision engine).
w	The world structure to check.

Returns: TRUE if there is a collision.

Definition at line 1060 of file world.c.

References CheckCollision(), FALSE, Tworld::nwcd, and Tworld::wcd.

Referenced by MoveAndCheckCD(), MoveWorld(), and MoveWorldDOF().

boolean MoveAndCheckCD	(	Tparameters *	p,
		boolean	all,
		unsigned int	tID,
		double *	sol,
		double *	solPrev,
		Tworld *	w
	)

Computes the link transforms from the solution point and calls MoveAndCheckCDFromTransforms.

The solution point is assumed to have values for all variables (including the dummy ones). Recall that a solution point with values only for the system variables can be compleated using RegenerateWorldSolutionPoint and that a solution point in the simplified system can be converted to a solution point in the full original system using RegenerateWorldOriginalPoint.

Parameters

p	The set of parameters.
all	TRUE if we want to detect all the collision. FALSE just to detect the first one.
tID	Identifier of the thread executing this collision check. See InitWorldCD. This parameter is only relevant if parameter 'mt' in InitWorldCD is larger than 0.
sol	The solution point (in the original system including values for all variables. The transform for each link is computed from the values here.
solPrev	Previous configuration. Only used for continuous collision detection (C_FCL collision engine).
w	The world structure to check.

Returns: TRUE if there is a collision.

Definition at line 1076 of file world.c.

References C_FCL, DeleteLinkTransforms(), FALSE, GetCDEngine(), GetLinkTransformsFromSolution(), MoveAndCheckCDFromTransforms(), and Tworld::wcd.

Referenced by main().

void GetLinkTransformsFromSolutionPoint	(	Tparameters *	p,
		boolean	simp,
		double *	sol,
		THTransform **	tl,
		Tworld *	w
	)

Computes a transform giving the pose for each link from values for a solution point.

The difference with respect to GetLinkTransformsFromSolution is that here only the system variables are given.

Parameters

p	The parameters.
simp	TRUE if the solution is given in the simplified form.
sol	The solution point.
tl	The output transforms. The space is allocated inside this function.
w	The world structure with the mechanical information.

Definition at line 1165 of file world.c.

References CT_REPRESENTATION, Error(), GetCSNumVariables(), GetLinkTransformsFromDOF(), GetLinkTransformsFromSolution(), GetParameter(), Tworld::kinCS, Tworld::m, NEW, RegenerateMechanismSolution(), RegenerateOriginalPoint(), REP_JOINTS, Tworld::stage, Tworld::systemVars, and WORLD_DEFINED.

Referenced by BioWordGetAtomPositionsFromConformation(), WorldPrintAtoms(), and WorldStoreRigidGroups().

unsigned int GetSolutionPointFromLinkTransforms	(	Tparameters *	p,
		THTransform *	tl,
		double **	sol,
		Tworld *	w
	)

Determines the mechanisms configuration from the transforms giving the global position of all links.

This is basically the inverse function of GetLinkTransformsFromSolutionPoint.

Parameters

p	The set of parameters.
tl	The transform giving the global pose of each link.
sol	The output configuration. The space for this array is allocated internally and its size depends on the CT_REPRESENTATION value. In any case only values for the system variables is allocated.
w	The world structure with the mechanical information.

Returns: The size of the solution point.

Definition at line 1215 of file world.c.

References CT_REPRESENTATION, DeleteBox(), GetCSNumSystemVariables(), GetMechanismDOFsFromTransforms(), GetParameter(), Tworld::kinCS, Tworld::m, Tworld::ndof, NEW, REP_JOINTS, Tworld::systemVars, and WorldDOF2Sol().

Referenced by BioWordConformationFromAtomPositions(), and InitWorldFromMolecule().

void DeleteLinkTransforms	(	THTransform *	tl,
		Tworld *	w
	)

Deletes the transforms computed by GetLinkTransformsFromDOF or by GetLinkTransformsFromSolution.

Parameters

tl	The array of transforms to delete.
w	The world structure.

Definition at line 1290 of file world.c.

References HTransformDelete(), and Tworld::nl.

Referenced by BioWordGetAtomPositionsFromConformation(), MoveAndCheckCD(), MoveWorld(), MoveWorldDOF(), WorldDOF2Sol(), WorldPrintAtoms(), WorldSample2DOF(), and WorldStoreRigidGroups().

void InitWorld ( Tworld * w )

Initializes an empty world.

The expected way to initialize worlds is using the InitWorldFromFile function. If you decide to do it yourself (i.e., using InitWorld and then adding links, joints, obstacles, etc) the function GenerateWorldEquations when all the information is already added to the world. Once this function is called no more information (links, joints, etc) can be possibly added to the world.

Parameters

w	The world to initialize.

Definition at line 1308 of file world.c.

References Tworld::branch2Link, Tworld::checkCollisionsLL, Tworld::checkCollisionsLO, Tworld::e, Tworld::endEffector, InitEnvironment(), InitMechanism(), Tworld::m, Tworld::nb, Tworld::nj, Tworld::nl, Tworld::no, NO_UINT, Tworld::np, Tworld::nsvars, Tworld::nvars, Tworld::stage, Tworld::systemVars, Tworld::wcd, and WORLD_IN_DEFINITION.

Referenced by InitWorldFromMolecule(), and main().

unsigned int AddLink2World	(	Tlink *	l,
		boolean	object,
		Tworld *	w
	)

Adds a link to the mechanism in the world.

Parameters

l	The link to add.
object	TRUE if the link has to be used as the object being manipulated by the rest of the mechanisms. This can be also seen as the end effector. Each mechanism only manipulates one object, the last link declared as such.
w	The world to update.

Returns: The identifier assigned to the new link.

Definition at line 1333 of file world.c.

References AddLink2Mechanism(), Tworld::checkCollisionsLL, Tworld::checkCollisionsLO, Tworld::endEffector, Error(), FALSE, LinkNBodies(), Tworld::m, MEM_EXPAND, Tworld::nb, NEW, Tworld::nl, Tworld::no, Tworld::np, Tworld::stage, and WORLD_IN_DEFINITION.

Referenced by InitWorldFromMolecule(), and main().

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 1383 of file world.c.

References AddJoint2Mechanism(), Error(), Tworld::m, Tworld::nj, Tworld::stage, and WORLD_IN_DEFINITION.

Referenced by InitWorldFromMolecule(), and main().

void AddObstacle2World	(	char *	name,
		Tpolyhedron *	o,
		Tworld *	w
	)

Adds an obstacle to the environment in the world.

Parameters

name	The name of the obstacle.
o	The convex object representing the obstacle.
w	The world to update.

Definition at line 1393 of file world.c.

References AddShape2Environment(), Tworld::checkCollisionsLO, Tworld::e, Error(), FALSE, MEM_EXPAND, NEW, Tworld::nl, Tworld::no, Tworld::np, Tworld::stage, and WORLD_IN_DEFINITION.

unsigned int GetWorldMobility ( Tworld * w )

Returns the number of degrees of freedom of the mechanism in the world. This consideres the presenceo of loops constraining the mobility.

Parameters

w	The world to query.

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

See Also: GetMechanismMobility.

Definition at line 1427 of file world.c.

References GetMechanismMobility(), and Tworld::m.

boolean IsMechanismInWorldAllSpheres ( Tworld * w )

Returns TRUE if the mechanisms in the world is composed by spheres only.

Parameters

w	The world to query.

Returns: TRUE if the mechanism in the world is sphere-based.

Definition at line 1432 of file world.c.

References IsMechanismAllSpheres(), and Tworld::m.

Referenced by main().

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 1437 of file world.c.

References GetLinkID(), and Tworld::m.

Referenced by main().

unsigned int GetWorldObstacleID	(	char *	obsName,
		Tworld *	w
	)

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

Parameters

obsName	The name of the obstacle.
w	The world to query.

Returns: The identifier assigned to the new link.

Definition at line 1442 of file world.c.

References Tworld::e, and GetObstacleID().

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 1447 of file world.c.

References GetMechanismLink(), and Tworld::m.

Referenced by InitWorldFromMolecule(), and main().

Tjoint* GetWorldJoint	(	unsigned int	jointID,
		Tworld *	w
	)

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

Parameters

jointID	The identifier of the joint to retrive.
w	The world to query.

Returns: The pointer to the requested joint, if any.

Definition at line 1452 of file world.c.

References GetMechanismJoint(), and Tworld::m.

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 1457 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 1462 of file world.c.

References Tworld::nj.

Referenced by main().

unsigned int GetWorldNObstacles ( Tworld * w )

Gets the number of obstacles in the environment included in the world. Note that each obstacle is a convex polyhedron (see Tpolyhedron).

Parameters

w	The world to query.

Returns: The number of obstacles in the environment included in the world.

Definition at line 1467 of file world.c.

References Tworld::no.

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 1472 of file world.c.

References Tworld::nb.

unsigned int GetWorldNConvexBodies ( Tworld * w )

Each link can include more than one convex polyhedron. Moreover, the environment is also defined as a set of convex polyhedrons. This function returns the sum of the number of convex polyhedrons defined over all links and the environment.

Parameters

w	The world to query.

Returns: The number of convex parts in the mechanism and in the environment included in the world.

Definition at line 1477 of file world.c.

References Tworld::np.

unsigned int GetWorldNDOF ( Tworld * w )

Gets the number of degrees of freedom in the world. Each joint introduces one or more degrees of freedom.

Note that this is different from GetWorldMobility. Here we return the sum of degrees of freedom of the joints.

Parameters

w	The world to query.

Returns: The number of degrees of freedom in the world.

Definition at line 1559 of file world.c.

References Tworld::ndof.

Referenced by main().

void GetWorldRangeDOF	(	unsigned int	ndof,
		Tinterval *	r,
		Tworld *	w
	)

Returns the valid range for a given degree of freedom.

Parameters

ndof	The identifier for the degree of freedom.
r	The output range.
w	The world to query.

Definition at line 1564 of file world.c.

References Tworld::dof2joint, Tworld::dof2range, Error(), GetJointRangeN(), GetMechanismJoint(), Tworld::m, Tworld::maxCoord, and Tworld::ndof.

Referenced by main().

void GetWorldJointLabel	(	unsigned int	ndof,
		char **	string,
		Tworld *	w
	)

Returns a label identifying eachjoint. This is used to generate interfaces. Right now we only return a valid label for the first degree of freedom of each joint and the label identifies the joint and not the particular degree of freedom. In all other cases the output is NULL.

Note that, if different from NULL, the string is initialized inside this call but must be deallocated by the caller.

Parameters

ndof	The identifier for the degree of freedom.
string	The output label. Possibly NULL.
w	The world to query.

Definition at line 1575 of file world.c.

References Tworld::dof2joint, Tworld::dof2range, Error(), GetJointName(), GetMechanismJoint(), and Tworld::m.

void GetWorldDOFLabel	(	unsigned int	ndof,
		char **	string,
		Tworld *	w
	)

Returns a label identifying each degree of freedom. This is used to generate interfaces. This is a variant of GetWorldJointLabel but where all dof get an identifier.

Note that, if different from NULL, the string is initialized inside this call but must be deallocated by the caller.

Parameters

ndof	The identifier for the degree of freedom.
string	The output label. Possibly NULL.
w	The world to query.

Definition at line 1585 of file world.c.

References Tworld::dof2joint, Tworld::dof2range, Error(), GetJointDOF(), GetJointName(), GetMechanismJoint(), Tworld::m, and NEW.

boolean IsWorldPolynomial ( Tworld * w )

inline

Checks if the system of equations derived from the world is polynomial.

Parameters

w	The world to query.

Returns: TRUE if the system is polynomial.

Definition at line 1607 of file world.c.

References Error(), IsCSPolynomial(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

void CheckAllCollisions	(	unsigned int	fl,
		unsigned int	fo,
		Tworld *	w
	)

Activates all the possible collision betwen links and links and obstacles.

Parameters

fl	First link. Use 0 if not sure what to use. This is used when including files where this function must affect only the links/objects defined in the current file.
fo	First object. Use 0 if not sure what to use.
w	The world to update.

Definition at line 1615 of file world.c.

References Tworld::checkCollisionsLO, CheckSelfCollisions(), DECOR_SHAPE, Tworld::e, Error(), GetObstacleShapeStatus(), IsGroundLink, Tworld::nl, Tworld::no, Tworld::stage, and WORLD_IN_DEFINITION.

Referenced by InitWorldFromMolecule().

void NoCheckAllCollisions	(	unsigned int	fl,
		unsigned int	fo,
		Tworld *	w
	)

Desactivates all the possible collision betwen links and links and obstacles.

Parameters

fl	First link. Use 0 if not sure what to use. This is used when including files where this function must affect only the links/objects defined in the current file.
fo	First object. Use 0 if not sure what to use.
w	The world to update.

Definition at line 1637 of file world.c.

References Tworld::checkCollisionsLO, Error(), FALSE, Tworld::nl, Tworld::no, NoCheckSelfCollisions(), Tworld::stage, and WORLD_IN_DEFINITION.

Referenced by main().

void CheckSelfCollisions	(	unsigned int	fl,
		Tworld *	w
	)

Activates all the possible collision betwen links.

Parameters

fl	First link. Use 0 if not sure what to use. This is used when including files where this function must affect only the links/objects defined in the current file.
w	The world to update.

Definition at line 1656 of file world.c.

References Tworld::checkCollisionsLL, Error(), Tworld::nl, Tworld::stage, and WORLD_IN_DEFINITION.

Referenced by CheckAllCollisions().

void NoCheckSelfCollisions	(	unsigned int	fl,
		Tworld *	w
	)

Desactivates all the possible collision betwen links.

Parameters

fl	First link. Use 0 if not sure what to use. This is used when including files where this function must affect only the links/objects defined in the current file.
w	The world to update.

Definition at line 1671 of file world.c.

References Tworld::checkCollisionsLL, Error(), FALSE, Tworld::nl, Tworld::stage, and WORLD_IN_DEFINITION.

Referenced by NoCheckAllCollisions().

void CheckLinkLinkCollision	(	unsigned int	a,
		unsigned int	b,
		Tworld *	w
	)

Activates all the possible collision betwen a particular pair of links.

Note that we don't have an interface to activate/desactivate the collision checking between links sub-parts. To this end you have to use the DECOR keyword in the sub-part definition (DECOR sub-parts are not used in collision checking).

Parameters

a	The identifier of the first link of the pair whose collision has to be checked.
b	The identifier of the second link of the pair whose collision has to be checked.
w	The world to update.

Definition at line 1685 of file world.c.

References Tworld::checkCollisionsLL, Error(), Tworld::stage, TRUE, and WORLD_IN_DEFINITION.

void NoCheckLinkLinkCollision	(	unsigned int	a,
		unsigned int	b,
		Tworld *	w
	)

Desactivates all the possible collision betwen a particular pair of links.

Parameters

a	The identifier of the first link of the pair whose collision has not to be checked.
b	The identifier of the second link of the pair whose collision has not to be checked.
w	The world to update.

Definition at line 1694 of file world.c.

References Tworld::checkCollisionsLL, Error(), FALSE, Tworld::stage, and WORLD_IN_DEFINITION.

void CheckLinkObstacleCollision	(	unsigned int	a,
		unsigned int	b,
		Tworld *	w
	)

Activates the possible collision between a particular link and an object in the environment.

Note that we don't have an interface to activate/desactivate the collision checking for a given link sub-part. To this end you have to use the DECOR keyword in the sub-part definition (DECOR sub-parts are not used in collision checking).

Parameters

a	The identifier of the link of the whose collision has to be checked.
b	The identifier of the obstacle whose collision has to be checked.
w	The world to update.

Definition at line 1703 of file world.c.

References Tworld::checkCollisionsLO, Error(), IsGroundLink, Tworld::stage, TRUE, and WORLD_IN_DEFINITION.

void NoCheckLinkObstacleCollision	(	unsigned int	a,
		unsigned int	b,
		Tworld *	w
	)

Desactivates the possible collision between a particular link and an object in the environment.

Parameters

a	The identifier of the link of the whose collision has not to be checked.
b	The identifier of the obstacle whose collision has not to be checked.
w	The world to update.

Definition at line 1712 of file world.c.

References Tworld::checkCollisionsLO, Error(), FALSE, Tworld::stage, and WORLD_IN_DEFINITION.

boolean AnyCollision ( Tworld * w )

Determines if any collision is activated and must be avoided.

Parameters

w	The world to query.

Returns: TRUE if there is at least one collision to avoid.

Definition at line 1721 of file world.c.

References Tworld::checkCollisionsLL, Tworld::checkCollisionsLO, FALSE, LinkCanCollide(), Tworld::nl, and Tworld::no.

Referenced by InitWorldCD(), on_cuikmove_save(), and PrintCollisions().

void PrintCollisions	(	FILE *	f,
		Tworld *	w
	)

Stores the collision information into a file in the format valid to be read by InitWorldFromFile.

Parameters

f	The file where to store the information.
w	The world with the collision information to print.

Definition at line 1733 of file world.c.

References AnyCollision(), Tworld::checkCollisionsLL, Tworld::checkCollisionsLO, Tworld::e, GetLinkName(), GetMechanismLink(), GetObstacleName(), Tworld::m, Tworld::nl, and Tworld::no.

Referenced by PrintWorld().

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. However,

In many cases we do not obtain a valid sample (the Newton process diverged).
The generated samples are not necessarily collision free.

Note that samples include values ONLY for the system variables (the kinematic subsystem has the same system variables than the global system). Thus, the boxe returned in the b_sol (and in the vector v) only include ranges (or values) for the system variables.

Parameters

p	A set of parameters.
v	Space where to store the solution as a point. This must be allocated by the caller.
b_sol	The solution as a punctual box. If the Newton diverged this will be a copy of the input box. This box is allocated inside the function.
w	The world from where to get the kinematic equation system.

Returns: TRUE if the Newton-Raphson process converged to a solution.

See Also: CuikNewton.

Definition at line 1762 of file world.c.

References CuikNewton(), DeleteBox(), Error(), GetBoxInterval(), GetCSSystemVars(), InitBox(), Tworld::kinCS, NEW, PrintBox(), SetBoxInterval(), Tworld::stage, and WORLD_DEFINED.

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

inline

Gets the system vars of the cuiksystem in the form of an array of booleans, one for each variable 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 1810 of file world.c.

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

Referenced by main(), and on_cuikmove_save().

unsigned int GetWorldVarTopology	(	unsigned int	vID,
		Tworld *	w
	)

inline

Determines the topology of a given variable.

Parameters

vID	The variable identifier.
w	The world to query.

Returns: The topology of the variable.

Definition at line 1818 of file world.c.

References GetCSVarTopology(), and Tworld::kinCS.

unsigned int GetWorldSimpVariableMask	(	Tparameters *	p,
		boolean **	sv,
		Tworld *	w
	)

Identifies pose related variable that survied in the simplified system.

Pose related variables are those giving the pose of a link (typically the system variables).

Here we are interested in identifying those variables that survive in the simplified system. This is a hack to be able to generate eigengrasp. Note that since is eigengrasp oriented, we do not consider the last link in the system (i.e., the manipulated object) but only the hand links.

Parameters

p	The set of parameters.
sv	A boolean array identifying the surviving variables.
w	The world to analyze.

Definition at line 1823 of file world.c.

References Error(), FALSE, GetCSNumVariables(), GetLinkPoseSimpVars(), GetMechanismLink(), Tworld::kinCS, Tworld::m, NEW, Tworld::nl, Tworld::stage, and WORLD_DEFINED.

Referenced by main().

void GetWorldVarNames	(	char **	vn,
		Tworld *	w
	)

inline

Returns an array with pointers to the variables names for the kinematic cuik system. This is used to print the variable names in the solution boxes so that

Parameters

vn	Array where to store the pointers to the variable names. The space for this array must be allocated/deallocated by the caller. The pointers themselves not.
w	The world to query.

Definition at line 1842 of file world.c.

References Error(), GetCSVariableNames(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

Referenced by main(), and on_cuikmove_save().

void GenerateWorldEquations	(	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 1850 of file world.c.

References AddEquationNoSimp(), AddMatrixEquation(), Tworld::branch2Link, COORD_EQ, CT_REPRESENTATION, DeleteEquation(), DeleteMEquation(), DeleteVariables(), Tworld::e, Error(), GenerateMEquationFromBranch(), GenerateTransEquationsFromBranch(), GetCSSystemVars(), GetCSVariables(), GetEnvironmentMaxCoordinate(), GetMechanismMaxCoordinate(), GetParameter(), InitEquations(), InitJacobian(), InitWorldCS(), InitWorldKinCS(), Tworld::kinCS, Tworld::m, Tworld::maxCoord, Tworld::nl, Tworld::nsvars, Tworld::nvars, Tworld::refEqs, Tworld::refEqsJ, REP_JOINTS, Tworld::stage, Tworld::systemVars, WORLD_DEFINED, WORLD_IN_DEFINITION, and WorldInitDOFInfo().

Referenced by GenerateWorldSingularityEquations(), and InitWorldFromMolecule().

void GenerateWorldSingularityEquations	(	Tparameters *	p,
		char *	ln,
		TCuikSystem *	cs,
		Tworld *	w
	)

Extends the kinematic subsystem with variables/equations to deal with singularities.

We re-defined the variables giving bounds for the joints so that introducing a singularity in the extreme of its range. Moreover, if requiered, we add equations giving the position of a link in the global frame.

Note that this only part of the process to generate a system of equations to detect singularities. The equations to detect the loss of rank of the Jacobian are to be added later. They are not added here to allow the user to select the variables for which the singularities are to be detected. This is to be done with a separate utility.

Parameters

p	A set of parameters.
ln	If we want to find position singularities, the name of the link link we want to focus on. If different from NULL, the equations giving the origin of the link in the global frame are added to the system.
cs	The resulting cuiksystem. The kinematic one from w plus the added information.
w	The world structure with the mechism to analyze.

Definition at line 1929 of file world.c.

References AddCt2Monomial(), AddEquation2CS(), AddMonomial(), AddVariable2CS(), AddVariable2Monomial(), CARTESIAN_VAR, COORD_EQ, CopyCuikSystem(), CopyEquation(), CT_REPRESENTATION, DeleteEquation(), DeleteMonomial(), DeleteVariable(), Error(), GenerateJointRangeSingularityEquations(), GenerateWorldEquations(), GetEquation(), GetLinkName(), GetMechanismJoint(), GetMechanismLink(), GetMechanismLinkID(), GetParameter(), InitMonomial(), Tworld::kinCS, LINK_TRANS, Tworld::m, Tworld::maxCoord, NEW, NewInterval(), NewVariable(), NFUN, Tworld::nj, NO_UINT, Tworld::refEqs, REP_JOINTS, ResetMonomial(), SetEquationType(), SetVariableInterval(), Tworld::stage, and WORLD_DEFINED.

Referenced by GenerateWorldTWSEquations(), and main().

void GenerateWorldTWSEquations	(	Tparameters *	p,
		char *	ln,
		TCuikSystem *	cs,
		Tworld *	w
	)

Extends the kinematic subsystem with variables/equations to detect translational workspace boundaries.

This is a composition of GenerateWorldSingularityEquations and AddJacobianEquations to detect a particular set of singularities.

Parameters

p	A set of parameters.
ln	The link whose translation we want to bound.
cs	The resulting cuiksystem. The kinematic one from w plus the added information.
w	The world structure with the mechism to analyze.

Definition at line 1994 of file world.c.

References AddSimplifiedJacobianEquations(), Error(), FALSE, GenerateWorldSingularityEquations(), GetCSNumVariables(), GetCSVariableID(), LINK_TRANS, NEW, NO_UINT, and TRUE.

Referenced by main().

void GetWorldJacobian	(	TJacobian *	J,
		Tworld *	w
	)

inline

Returns the symbolic Jacobian for the kinematic equations on a given point

The size of the Jacobian matrix is the number of equations of the kinematic system times the number of variables.

Parameters

J	The returned Jacobian.
w	The world to query.

See Also: GetCSJacobian.

Definition at line 2027 of file world.c.

References Error(), GetCSJacobian(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

void GetWorldSimpJacobian	(	Tparameters *	p,
		TJacobian *	J,
		Tworld *	w
	)

inline

Returns the symbolic Jacobian for the simplified kinematic equations on a given point

The size of the Jacobian matrix is the number of equations of the simplified kinematic system times the number of variables.

Parameters

p	The set of parameters.
J	The returned Jacobian matrix.
w	The world to query.

See Also: GetSimpCSJacobian.

Definition at line 2035 of file world.c.

References Error(), GetSimpCSJacobian(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

unsigned int GetWorldSimpTopology	(	Tparameters *	p,
		unsigned int **	t,
		Tworld *	w
	)

inline

Gets the topology of the variables in simplified kinematic equations.

Parameters

p	The set of parameters.
t	Space for the output array (one entry per variable).
w	The world to query.

Returns: The number of variables in the simplified kinematic system (i.e., the number of entries in the output array).

See Also: GetSimpCSTopology

Definition at line 2043 of file world.c.

References Error(), GetSimpCSTopology(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

unsigned int WorldSimpCuikNewton	(	Tparameters *	p,
		double *	pt,
		Tworld *	w
	)

inline

Tries to reach the kinematic manifold from a given point. This point in given in the simplified space (without un-used variables nor dummies).

Parameters

p	The set of parameters.
pt	The initial/output point (in the simplified space).
w	The world with the kinematic equations.

Returns: DIVERGED if we could not reach a solution point or CONVERGED_IN_GLOBAL if a solution point is found.

See Also: CuikNewtonSimp.

Definition at line 2052 of file world.c.

References CuikNewtonSimp(), and Tworld::kinCS.

void EvaluateWorldJacobian	(	double *	p,
		double ***	J,
		Tworld *	w
	)

inline

Returns the matrix resulting from evaluating the Jacobian for the kinematic equations on a given point

The size of the Jacobian matrix is the number of equations of the kinematic system times the number of variables.

Parameters

p	The point on which to evalutate the Jacobian.
J	The returned Jacobian matrix. This is allocated internally and must be deallocated externally.
w	The world to query.

See Also: EvaluateCSJacobian.

Definition at line 2057 of file world.c.

References Error(), EvaluateCSJacobian(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

void WorldEvaluateEquations	(	double *	pt,
		double *	r,
		Tworld *	w
	)

inline

Evaluates the set of kinematic equations on a given point.

Parameters

pt	The evaluation point.
r	The array where to store the evaluation for each equation (must be allocated externally).
w	The world with the kinematic equations.

See Also: EvaluateCSEquations.

Definition at line 2065 of file world.c.

References Error(), EvaluateCSEquations(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

boolean WorldInequalitiesHold	(	double *	pt,
		Tworld *	w
	)

inline

Checks if all the intequalities in the system hold for a given point.

Parameters

pt	The point.
w	The world to query.

Returns: TRUE if all inequalities hold.

Definition at line 2073 of file world.c.

References Error(), InequalitiesHoldOnPoint(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

boolean WorldSimpInequalitiesHold	(	Tparameters *	p,
		double *	pt,
		Tworld *	w
	)

inline

Checks if all the intequalities in the system hold for a given point for the simplified system.

Parameters

p	The set of parameters.
pt	The point in the simplified system.
w	The world to query.

Returns: TRUE if all inequalities hold for the simplified system.

Definition at line 2081 of file world.c.

References Error(), Tworld::kinCS, SimpInequalitiesHoldOnPoint(), Tworld::stage, and WORLD_DEFINED.

double WorldErrorInSimpInequalities	(	Tparameters *	p,
		double *	pt,
		Tworld *	w
	)

inline

Determines the maximum error in the inequalites for the simplified system.

Parameters

p	The set of parameters.
pt	The point in the simplified system.
w	The world to query.

Returns: The maximum error in the inequalities in the simplified system.

Definition at line 2089 of file world.c.

References Error(), ErrorInSimpInequalitiesOnPoint(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

void WorldEvaluateSimpEquations	(	Tparameters *	p,
		double *	pt,
		double *	r,
		Tworld *	w
	)

inline

Evaluates the simplified kinematic equations on a given point.

Parameters

p	The set of parameters.
pt	The evaluation point (in the simplified space).
r	The array where to store the evaluation for each equation (must be allocated externally).
w	The world with the kinematic equations.

See Also: EvaluateSimpCSEquations.

Definition at line 2097 of file world.c.

References Error(), EvaluateSimpCSEquations(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

void WorldEvaluateSubSetSimpEquations	(	Tparameters *	p,
		boolean *	se,
		double *	pt,
		double *	r,
		Tworld *	w
	)

inline

Evaluates a subset of the simplified kinematic equations on a given point.

Parameters

p	The set of parameters.
se	The array of booleans selecting the equations to evaluate.
pt	The evaluation point (in the simplified space).
r	The array where to store the evaluation for each equation (must be allocated externally).
w	The world with the kinematic equations.

See Also: EvaluateSimpCSEquations.

Definition at line 2105 of file world.c.

References Error(), EvaluateSubSetSimpCSEquations(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

double WorldErrorInEquations	(	double *	pt,
		Tworld *	w
	)

Evaluates the kinematic equations on a point and returns the norm of the error.

Parameters

pt	The point on which to evaluate the kinematic equations.
w	The world with the equations to evaluate.

Returns: The norm of the error for the given point.

See Also: ErrorInCSEquations.

Definition at line 2113 of file world.c.

References Error(), ErrorInCSEquations(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

Referenced by MoveWorld(), MoveWorldDOF(), and WorldErrorFromDOFs().

double WorldErrorInSimpEquations	(	Tparameters *	p,
		double *	pt,
		Tworld *	w
	)

inline

The same as WorldErrorInEquations but working on the simplified set of kinematic equations.

Parameters

p	The set of parameters.
pt	The point on which to evaluate the kinematic equations.
w	The world with the equations to evaluate.

Returns: The norm of the error for the given point.

See Also: ErrorInSimpCSEquations.

Definition at line 2121 of file world.c.

References Error(), ErrorInSimpCSEquations(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

double WorldErrorFromDOFs	(	Tparameters *	p,
		double *	dof,
		Tworld *	w
	)

Error in equation from the configuration given as degrees of freedom. If the internal representation is not DOF, the configuration is converted before computing the error.

Parameters

p	The set of parameters.
dof	The degrees of freedom given the configuration.
w	The world with the equations to evaluate.

Returns: The norm of the error for the given configuration.

Definition at line 2129 of file world.c.

References DeleteBox(), WorldDOF2Sol(), and WorldErrorInEquations().

Referenced by on_cuikmove_save().

double EvaluateWorldCost	(	Tparameters *	p,
		boolean	simp,
		double *	pt,
		void *	w
	)

inline

Evaluates the functions cost defined in a world in a given point. This is basically used by T-RRT (see also EvaluateCSCost).

Parameters

p	The set of parameters.
simp	TRUE if the solution point is given in the simplified sytem.
pt	The solution point
w	The cuiksystem with the equation to evaluate.

Definition at line 2142 of file world.c.

References Error(), EvaluateCSCost(), and WORLD_DEFINED.

unsigned int GetWorldNumVariables ( Tworld * w )

Return the number of variables in the kinematic subsystem (as defined and without any simplification).

Parameters

w	The world to query.

Returns: The number of variables in the kinematic subsystem.

Definition at line 2150 of file world.c.

References Error(), GetCSNumVariables(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

unsigned int GetWorldNumSystemVariables ( Tworld * w )

inline

Return the number of system variables in the kinematic subsystem (as defined and without any simplification).

Parameters

w	The world to query.

Returns: The number of variables in the kinematic subsystem.

Definition at line 2158 of file world.c.

References Error(), GetCSNumSystemVariables(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

Referenced by BioWorldConformationSize(), and main().

unsigned int RegenerateWorldOriginalPoint	(	Tparameters *	p,
		double *	s,
		double **	o,
		Tworld *	w
	)

inline

Reconstruct a point in original kinematic system from a point in the simplified system.

Parameters

p	The set of parameters.
s	The point in the simplified system.
o	The output on in the original system. Space allocated internally.
w	The world to query.

Returns: The size of the output array (number of variables in the original system).

See Also: RegenerateOriginalPoint.

Definition at line 2166 of file world.c.

References CT_REPRESENTATION, Error(), GetCSNumVariables(), GetParameter(), Tworld::kinCS, Tworld::m, NEW, RegenerateMechanismSolution(), RegenerateOriginalPoint(), REP_JOINTS, Tworld::stage, and WORLD_DEFINED.

unsigned int WorldGenerateSimplifiedPoint	(	Tparameters *	p,
		double *	o,
		double **	s,
		Tworld *	w
	)

inline

Reconstruct a point in the simplified kinematic system from a point in the original system.

Parameters

p	The set of parameters.
o	The point in the original system.
s	The output in the simplified system. Space allocated internally.
w	The world to query.

Returns: The size of the output array (number of variables in the simplified system).

See Also: GenerateSimplified Point.

Definition at line 2196 of file world.c.

References Error(), GenerateSimplifiedPoint(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

void GetWorldInitialBox	(	Tbox *	b,
		Tworld *	w
	)

inline

Gets the kinematic search space for a given problem. The search space is deduced from the ranges of the kinematic 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 2204 of file world.c.

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

void GetWorldSimpInitialBox	(	Tparameters *	p,
		Tbox *	b,
		Tworld *	w
	)

inline

The same GetWorldInitialBox

Parameters

p	The set of parameters.
b	The output box with the search space.
w	The world to query.

Definition at line 2212 of file world.c.

References Error(), GenerateSimpInitialBox(), Tworld::kinCS, Tworld::stage, and WORLD_DEFINED.

unsigned int MaxWorldReduction	(	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 2220 of file world.c.

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

void PlotWorld	(	Tparameters *	pr,
		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

pr	The set of 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 2260 of file world.c.

References Close3dBlock(), Tworld::e, Error(), InitWorldCD(), Tworld::m, PlotEnvironment(), PlotMechanism(), Tworld::stage, Start3dBlock(), Tworld::wcd, and WORLD_DEFINED.

Referenced by AnimateWorld(), and main().

unsigned int RegenerateWorldSolutionPoint	(	Tparameters *	pr,
		double *	p,
		double **	v,
		Tworld *	w
	)

Computes the non-system variable in a kinematic solution.

Parameters

pr	The set of parameters.
p	The input kinematc solution point with values for the system variables only.
v	The output kinematic solution point with values for all variables. The space for this point is allocated internally.
w	The world.

Returns: Then dimensionality of the output point.

Definition at line 2276 of file world.c.

References Error(), GetCSNumVariables(), Tworld::kinCS, Tworld::m, NEW, RegenerateMechanismSolution(), Tworld::stage, Tworld::systemVars, and WORLD_DEFINED.

Referenced by main().

void WorldPrintAtoms	(	Tparameters *	pr,
		FILE *	f,
		double *	pt,
		Tworld *	w
	)

Generates a file with the atom centers in global coordiantes. The global coordinates are those given by a solution point (parameter pt).

This is used only when the world represents a molecule and we want to store the atom (i.e., the sphere) positions into a file. (world where the mechanisms is formed by spheres only are considered molecules).

The atoms centers are latter used by cuikpdb2world that coordinates biological inforamation (pdb file) and a world. It is very important that the atoms in the world are given exactly in the same order as those in the pdb.

This function assumes that the enviroment is empty (i.e., the world is fully defined by the links).

NOTE: the file with the atom positions generated by this functions is only used to generate world from pdb files. This is somehow weird since to generate the atom files we already need a world file. We proceed in way since the generated world is "sync" with the pdb and then, the results fo cuikatlas, cuikatlascenters, ... can be incorporated to the pdb (to generate new pdb's that can be analyzed with any standard molecular analysis software).

Parameters

pr	The set of parameters.
f	The file where to store the atom centers.
pt	The solution point.
w	The world with the atoms.

Definition at line 2304 of file world.c.

References DeleteLinkTransforms(), Tworld::e, FALSE, GetEnvironmentNObstacles(), GetLinkTransformsFromSolutionPoint(), Tworld::m, MechanismPrintAtoms(), and Warning().

Referenced by main().

void WorldStoreRigidGroups	(	Tparameters *	pr,
		FILE *	f,
		double *	pt,
		Tworld *	w
	)

Generates files where the atoms (centers and radius) are stored grouped in rigid clusters. These files are not used within the CuikSuite, but only when testing the RigidCLL. We basically use the CuikSuite machinery to easily generate testing files for the RigidCLL package.

This function assumes that the enviroment is empty (i.e., the world is fully defined by the links). Moreover, for proper operation, all links must be composed by spheres (atoms).

Parameters

pr	The set of parameters.
f	The file where to store the atom centers.
pt	The solution point.
w	The world with the atoms.

Definition at line 2318 of file world.c.

References DeleteLinkTransforms(), Tworld::e, FALSE, GetEnvironmentNObstacles(), GetLinkTransformsFromSolutionPoint(), Tworld::m, MechanismStoreRigidAtoms(), and Warning().

Referenced by main().

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. In this context we assume that all balls in the mechanism represent an atom.

This is mainly used when manipulating worlds derived form biological information.

This Jacobian is used to transform the gradient of the energy in atom coordinates to a gradient in internal coordinates.

If the world is not only formed by spheres an error is triggered.

Parameters

pr	The set of parameters.
sol	The configuration.
nr	Number of rows of the output Jacobian.
nc	Number of columns of the output Jacobian.
J	The output matrix with the Jacobian.
w	The world.

Definition at line 2332 of file world.c.

References CT_REPRESENTATION, Error(), EvaluateJacobianXVectors(), GetLinkBody(), GetMechanismLink(), GetParameter(), GetPolyhedronCenter(), GetPolyhedronType(), IsMechanismAllSpheres(), LinkNBodies(), Tworld::m, NEW, Tworld::nl, Tworld::refEqsJ, REP_JOINTS, and SPHERE.

void MoveWorld	(	Tparameters *	pr,
		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

pr	The set of 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 2387 of file world.c.

References CT_REPRESENTATION, DeleteLinkTransforms(), Error(), GetBoxCenter(), GetBoxInterval(), GetBoxNIntervals(), GetCSNumSystemVariables(), GetCSNumVariables(), GetLinkTransformsFromSolution(), GetParameter(), IntervalCenter(), Tworld::kinCS, Tworld::m, MoveAndCheckCDFromTransforms(), MoveMechanismFromTransforms(), MoveWorldDOF(), NEW, RegenerateMechanismSolution(), REP_JOINTS, Tworld::stage, Tworld::systemVars, TRUE, Warning(), WORLD_DEFINED, and WorldErrorInEquations().

Referenced by AnimateWorld(), and main().

void PrintWorldAxes	(	Tparameters *	pr,
		FILE *	f,
		Tbox *	b,
		Tworld *	w
	)

Prints a point and the axis for each of the different joints in the mechanisms. This is latter used to generate the mechanism Jacobian.

The values are all in global coordinates.

Parameters

pr	The set of parameters.
f	The file where to store the information.
b	The box with the solution to use.
w	The world to query.

Definition at line 2455 of file world.c.

References CT_REPRESENTATION, Error(), EvaluateEqualityEquations(), FALSE, GetBoxInterval(), GetCSNumVariables(), GetMechanismJoint(), GetParameter(), IntervalCenter(), Tworld::kinCS, Tworld::m, NEW, Tworld::nj, Tworld::nl, PrintJointAxes(), Tworld::refEqs, RegenerateMechanismSolution(), REP_JOINTS, Tworld::stage, Tworld::systemVars, and WORLD_DEFINED.

Referenced by main().

void PrintWorldCollisionInfo	(	FILE *	f,
		char *	fname,
		Tworld *	w
	)

Prints information collected durint last collision check.

See StoreCollisionInfo for more details.

This function implicitly assumes that the last link is an abject being manipulated by the rest of links.

Parameters

f	File where to write the collision script.
fname	File from where we obtained the world information. The last part of the filename (from the last '_' to the end) is assumed to be the name of the object. This name is the one used to call the sub-matlab scripts giving contact points and normals on the object.
w	The world.

Definition at line 2508 of file world.c.

References Tworld::endEffector, Tworld::m, StoreCollisionInfo(), and Tworld::wcd.

Referenced by on_cuikmove_save().

void MoveWorldDOF	(	Tparameters *	pr,
		Tplot3d *	pt,
		double *	dof,
		Tworld *	w
	)

Moves a mechanisms to a configuration given by values for the different degrees of freedom of the joints forming the mechanisms. This might be a configuration not fulfilling the whole set of equations (loop closures might be violated).

This basically generates a configuration in our formulation from the degrees of freedom values and calls MoveWorld.

Parameters

pr	The set of parameters.
pt	The 3d scene where the world has been previously added (see PlotWorld).
dof	The array of doubles given the values for all the degrees of freedom in the mechanism.
w	The world with the mechanism to move.

Definition at line 2514 of file world.c.

References DeleteBox(), DeleteLinkTransforms(), Error(), GetLinkTransformsFromDOF(), Tworld::m, MoveAndCheckCDFromTransforms(), MoveMechanismFromTransforms(), PrintCollisionInfo(), Tworld::stage, TRUE, Tworld::wcd, WORLD_DEFINED, WorldDOF2Sol(), and WorldErrorInEquations().

Referenced by main(), and MoveWorld().

unsigned int WorldDOF2Sol	(	Tparameters *	p,
		double *	dof,
		double **	sol,
		Tbox *	b,
		Tworld *	w
	)

Defines a configuration in cuik variables from a configuration given by degrees of freedom.

There is no guarantee that the ouput box fulfill all equations (it is generated from a dof probably adjusted in open loop).

Parameters

p	The set of parameters.
dof	The values for each of the degrees of freedom of the mechanism.
sol	The re-constructed solution point (including system and non-system variables!!).
b	The solution point in form of a box.
w	The world with the mechanism description.

Returns: The number of elements in the solution.

Definition at line 2543 of file world.c.

References AXIS_H, CT_REPRESENTATION, DeleteLinkTransforms(), Tworld::dof2joint, Tworld::dof2range, Error(), GenerateJointSolution(), GenerateLinkSolution(), GetCSNumVariables(), GetCSVariableID(), GetJointRangeTopology(), GetLinkName(), GetLinkTransformsFromDOF(), GetMechanismJoint(), GetMechanismLink(), GetParameter(), HTransformGetElement(), INF, InitBoxFromPoint(), IsGroundLink, Tworld::joint2dof, JointFromID(), JointToID(), Tworld::kinCS, LINK_TRANS, Tworld::m, Tworld::ndof, NEW, Tworld::nj, Tworld::nl, NO_UINT, Tworld::openLink, PI2PI, REP_JOINTS, Tworld::stage, TOPOLOGY_S, and WORLD_DEFINED.

Referenced by GetSolutionPointFromLinkTransforms(), main(), MoveWorldDOF(), on_cuikmove_save(), and WorldErrorFromDOFs().

void WorldSample2DOF	(	Tparameters *	p,
		double *	sample,
		double *	dof,
		Tworld *	w
	)

Transform a configuration given in cuik variables to configuratoin given in degrees of freedom.

In some sense this is the inverse of WorldDOF2Sol. Note the difference in what here the sample must include only system variables.

Parameters

p	The set of parameters.
sample	The array with the sample to convert. This MUST include only system variables.
dof	Space where to store the ouput. It must have as many entries as dof of the mechanisms (see GetWorldNDOF).
w	the world with the mechanism description.

Definition at line 2625 of file world.c.

References CT_REPRESENTATION, DeleteLinkTransforms(), Error(), GetCSNumVariables(), GetLinkTransformsFromSolution(), GetMechanismDOFsFromTransforms(), GetParameter(), Tworld::kinCS, Tworld::m, Tworld::ndof, NEW, RegenerateMechanismSolution(), REP_JOINTS, Tworld::stage, Tworld::systemVars, and WORLD_DEFINED.

Referenced by main().

void AnimateWorld	(	Tparameters *	pr,
		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.

Parameters

pr	The set of 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 2677 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_DEFINED.

Referenced by main().

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

Generate a file with the cuiksystem 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 2721 of file world.c.

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

Referenced by main().

void PrintWorld	(	char *	fname,
		int	argc,
		char **	arg,
		Tworld *	w
	)

Prints the world in a form that can be read by InitWorldFromFile.

Parameters artc and arg can be used to add information in the world about how it was generated. Use 0 and NULL to add no information.

Parameters

fname	The file where to store the world information.
argc	Number of strings to be added to the world file header as comments.
arg	Strings to be added to the world file as comments. Right now, this is used to store in the command line used to create it.
w	The world to print.

Definition at line 2742 of file world.c.

References CreateFileName(), DeleteFileName(), Tworld::e, Error(), GetFileExtension(), GetFileFullName(), GetFileName(), GetFilePath(), Tworld::m, PrintCollisions(), PrintEnvironment(), PrintMechanism(), and WORLD_EXT.

Referenced by main(), and PrintBioWorld().

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 2792 of file world.c.

References DeleteEnvironment(), DeleteMechanism(), DeleteWorldCD(), DeleteWorldCollisionInfo(), DeleteWorldCS(), Tworld::e, Tworld::m, Tworld::stage, and WORLD_DEFINED.

Referenced by DeleteBioWorld(), and main().

Institut de Robòtica i Informàtica Industrial

Detailed Description

Functions

Function Documentation

Institut de Robòtica
i Informàtica Industrial