link.h File Reference

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

#include "vector.h"
#include "interval.h"
#include "equation.h"
#include "cuiksystem.h"
#include "box.h"
#include "cpolyhedron.h"
#include "plot3d.h"
#include "color.h"
#include "htransform.h"
#include <stdlib.h>

Go to the source code of this file.

Data Structures
struct	Tlink
	Information about each rigid part of a mechanisms. More...
Defines
#define	ROT_REP   1
	Type of parametrization of the rotaion matrices.
#define	ROT_REDUNDANCY   0
	Redundancy in the generated equations.
#define	INIT_NUM_SHAPES   2
	Initial room for shapes in a link.
#define	DLC_R   0.5
	Red component of the default color for links.
#define	DLC_G   0.5
	Green component of the default color for links.
#define	DLC_B   0
	Blue component of the default color for links.
#define	CC_R   1
	Red component of the color for links in collision.
#define	CC_G   0
	Green component of the color for links in collision.
#define	CC_B   0
	Blue component of the color for links in collision.
#define	IsGroundLink(id)   (id==0)
	A test that return TRUE if the given identifier is that of the ground link.
Functions
void	InitLink (char *name, Tlink *l)
	Constructor.
void	CopyLink (Tlink *l_dst, Tlink *l_src)
	Copy constructor.
void	AddBody2Link (Tcpolyhedron *b, Tlink *l)
	Adds a body to the link.
void	ChangeLinkReferenceFrame (double **p1, double **p2, Tlink *l)
	Sets a new reference frame for the link.
unsigned int	LinkNBodies (Tlink *l)
	Gets the number of convex parts of the link.
Tcpolyhedron *	GetLinkBody (unsigned int i, Tlink *l)
	Gets one of the convex parts of the link.
char *	GetLinkName (Tlink *l)
	Gets the name of a link.
boolean	IsAllSpheres (Tlink *l)
	Identifies links formed only by spheres.
void	GenerateLinkRot (Tparameters *p, unsigned int lID, TCuikSystem *cs, Tlink *l)
	Generate the variables and constraints related with the rotation of the link reference frame.
void	ApplyLinkRot (double sf, unsigned int sv, double *p, Tequation *eq, TCuikSystem *cs, boolean groundLink, Tlink *l)
	Transforms a vector from the local reference frame to the global one and accumulates the resulting expression into the given equations.
void	RegenerateLinkSolution (TCuikSystem *cs, double *sol, boolean groundLink, Tlink *l)
	Computes the values for the dummy variables used to represent the rotation matrices for a given link.
void	GetTransform2Link (TCuikSystem *cs, double *sol, boolean groundLink, double *r, THTransform *t, Tlink *l)
	Gets the homogeneous transform associated with a link for a given solution point.
double	GetLinkMaxCoordinate (Tlink *l)
	Returns the sum of the maximum coordinate value for all the convex polyhedrons in the link.
void	PlotLink (Tplot3d *pt, double axesLength, Tlink *l)
	Adds a link to a 3d scene.
void	MoveLink (Tplot3d *pt, TCuikSystem *cs, double *sol, double *r, Tlink *l)
	Displaces a link in a 3d scene.
void	DeleteLink (Tlink *l)
	Destructor.

---

Detailed Description

Definition of the Tlink type and the associated functions.

See also:

Tlink, link.c.

Definition in file link.h.

---

Define Documentation

#define ROT_REP   1

Rotation matrices can be parametrized in different ways. The key issue is that all entries in the rotation matrix should be linear so that, at most, the resulting expressions of multiplying vectors by matrices result in, at most, bilinear expressions.

The possible values for ROT_REP are

• 0 Using three vectors, that is, 9 parameters (ux,uy,uz) (vy,vy,vz) (wx,wy,wz) such that ||u||=||v||=||w||=1, u v =0, and u X v = w.
• 1 Using two vectors, u and v, and replacing all the occurences of vector elements of vector w by the expressions resulting from the vector product u X v = w. Since these expressions are bilinear, we use dummy variables to convert them in linear expression.
• 2 Using quaternions. The rotation matrix is
  
  with , , and .
  

Different representations produce different number of system/dummy variables and different number of system and dummy equations.

The change in the representation affects GenerateLinkRot, ApplyLinkRot and GetTransform2Link.

The default value for this constant is set to 1 since this is the representation type that, in genearal, give simpler sets of equations. Value 0 is used for debugging purposes, and value 2 result in too complex systems of equations.

Note that by using quaternions it is not possible (yet) to change the internal reference frame of the link using ChangeLinkReferenceFrame.

In general the best options to use are 0 and 1, while 2 (quaternions) was implemented only for experimental purposes.... an non successfull experiment, though.

Definition at line 74 of file link.h.

#define ROT_REDUNDANCY   0

When ROT_REP is 0, we can generate some redundant equations regarding the orthonormality of the three coordinates axis. By setting this constant to 0 these redundancy is avoided.

Definition at line 84 of file link.h.

Referenced by GenerateLinkRot().

#define INIT_NUM_SHAPES   2

Initial room for shapes in a link. Each shape is a convex polyhedron. It will be enlarged if necessary.

See also:

Tlink, Tcpolyhedron.

Definition at line 94 of file link.h.

Referenced by CopyLink(), and InitLink().

#define DLC_R   0.5

Red component of the default color for links.

See also:

Tplot3d

Definition at line 102 of file link.h.

#define DLC_G   0.5

Green component of the default color for links.

See also:

Tplot3d

Definition at line 110 of file link.h.

#define DLC_B   0

Blue component of the default color for links.

See also:

Tplot3d

Definition at line 118 of file link.h.

#define CC_R   1

Red component of the color for links in collision.

See also:

Tplot3d

Definition at line 127 of file link.h.

#define CC_G   0

Green component of the color for links in collision.

See also:

Tplot3d

Definition at line 135 of file link.h.

#define CC_B   0

Blue component of the color for links in collision.

See also:

Tplot3d

Definition at line 143 of file link.h.

#define IsGroundLink

(

id

)

(id==0)

A test that return TRUE if the given identifier is the one correcponding to the ground link.

Definition at line 152 of file link.h.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GenerateLinkRot(), MoveJoint(), and RegenerateMechanismSolution().

---

Function Documentation

void InitLink	(	char *	name,
		Tlink *	l
	)

Initializes a link structure with no bodies.

Parameters:

	name	The name to give to the link.
	l	The links to initialize.

Definition at line 16 of file link.c.

References Tlink::axisID, Tlink::bodies, Tlink::c, CopyVoidPtr(), DeleteVoidPtr(), HTransformIdentity(), INIT_NUM_SHAPES, InitVector(), Tlink::maxCoord, Tlink::name, NEW, NO_UINT, Tlink::R, and Tlink::s.

Here is the call graph for this function:

void CopyLink	(	Tlink *	l_dst,
		Tlink *	l_src
	)

Initializes a link copying the information from another link.

Parameters:

	l_dst	The link to initialize.
	l_src	The link from where to copy.

Definition at line 42 of file link.c.

References AddBody2Link(), Tlink::axisID, Tlink::bodies, Tlink::c, CopyVoidPtr(), DeleteVoidPtr(), GetLinkBody(), HTransformCopy(), INIT_NUM_SHAPES, InitVector(), LinkNBodies(), Tlink::maxCoord, Tlink::name, NEW, Tlink::R, and Tlink::s.

Referenced by AddLink2Mechanism().

Here is the call graph for this function:

Here is the caller graph for this function:

void AddBody2Link	(	Tcpolyhedron *	b,
		Tlink *	l
	)

Adds a body to the link.

Parameters:

	b	The convex polyhedron to add to the link
	l	The link where to add the new polyhedron.

Definition at line 70 of file link.c.

References Tlink::bodies, CopyCPolyhedron(), GetCPolyhedronMaxCoordinate(), Tlink::maxCoord, NEW, and NewVectorElement().

Referenced by AddBody2Mechanism(), and CopyLink().

Here is the call graph for this function:

Here is the caller graph for this function:

void ChangeLinkReferenceFrame	(	double **	p1,
		double **	p2,
		Tlink *	l
	)

In order to get the simplest possible set of solutions we can select a reference frame for the link different from that used to defined the bodies. This internal reference frame is defined from two (not necessarily orthogonal) vectors. Therefore, we store the sin and cos between the two vectors defining the internal reference frame and the rotation from the frame where bodies are defined and the internal reference frame.

This function does not change the points defining the bodies. (they remain in the original frame), but it changes the way they are interpreted changing the workings of functions GenerateLinkRot and ApplyLinkRot.

If the two given vectors are (almost) aligned, the internal reference frame is not changed.

When using a rotation representation based on quaternions it is not possible to change the internal link reference frame.

Parameters:

	p1	Two 3d points defining the first vector for the new internal reference frame.
	p2	Two 3d points defining the second vector for the new internal reference frame.
	l	The link whose reference frame we want to change.

Definition at line 82 of file link.c.

References Tlink::c, HTransformSetElement(), Tlink::R, and Tlink::s.

Referenced by InitWorldKinCS().

Here is the call graph for this function:

Here is the caller graph for this function:

unsigned int LinkNBodies

(

Tlink *

l

)

Gets the number of convex parts of the link.

Parameters:

l

The link to query.

Returns:

The number of convex parts of the link.

Definition at line 148 of file link.c.

References Tlink::bodies, and VectorSize().

Referenced by AddLink2World(), CopyLink(), DeleteLink(), MoveLink(), and PlotLink().

Here is the call graph for this function:

Here is the caller graph for this function:

Tcpolyhedron* GetLinkBody	(	unsigned int	i,
		Tlink *	l
	)

Gets one of the convex parts of the link.

Parameters:

	i	The identifier of the convex part to retrive. The identifier of a part is given by the order at which they are added to the link.
	l	The link to query.

Returns:

A pointer to the requested convex part or NULL if the link does not have any part with the given identifier.

Definition at line 153 of file link.c.

References Tlink::bodies, and GetVectorElement().

Referenced by CopyLink(), DeleteLink(), GetMechanismDefiningPoint(), MoveLink(), and PlotLink().

Here is the call graph for this function:

Here is the caller graph for this function:

char* GetLinkName

(

Tlink *

l

)

Gets the name of a link.

Parameters:

l

The link to query.

Returns:

A pointer to a string with the name of the link.

Definition at line 164 of file link.c.

References Tlink::name.

Referenced by GenerateEquationsFromBranch(), GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetLinkID(), and RegenerateJointSolution().

Here is the caller graph for this function:

boolean IsAllSpheres

(

Tlink *

l

)

Returns TRUE if all non-DECOR bodies in the link (i.e., bodies that are considered in the collision checking) are spheres.

This function is used when generating the collision avoidance related equations. If all involved bodies are spheres we do not use separating planes but we directly bound the squared distance between the sphere centers to be larger than the squared sum of radii.

Parameters:

l

The link to query.

Returns:

TRUE if all non-DECOR bodies in the link are spheres.

void GenerateLinkRot	(	Tparameters *	p,
		unsigned int	lID,
		TCuikSystem *	cs,
		Tlink *	l
	)

Each link has a 3D reference frame formed by three vectors (in general orthonormal vectors unless ChangeLinkReferenceFrame is used). This functions add the variales representing the rotation of this reference frame with respect to the gobal reference frame. The translation from this global reference frame to the origin of the frame attached to the link is given by equations involving only rotation variables (and fixed vectors) and can only be computed at the level where we have together links (mechanisms) and cuiksystems, that is, at the world.h level.

Parameters:

	p	A set of parameters. They include, for instance the dummification level.
	lID	The identifier of the link in the mechanism. Used to generate unique names for the new variables.
	cs	The cuiksystem where to add the variables and equations.
	l	The link from where to generate the variables and equation.

Definition at line 170 of file link.c.

References AddEquation2CS(), AddVariable2CS(), Tlink::c, DeleteEquation(), DeleteVariable(), GenerateCrossProductEquations(), GenerateDotProductEquation(), GenerateNormEquation(), GetCSVariableID(), IsGroundLink, LINK_ROT, Tlink::name, NEW, NewInterval(), NewVariable(), NO_UINT, ROT_REDUNDANCY, Tlink::s, SetVariableInterval(), and SYSTEM_VAR.

Referenced by InitWorldKinCS().

Here is the call graph for this function:

Here is the caller graph for this function:

void ApplyLinkRot	(	double	sf,
		unsigned int	sv,
		double *	p,
		Tequation *	eq,
		TCuikSystem *	cs,
		boolean	groundLink,
		Tlink *	l
	)

Apply the changes of basis given by the link reference frame to a vector. This results in an expression that transforms the vector from the local reference frame to the global one. These expressions are added to the given equations (one expression for X, another for Y, and another for Z).

Parameters:

	sf	Constant scale factor to apply to the input vector.
	sv	Variable scale factor given as the identifier of the range in the given box whose center have to be used as scale factor. If no variable scale factor is to be used this parameter should be NO_UINT.
	p	The 3D vector in the link reference frame that we want to transform to the global one and to add to trans.
	eq	Set of 3 equations where to add the 3 expressions resulting from this function (one for the X component of the vector in the globla frame, another for the Y expression and, finally the expression for the Z component).
	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers. The numerical identifier of the variable is used to generate a unique name for it.
	groundLink	TRUE if the given link is the ground link.
	l	The link to use to rotate the given input vector p.

Definition at line 285 of file link.c.

References AddCt2Monomial(), AddMonomial(), AddVariable2Monomial(), DeleteMonomial(), Error(), GetCSVariableID(), HTransformApply(), InitMonomial(), LINK_ROT, Tlink::name, NEW, NO_UINT, Tlink::R, ResetMonomial(), and ZERO.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), and GenerateJointRangeEquations().

Here is the call graph for this function:

Here is the caller graph for this function:

void RegenerateLinkSolution	(	TCuikSystem *	cs,
		double *	sol,
		boolean	groundLink,
		Tlink *	l
	)

Solution points only include values for the system (and secondary) variables. However, in some formulations, the frame of reference for each link is represented using not only system variables, but dummy variable too. This function computes the values for the link-related dummy variables form the system ones for a given link.

Parameters:

	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers, i.e., the number of range of the box to use to instantiate each variable.
	sol	The solution point with the values for the system variables. At the end of the function, this vector also has values for the link-related dummy variables.
	groundLink	TRUE if the given link is the ground link. The rotation matrix for the ground link is fixed (the identity) and, its rotation matrix generates no variables (nor system nor dummy).
	l	The link.

Definition at line 351 of file link.c.

Referenced by RegenerateMechanismSolution().

Here is the caller graph for this function:

void GetTransform2Link	(	TCuikSystem *	cs,
		double *	sol,
		boolean	groundLink,
		double *	r,
		THTransform *	t,
		Tlink *	l
	)

Returns the homogeneous transform with the position and orientation of the link for a given solution point.

Parameters:

	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers, i.e., the number of range of the box to use to instantiate each variable.
	sol	The solution point.
	groundLink	TRUE if the given link is the ground link.
	r	3d vector with the translation from the ground link to this link. The variables for these translations are removed from the cuiksystems and computed from rotations and fixed vectors. Consequently they can only be computed at the level where we have cuiksystems and links (i.e. in world.h)
	t	The returned homogeneous transform.
	l	The link whose reference frame we want to retrive.

Definition at line 358 of file link.c.

References AXIS_H, Error(), GetCSVariableID(), HTransformIdentity(), HTransformOrthonormalize(), HTransformProduct(), HTransformSetElement(), LINK_ROT, Tlink::name, NEW, NO_UINT, and Tlink::R.

Referenced by MoveJoint(), and MoveLink().

Here is the call graph for this function:

Here is the caller graph for this function:

double GetLinkMaxCoordinate

(

Tlink *

l

)

Returns the sum of the maximum coordinate value (either for X,Y or Z) for all the convex polyhedrons in the link. This is used in higher levels to define an over-estimate bounding-box of the world. This bounding box is used to define the ranges for some of the coordinate and separating planes variables.

Parameters:

l

The link to query.

Returns:

The sum of the maximum coordinate values for all bodies in the link.

Definition at line 975 of file link.c.

References Tlink::maxCoord.

Referenced by AddBody2Mechanism(), and AddLink2Mechanism().

Here is the caller graph for this function:

void PlotLink	(	Tplot3d *	pt,
		double	axesLength,
		Tlink *	l
	)

Adds a link to a 3d scene. It adds the diffent parts of the link as if they are expressed in the global frame. To displace the use MoveLink.

Parameters:

	pt	The 3d geometry where to add the link.
	axesLength	>0 to display the link axes. The value is the length of the lines representing the axes. The reference axes for the link are represeted as red for X, green for Y and blue for Z.
	l	The link to add to the scene.

Definition at line 980 of file link.c.

References Tlink::axisID, Close3dObject(), DeleteColor(), GetLinkBody(), LinkNBodies(), NewColor(), NO_UINT, PlotCPolyhedron(), PlotVect3d(), and StartNew3dObject().

Referenced by PlotMechanism().

Here is the call graph for this function:

Here is the caller graph for this function:

void MoveLink	(	Tplot3d *	pt,
		TCuikSystem *	cs,
		double *	sol,
		double *	r,
		Tlink *	l
	)

Displaces a link previously added to a 3d scene.

The parameters for the rotation to apply are taken from the central points of interval of the the given box corresponding to the variables expressing the reference frame for the link.
The parameters for the translation are externally given.

This function triggers an error (actually the error is triggered in GetTransform2Link) if applied to the groundlink. Note that the groundLink is not suposed to move.

Parameters:

	pt	The 3d scene where the apply the displacement.
	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers, i.e., the number of range of the box to use to instantiate each variable.
	sol	The solution point. Only values for system variables are used.
	r	A 3d vector with the translation from the ground link to the reference frame of the link.
	l	The link to move.

Definition at line 1019 of file link.c.

References Tlink::axisID, FALSE, GetLinkBody(), GetTransform2Link(), HTransformDelete(), LinkNBodies(), Move3dObject(), MoveCPolyhedron(), and NO_UINT.

Referenced by MoveMechanism().

Here is the call graph for this function:

Here is the caller graph for this function:

void DeleteLink

(

Tlink *

l

)

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

Parameters:

l

The link to delete.

Definition at line 1055 of file link.c.

References Tlink::bodies, DeleteCPolyhedron(), DeleteVector(), GetLinkBody(), HTransformDelete(), LinkNBodies(), Tlink::name, and Tlink::R.

Referenced by DeleteMechanism().

Here is the call graph for this function:

Here is the caller graph for this function: