joint.c File Reference

Detailed Description

Implementaton of the function operating on Tjoint.

See Also

Tjoint, joint.h.

Definition in file joint.c.

Functions
void	InitJoint (unsigned int t, unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
	Initializes all fields for a joint with dummy values. More...
void	NewFreeJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
	Constructor. More...
void	NewFixJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, THTransform *t, Tjoint *j)
	Constructor. More...
void	NewRevoluteJoint (unsigned int id, unsigned int r, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, boolean hasLimits, Tinterval *range, double **rPoints, boolean avoidLimits, double avoidLimitsWeight, Tjoint *coupled, Tjoint *j)
	Constructor. More...
void	NewUniversalJoint (unsigned int id, unsigned int r, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, boolean hasLimits, Tinterval *range1, Tinterval *range2, double **rPoints, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
	Constructor. More...
void	NewSphericalJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, boolean hasLimits, double range, double **rPoints, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
	Constructor. More...
void	NewPrismaticJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tinterval *range, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
	Constructor. More...
void	NewSphSphLowJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
	Constructor. More...
void	NewSphSphUpJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
	Constructor. More...
void	NewRevLowJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
	Constructor. More...
void	NewRevUpJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tjoint *j)
	Constructor. More...
void	NewInPatchJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, double **patch, boolean avoidLimits, double avoidLimitsWeight, Tjoint *j)
	Constructor. More...
void	CopyJoint (Tjoint *j_dst, Tjoint *j_src)
	Copy constructor. More...
unsigned int	GetJointType (Tjoint *j)
	Gets the joint type. More...
double	GetJointMaxCoordinate (Tjoint *j)
	Gets the maximum coordinate. More...
unsigned int	GetJointID (Tjoint *j)
	Gets the joint identifier. More...
unsigned int	JointFromID (Tjoint *j)
	Gets the identifier of the first link involved in the joint. More...
Tlink *	JointFrom (Tjoint *j)
	Gets a pointer to the first link involved in the joint. More...
unsigned int	JointToID (Tjoint *j)
	Gets the identifier of the second link involved in the joint. More...
Tlink *	JointTo (Tjoint *j)
	Gets a pointer to the second link involved in the joint. More...
void	GetJointName (char **name, Tjoint *j)
	Returns a string identifying the joint. More...
void	GetJointDOFName (unsigned int ndof, char **name, Tjoint *j)
	Label for a specific dof of a joint. More...
void	GetJointPoint (unsigned int link, unsigned int point, double *p, Tjoint *j)
	Gets one of the points defining the rotation/sliding axis for the joint. More...
boolean	LimitedJoint (Tjoint *j)
	Checks if a joint has limits. More...
Tinterval *	GetJointRange (Tjoint *j)
	Checks the limits of a joint. More...
Tinterval *	GetJointSecondRange (Tjoint *j)
	Checks the second limit of a universal joint. More...
void	GetJointRangeN (unsigned int nr, double mt, Tinterval *r, Tjoint *j)
	Returns one of the ranges of the joint. More...
unsigned int	GetJointRangeTopology (unsigned int nr, Tjoint *j)
	Returns the topology of one of the ranges of the joint. More...
unsigned int	CoupledWith (Tjoint *j)
	Returns the identifier of the joint coupled with the query joint. More...
unsigned int	GetJointDOF (Tjoint *j)
	Computes the degrees of freedom allowed by a given joint. More...
void	GenerateJointRangeSingularityEquations (Tparameters *p, TCuikSystem *cs, Tjoint *j)
	Modifies the cuik system to detect end range singularities. More...
void	GenerateJointRangeEquations (Tparameters *p, TCuikSystem *cs, Tjoint *j)
	Generate the constraints related with the joint limits. More...
void	GenerateJointEquationsInBranch (Tparameters *p, double s, TCuikSystem *cs, Tequation *eq, Tjoint *j)
	Generate the constraints of a joint in a sequence. More...
void	JointForceEquation (Tparameters *pr, unsigned int linkID, TCuikSystem *cs, Tequation *eq, Tjoint *j)
	Add the force terms derived from a given joint. More...
void	GenerateJointEquations (Tparameters *p, double maxCoord, TCuikSystem *cs, Tjoint *j)
	Generate the constraints related with the joint. More...
void	RegenerateJointSolution (Tparameters *p, TCuikSystem *cs, double *sol, Tjoint *j)
	Computes the values for the dummy variables used in the joint equations. More...
void	RegenerateJointBox (Tparameters *p, TCuikSystem *cs, Tbox *b, Tjoint *j)
	Computes the values for the dummy variables used in the joint equations. More...
void	GenerateJointSolution (Tparameters *p, double *dof, THTransform *t1, THTransform *t2, TCuikSystem *cs, double *sol, Tjoint *j)
	Generates a solution point from degrees of freedom. More...
void	GetJointTransform (double *dof, THTransform *t, Tjoint *j)
	Computes the transform induced by the joint. More...
void	GetJointDOFValues (Tparameters *p, THTransform *t1, THTransform *t2, double *dof, Tjoint *j)
	Recovers the joint DOFs from the absolute poses of the links. More...
void	GetJointTransSeq (Tparameters *p, TCuikSystem *cs, TTransSeq *ts, Tjoint *j)
	Build the sequence of transforms passing through the joint. More...
void	PrintJointAxes (Tparameters *p, FILE *f, TCuikSystem *cs, double *sol, double *r, Tjoint *j)
	Prints the joint axis in world coordinates. More...
void	PrintJoint (FILE *f, Tjoint *j)
	Stores the joint information into a file. More...
void	DeleteJoint (Tjoint *j)
	Destructor. More...

Function Documentation

void InitJoint	(	unsigned int	t,
		unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		Tjoint *	j
	)

Initializes all fields for a joint with dummy/defalult values. In this way the functions defining new joints only have to worry about setting the right values in the right.

Using this set up functions defining new joints MUST call this function at the beginning of the definition of the new joint.

Parameters

t	Type of joint.
id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
j	The joint to initialize

Definition at line 42 of file joint.c.

References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::coupled, Error(), FALSE, Tjoint::hasLimits, HTransformIdentity(), Tjoint::id, INF, Tjoint::link, Tjoint::linkID, Tjoint::maxCoord, NewInterval(), Tjoint::normals, Tjoint::normRange, Tjoint::offset, Tjoint::offset2, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::range, Tjoint::range2, and Tjoint::t.

Referenced by NewFixJoint(), NewFreeJoint(), NewInPatchJoint(), NewPrismaticJoint(), NewRevoluteJoint(), NewSphericalJoint(), and NewUniversalJoint().

void NewFreeJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		Tjoint *	j
	)

Defines a free joint between links. A free joint is a dummy joint imposing no constraint between the relative movements of the links. It is used to see a different free flying mechanisms as a single mechanism. This basically simplify the structures we use (we don't have to implement a multi-mechanism stucture, for instance) and the generation of equations from the mechanism definition can be easily implemented with a single ground link.

Up to now we hardly used/tested this type of joints.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
j	The joint to initialize.

Definition at line 93 of file joint.c.

References FREE_JOINT, INF, InitJoint(), and Tjoint::maxCoord.

Referenced by GenerateKinTree(), and main().

void NewFixJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		THTransform *	t,
		Tjoint *	j
	)

Defines a fix joint between links. A fix joint fixes the relative translation and orientation between two links.

Fix joints are typically used when placing the end effector of a robot to compute its inverse kinematics.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
t	Homogeneous transform defining the relative position between the two links.
j	The joint to initialize.

Definition at line 102 of file joint.c.

References FIX_JOINT, HTransformCopy(), HTransformGetElement(), InitJoint(), Tjoint::maxCoord, Norm(), Tjoint::points, and Tjoint::preT.

Referenced by main().

void NewRevoluteJoint	(	unsigned int	id,
		unsigned int	r,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		boolean	hasLimits,
		Tinterval *	range,
		double **	rPoints,
		boolean	avoidLimits,
		double	avoidLimitsWeight,
		Tjoint *	coupled,
		Tjoint *	j
	)

Defines a new revolute joint between two links.

Note that we assume centered rotations, that is, if there is any limit in the rotation it is always in the for of a symmetric interval [-range,range]. This is completely general (by defining appropriate reference vectors the rotation range can be made symmetric around 0 and it helps when defining constraints.

Parameters

id	Unique identifier given to the joint.
r	Type of represention used. Value for CT_REPRESENTATION.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Four points defining the rotation axis. The first two points define the rotation axis in the reference frame of link1 and the two other points are the same vector in the frame of reference of link2.
hasLimits	TRUE if the joint to create include limits for the rotation movement.
range	If hasLimits is TRUE, this is the valid range for the rotation (in radiants).
rPoints	If hasLimits is TRUE, two points that define the reference position (i.e., the zero position) for the rotation. Actually points[0]->rPoints[0] define one vector in link1 and points[2]->rPoints[1] another vector in link2 that must coincide when the angle is 0. In other works, the angle between these vectors is the angle between the links and we only allow it to move from -range, to range. The vector points[0]->rPoints[0] must be orthogonal to points[0]->points[1] and the same applies to points[2]->rPoints[1] and points[2]->points[3].
avoidLimits	TRUE if the search process has to be focussed in the center of the valid ranges for the joint. This flag in only considered if hasLimits is true.
avoidLimitsWeight	If avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
coupled	Pointer for the joint coupled with this one. This is only not NULL for some revolute joints coupled with the previous joint (revolute too and both joints with limits).
j	The joint to initialize.

Definition at line 124 of file joint.c.

References ADJUST_REAL, Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, CopyInterval(), Tjoint::coupled, CrossProduct(), DefineNormalVector(), Error(), FALSE, Tjoint::hasLimits, HTransformApply(), HTransformDelete(), HTransformFromVectors(), HTransformInverse(), HTransformProduct(), HTransformRx(), HTransformX2Vect(), InitJoint(), IntervalCenter(), IntervalOffset(), IntervalSize(), Tjoint::maxCoord, Norm(), Tjoint::normals, Tjoint::offset, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::range, REP_JOINTS, REV_JOINT, Tjoint::t, Tjoint::vrange, and ZERO.

Referenced by InitWorldFromMolecule(), main(), NewRevLowJoint(), and NewRevUpJoint().

void NewUniversalJoint	(	unsigned int	id,
		unsigned int	r,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		boolean	hasLimits,
		Tinterval *	range1,
		Tinterval *	range2,
		double **	rPoints,
		boolean	avoidLimits,
		double	avoidLimitsWeight,
		Tjoint *	j
	)

Defines a new universal joint between two links.

Parameters

id	Unique identifier given to the joint.
r	Type of represention used. Value for CT_REPRESENTATION.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Four points defining the two rotation axes. The first two points define the rotation axis in the reference frame of link1 and the second pair of points point is the same point in the frame of reference of link2. The constraint imposed by the universal joint forces these two vectors to be orthogonal when
hasLimits	TRUE if the joint to create include limits.
range1	If hasLimits is TRUE, this is the valid range for the first rotation (in radiants).
range2	If hasLimits is TRUE, this is the valid range for the second rotation (in radiants).
rPoints	points[0]->rPoints[0] define the vector in link1 that must coincide with vector points[2]->points[3] when the first rotation is zero. points[2]->rPoints[1] define the vector in link2 that must coincide with vector points[0]->points[1] when the second rotation is zero.
avoidLimits	TRUE if the search process has to be focussed in the center of the valid ranges for the joint. This flag in only considered if hasLimits is true.
avoidLimitsWeight	If avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
j	The joint to initialize.

Definition at line 280 of file joint.c.

References ADJUST_REAL, Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, CopyInterval(), CrossProduct(), DefineNormalVector(), Error(), FALSE, Tjoint::hasLimits, HTransformApply(), HTransformDelete(), HTransformFromVectors(), HTransformInverse(), HTransformProduct(), HTransformRx(), HTransformX2Vect(), InitJoint(), IntervalCenter(), IntervalOffset(), IntervalSize(), Tjoint::maxCoord, Norm(), Tjoint::normals, Tjoint::offset, Tjoint::offset2, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::range, Tjoint::range2, REP_JOINTS, UNV_JOINT, Tjoint::vrange, and ZERO.

void NewSphericalJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		boolean	hasLimits,
		double	range,
		double **	rPoints,
		boolean	avoidLimits,
		double	avoidLimitsWeight,
		Tjoint *	j
	)

Defines a new spherical joint between two links.

The limits for a spherical joint are defined symmetrically around an axis rigidly linked to the first link.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Two points defining the rotation point. The first point define the rotation point in the reference frame of link1 and the second point is the same point in the frame of reference of link2.
hasLimits	TRUE if the joint to create include limits.
range	If hasLimits is TRUE, the valid value of rotation angle between the two links is [-range,range] .
rPoints	points[0]->rPoints[0] and points[2]->rPoints[1] define the vectors in link1 and link2 respectively that must coincide when the angle between links is 0. In other works, the angle between these vectors is the angle between the links and we only allow it to move from -range, to range.
avoidLimits	TRUE if the search process has to be focussed in the center of the valid ranges for the joint. This flag in only considered if hasLimits is true.
avoidLimitsWeight	If avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
j	The joint to initialize.

Definition at line 444 of file joint.c.

References ADJUST_REAL, Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Error(), FALSE, Tjoint::hasLimits, HTransformInverse(), HTransformProduct(), HTransformTxyz(), HTransformX2Vect(), InitJoint(), IntervalSize(), Tjoint::maxCoord, NewInterval(), Norm(), Tjoint::offset, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::range, SPH_JOINT, Tjoint::vrange, and ZERO.

Referenced by NewSphSphLowJoint(), and NewSphSphUpJoint().

void NewPrismaticJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		Tinterval *	range,
		boolean	avoidLimits,
		double	avoidLimitsWeight,
		Tjoint *	j
	)

Defines a new prismatic joint between two links.

Note that prismatic joints are always limited.

IMPORTANT: We assume that reference frames for the two links connected by a prismatic joint are aligned (the XYZ axis are parallel and just displaced).

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Four points defining the sliding axis. The first two points define the sliding axis in the reference frame of link1 and the two other points are the same vector in the frame of reference of link2.
range	Valid range of values for the prismatic joint.
avoidLimits	TRUE if the search process has to be focussed in the center of the valid ranges for the joint. This flag in only considered if hasLimits is true.
avoidLimitsWeight	If avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
j	The joint to intialize.

Definition at line 533 of file joint.c.

References ADJUST_REAL, Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, CopyInterval(), Error(), Tjoint::hasLimits, HTransformInverse(), HTransformTxyz(), InitJoint(), IntervalSize(), Tjoint::maxCoord, Norm(), Tjoint::normals, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, Tjoint::range, TRUE, and ZERO.

Referenced by main().

void NewSphSphLowJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		Tjoint *	j
	)

Defines the lower part of a spherical-spherical composite joint between two links.

NOTE: sph_sph_low joints are used to connect legs. The rotation about the leg axis is not relevant and thus, these joints can be seen as providing only two dof. However, when using a joint-based representation the third dof can not be ignored. This is so because when using a link-based formulation the rotation is fixed (arbitrarily but fixed) and the third dof is necessary to perfectly match the trnsformation when translating from joints to links and the reverse.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Two points defining the end points of the spherical-spherical joint. The first point in given in the first link and the second in the second link.
j	The joint to initialize.

Definition at line 601 of file joint.c.

References FALSE, INF, NewSphericalJoint(), SPH_SPH_LOW_JOINT, and Tjoint::t.

Referenced by AddLeg2World().

void NewSphSphUpJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		Tjoint *	j
	)

Defines the upper part of a spherical-spherical composite joint between two links.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Two points defining the end points of the spherical-spherical joint. The first point in given in the first link and the second in the second link.
j	The joint to initialize.

Definition at line 612 of file joint.c.

References FALSE, HTransformIdentity(), INF, IsVariableLengthLink(), NewSphericalJoint(), Tjoint::preT, SPH_SPH_UP_JOINT, and Tjoint::t.

Referenced by AddLeg2World().

void NewRevLowJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		Tjoint *	j
	)

Defines the lower part of a spherical-spherical planar composite joint between two links. The rotation is defined in the Z axis (the mechanism is limited to the XY plane).

The planar version of NewSphSphLowJoint.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Two points defining the end points of the spherical-spherical joint. The first point in given in the first link and the second in the second link.
j	The joint to initialize.

Definition at line 627 of file joint.c.

References Error(), FALSE, HTransformTxyz(), NEW, NewRevoluteJoint(), Tjoint::postT, Tjoint::preT, REP_JOINTS, REV_LOW_JOINT, Tjoint::t, and ZERO.

Referenced by AddLeg2World().

void NewRevUpJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		Tjoint *	j
	)

Defines the upper part of a spherical-spherical planar composite joint between two links. The rotation is defined in the Z axis (the mechanism is limited to the XY plane).

The planar version of NewSphSphUpJoint

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	Two points defining the end points of the spherical-spherical joint. The first point in given in the first link and the second in the second link.
j	The joint to initialize.

Definition at line 668 of file joint.c.

References Error(), FALSE, HTransformIdentity(), HTransformTxyz(), IsVariableLengthLink(), NEW, NewRevoluteJoint(), Tjoint::postT, Tjoint::preT, REP_JOINTS, REV_UP_JOINT, Tjoint::t, and ZERO.

Referenced by AddLeg2World().

void NewInPatchJoint	(	unsigned int	id,
		unsigned int	linkID1,
		Tlink *	link1,
		unsigned int	linkID2,
		Tlink *	link2,
		double **	points,
		double **	patch,
		boolean	avoidLimits,
		double	avoidLimitsWeight,
		Tjoint *	j
	)

Defines a new in-patch joint between two links. An in-patch joint is a generalization of a planar joint where the plane is replaced by a order one Bezier patch. This patch is defined by 4 points. When the four points are coplanar this joint is a planar joint.

This type of joints are basically used to define contact between links.

Parameters

id	Unique identifier given to the joint.
linkID1	Identifier of the first link to relate.
link1	Pointer to the first link to relate.
linkID2	Identifier of the second link to relate.
link2	Pointer to the first link to relate.
points	A point and a normal vector on the first link. This joint forces the point to be in the patch and the normal vector to be aligned with that of the patch at the contact point.
patch	Four points defining a first-order Bezier patch in the second link.
avoidLimits	TRUE if the search process has to be focussed in the center of the valid ranges for the joint. This flag in only considered if hasLimits is true.
avoidLimitsWeight	If avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
j	The joint to initialize.

Definition at line 715 of file joint.c.

References ADJUST_REAL, Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, CrossProduct(), DotProduct(), Error(), Tjoint::hasLimits, HTransformIdentity(), HTransformX2Vect(), IN_PATCH_JOINT, InitJoint(), IntervalAdd(), IntervalScale(), IntervalSqrt(), Tjoint::maxCoord, NewInterval(), Norm(), Tjoint::normals, Tjoint::normRange, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::range, Tjoint::range2, TRUE, and ZERO.

void CopyJoint	(	Tjoint *	j_dst,
		Tjoint *	j_src
	)

Defines a joint structure copying data from anther joint.

Parameters

j_dst	The joint to create.
j_src	The joint from where to copy.

Definition at line 879 of file joint.c.

References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, CopyInterval(), Tjoint::coupled, Tjoint::hasLimits, HTransformCopy(), Tjoint::id, Tjoint::link, Tjoint::linkID, Tjoint::maxCoord, Tjoint::normals, Tjoint::normRange, Tjoint::offset, Tjoint::offset2, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::range, Tjoint::range2, Tjoint::t, and Tjoint::vrange.

Referenced by AddJoint2Mechanism().

unsigned int GetJointType

(

Tjoint *

j

)

Gets the joint type.

Parameters

j	The joint to query.

Returns

The joint type.

Definition at line 931 of file joint.c.

References Tjoint::t.

Referenced by AddJoint2Mechanism(), AllRevolute(), IsRevoluteBinaryLink(), and PrintMechanism().

double GetJointMaxCoordinate

(

Tjoint *

j

)

Gets the maximum coordinate used in the joint definition (eigher in X, Y or Z).

Parameters

j	The joint to query.

Returns

The maximum coordiante.

Definition at line 936 of file joint.c.

References Tjoint::maxCoord.

Referenced by AddJoint2Mechanism().

unsigned int GetJointID

(

Tjoint *

j

)

Gets the joint identifier (given to the joint when created).

Parameters

j	The joint to query.

Returns

The joint identifier.

Definition at line 941 of file joint.c.

References Tjoint::id.

Referenced by GetTransformFromBranch().

unsigned int JointFromID

(

Tjoint *

j

)

Gets the identifier of the first link involved in the joint.

Parameters

j	The joint to query.

Returns

The identifier of the first link involved in the joint.

Definition at line 946 of file joint.c.

References Tjoint::linkID.

Referenced by Branches2Links(), GenerateKinTree(), GetBranchDestination(), GetBranchOrigin(), GetLinkTransformsFromDOF(), GetMechanismDOFsFromTransforms(), IsRevoluteBinaryLink(), NoCheckConnectedCollisions(), and WorldDOF2Sol().

Tlink* JointFrom

(

Tjoint *

j

)

Gets a pointer to the first link involved in the joint.

Parameters

j	The joint to query.

Returns

The pointer to the first link involved in the joint.

Definition at line 951 of file joint.c.

References Tjoint::link.

Referenced by GenerateMEquationFromBranch(), GetLinkTransformsFromDOF(), GetMechanismDOFsFromTransforms(), and PrintMechanism().

unsigned int JointToID

(

Tjoint *

j

)

Gets the identifier of the second link involved in the joint.

Parameters

j	The joint to query.

Returns

The identifier of the second link involved in the joint.

Definition at line 956 of file joint.c.

References Tjoint::linkID.

Referenced by Branches2Links(), GenerateKinTree(), GetBranchDestination(), GetBranchOrigin(), GetLinkTransformsFromDOF(), GetMechanismDOFsFromTransforms(), IsRevoluteBinaryLink(), NoCheckConnectedCollisions(), and WorldDOF2Sol().

Tlink* JointTo

(

Tjoint *

j

)

Gets a pointer to the second link involved in the joint.

Parameters

j	The joint to query.

Returns

The pointer to the second link involved in the joint.

Definition at line 961 of file joint.c.

References Tjoint::link.

Referenced by PrintMechanism().

void GetJointName	(	char **	name,
		Tjoint *	j
	)

Returns a string identifying the type of joint and the connected links. This is basically used for interface.

Parameters

name	The joint name. The space for this string is allocated inside this function but must be deallocated externally.
j	The joint to query.

Definition at line 966 of file joint.c.

References Error(), FIX_JOINT, FREE_JOINT, GetLinkName(), IN_PATCH_JOINT, Tjoint::link, NEW, PRS_JOINT, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by GetWorldDOFLabel(), and GetWorldJointLabel().

void GetJointDOFName	(	unsigned int	ndof,
		char **	name,
		Tjoint *	j
	)

Returns a label that uniquely identify a given dof in a joint. If the joint only has a single dof, the label is NULL (i.e. the name of the joint is enough to identify the dof).

This is used to create interfaces.

Parameters

ndof	Number of degree of freedom to identify.
name	The label to create. The space is allocated in the function and must be deallocted by the caller.
j	The joint to query.

Definition at line 1006 of file joint.c.

References Error(), FIX_JOINT, FREE_JOINT, IN_PATCH_JOINT, NEW, PRS_JOINT, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by GetWorldDOFLabel().

void GetJointPoint	(	unsigned int	link,
		unsigned int	point,
		double *	p,
		Tjoint *	j
	)

Gets one of the points defining the rotation/sliding axis for the joint.

Parameters

link	0 if we want to get the point in the first link reference frame and 1 if we want to get it in the frame of the second link.
point	0/1 according if we want to get the origin of the axis or the end.
p	An array where to store the requested point.
j	The joint to query.

Definition at line 1102 of file joint.c.

References Tjoint::points.

Referenced by IsRevoluteBinaryLink(), and PrintMechanism().

boolean LimitedJoint

(

Tjoint *

j

)

Checks if a joint has limits.

Parameters

j	The joint to query.

Returns

TRUE if there joint jas limits.

Definition at line 1112 of file joint.c.

References Tjoint::hasLimits.

Tinterval* GetJointRange

(

Tjoint *

j

)

Checks the limits of a joint.

Parameters

j	The joint to query.

Returns

A pointer to the interval with the limits of the joint. For joints with no limits, this interval will be non-meaningful.

Definition at line 1117 of file joint.c.

References Tjoint::range.

Tinterval* GetJointSecondRange

(

Tjoint *

j

)

Checks the second limit of a universal joint.

Only universal joints have a second range.

Parameters

j	The joint to query.

Returns

A pointer to the interval with the limits of the joint. For joints with no limits, this interval will be non-meaningful.

Definition at line 1122 of file joint.c.

References Tjoint::range2.

void GetJointRangeN	(	unsigned int	nr,
		double	mt,
		Tinterval *	r,
		Tjoint *	j
	)

Returns the ranges for one of the degrees of freedom of the joint.

Parameters

nr	Number of degree of freedom.
mt	Maximum value for non-constrained translations.
r	Output range.
j	The joint to query.

Definition at line 1127 of file joint.c.

References CopyInterval(), Error(), FIX_JOINT, FREE_JOINT, Tjoint::hasLimits, IN_PATCH_JOINT, IntervalOffset(), M_PI, NewInterval(), Tjoint::offset, Tjoint::offset2, PRS_JOINT, Tjoint::range, Tjoint::range2, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by GenerateJointEquations(), and GetWorldRangeDOF().

unsigned int GetJointRangeTopology	(	unsigned int	nr,
		Tjoint *	j
	)

Returns the topology of one of the degrees of freedom of the joint.

Parameters

nr	Number of degree of freedom.
j	The joint to query.

Returns

The topology (TOPOLOGY_R or TOPOLOGY_S).

Definition at line 1214 of file joint.c.

References Error(), FIX_JOINT, FREE_JOINT, Tjoint::hasLimits, IN_PATCH_JOINT, PRS_JOINT, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, TOPOLOGY_R, TOPOLOGY_S, and UNV_JOINT.

Referenced by GenerateJointEquations(), WorldDOF2Sol(), and WorldDOFTopology().

unsigned int CoupledWith

(

Tjoint *

j

)

Returns the identifier of the joint coupled with the query joint.

Note that up to now we have a very limited implementation of the coupling between joints: Only revolute joints (with limits) can be coupled and always with a 1 to 1 relation.

If we have a pair of joints coupled (j1,j2) with j1 having a lower identifier than j2 (i.e., appearing before in the mechanism definition), this function only returns something different from NO_UINT for j2. In other words, the function only identifies the coupled but not the original joint.

Parameters

j	The joint to query.

Returns

The identifier of the coupled joint of NO_UINT if the joint is not coupled.

Definition at line 1285 of file joint.c.

References Tjoint::coupled, and NO_UINT.

Referenced by WorldInitDOFInfo().

unsigned int GetJointDOF

(

Tjoint *

j

)

Computes the degrees of freedom allowed by a given joint. The degrees of freedom constrained by the joint are 6-the allowed ones (i.e., the returned by this function.

Parameters

j	The joint to query.

Returns

The degrees of freedom allowed by the joint.

Definition at line 1293 of file joint.c.

References Tjoint::coupled, Error(), FIX_JOINT, FREE_JOINT, IN_PATCH_JOINT, PRS_JOINT, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by GenerateJointEquations(), GetMechanismDOFsFromTransforms(), GetMechanismMobility(), GetWorldDOFLabel(), and WorldInitDOFInfo().

void GenerateJointRangeSingularityEquations	(	Tparameters *	p,
		TCuikSystem *	cs,
		Tjoint *	j
	)

Workspace boundaries are detected as singularities. Here we introduce some changes in the range variables so that we force a singularity in the extremes of the ranges for the joints.

Parameters

p	A set of parameters. They include, for instance the dummification level.
cs	The cuiksystem where to add the variables and equations.
j	The joint from where to generate the variables and equations.

Definition at line 1333 of file joint.c.

References AddCt2Monomial(), AddEquation2CS(), AddMonomial(), AddVariable2CS(), AddVariable2Monomial(), COS_VAR, COS_VAR_SING, COS_VAR_UNI, COS_VAR_UNI_SING, CT_REPRESENTATION, DeleteEquation(), DeleteMonomial(), DeleteVariable(), EQU, Error(), GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::hasLimits, Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, IN_PATCH_JOINT_CTRL_VAR_SING_COS, IN_PATCH_JOINT_CTRL_VAR_SING_SIN, InitEquation(), InitMonomial(), Tjoint::link, LowerLimit(), NEW, NewInterval(), NewVariable(), NFUN, NO_UINT, PRS_JOINT, PRS_JOINT_VAR, PRS_JOINT_VAR_SING_COS, PRS_JOINT_VAR_SING_SIN, Tjoint::range, Tjoint::range2, REP_JOINTS, ResetEquation(), ResetMonomial(), REV_JOINT, SetEquationCmp(), SetEquationType(), SetEquationValue(), SetVariableInterval(), SPH_JOINT, SYSTEM_EQ, SYSTEM_VAR, Tjoint::t, UNV_JOINT, and UpperLimit().

Referenced by GenerateWorldSingularityEquations().

void GenerateJointRangeEquations	(	Tparameters *	p,
		TCuikSystem *	cs,
		Tjoint *	j
	)

Adds to the given cuiksystem the variables and equations necessary to deal with the joint ranges. For prismatic joints this is one variable and its associated range. For revolute joints, we have to define two vectors one associted with link1 and the other link2 and define the scalar and vector products (this produces a third vector aligned with the rotation axis) to get the cosinus/sinus of the rotated angle that are then bounded given the range for the allowed rotation between the two links.

Parameters

p	A set of parameters. They include, for instance the dummification level.
cs	The cuiksystem where to add the variables and equations.
j	The joint from where to generate the variables and equations.

Definition at line 1521 of file joint.c.

References AddCt2Monomial(), AddEquation2CS(), AddMonomial(), AddTerm2SearchCriterion(), AddVariable2CS(), AddVariable2Monomial(), ApplyLinkRot(), Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, COS_VAR, COS_VAR_UNI, Tjoint::coupled, CT_REPRESENTATION, DeleteEquation(), DeleteInterval(), DeleteMonomial(), DeleteVariable(), EQU, Error(), FIX_JOINT, FREE_JOINT, GenerateDotProductEquation(), GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::hasLimits, Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, INF, InitEquation(), InitMonomial(), IntervalCenter(), IntervalCosine(), IntervalSize(), IsGroundLink, Tjoint::link, Tjoint::linkID, NEW, NewInterval(), NewVariable(), NFUN, NO_UINT, PRS_JOINT, PRS_JOINT_VAR, Tjoint::range, Tjoint::range2, REP_JOINTS, ResetMonomial(), REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, ROT_JOINT_VAR_REF, SECONDARY_VAR, SetEquationCmp(), SetEquationType(), SetEquationValue(), SetVariableInterval(), SPH_JOINT, SPH_JOINT_VAR_REF, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, SYSTEM_EQ, SYSTEM_VAR, Tjoint::t, UNV_JOINT, UNV_JOINT_VAR, UNV_JOINT_VAR_REF, and Tjoint::vrange.

Referenced by InitWorldKinCS().

void GenerateJointEquationsInBranch	(	Tparameters *	p,
		double	s,
		TCuikSystem *	cs,
		Tequation *	eq,
		Tjoint *	j
	)

Adds to the given equations the constraints of a joint

Before using this method, the variables and equations for individual joints have to be generated (using GenerateJointEquations).

When considered in a sequence, joints define a sum of vectors taking from the origin of the frame of reference of one link to the origin of the frame of reference for the next link. When these sums are taken on a closed branch, we get the loop equations.

Since links and joints are defined in 3D, summing vectors defined on links and joints define 3 equations, one for X, one for Y, and another for Z.

Parameters

p	The set of parameters.
s	Orientation of the joint in the sequence (1 for positive and -1 for negative).
cs	The cuiksystem where the joint variables are defined.
eq	Three equations (X, Y, and Z) where to add the joint constraints.
j	The joint from where to generate the variables and equations.

Definition at line 1812 of file joint.c.

References AddCt2Monomial(), AddMonomial(), AddVariable2Monomial(), ApplyLinkRot(), CT_REPRESENTATION, DeleteMonomial(), Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, InitMonomial(), IsGroundLink, Tjoint::link, Tjoint::linkID, NEW, NFUN, NO_UINT, Tjoint::normals, Tjoint::points, PRS_JOINT, PRS_JOINT_VAR, REP_JOINTS, ResetMonomial(), REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by GenerateTransEquationsFromBranch().

void JointForceEquation	(	Tparameters *	pr,
		unsigned int	linkID,
		TCuikSystem *	cs,
		Tequation *	eq,
		Tjoint *	j
	)

In tensegrity systems the forces exerced by the joints coincident in a given link should be balanced.

Only STRUT, BAR, and CABLE joints can be used in tenegrities. STRUTS and BARS are compressed and CABLEs are in tension. The sum of compressions and tensions should vanish at each link (i.e. node).

Parameters

pr	The set of parameters.
linkID	The link in process.
cs	The CuikSystem where the joint variables are defined.
eq	The equations to extend.
j	The joint to process.

Definition at line 1925 of file joint.c.

References ApplyLinkRotNoDeform(), FALSE, GetCSVariableID(), GetLinkForceModel(), GetLinkName(), Tjoint::link, LINK_MAX_FORCE, Tjoint::linkID, NEW, and NO_FORCE.

Referenced by GenerateForceEquilibriumEquations().

void GenerateJointEquations	(	Tparameters *	p,
		double	maxCoord,
		TCuikSystem *	cs,
		Tjoint *	j
	)

Adds to the given cuiksystem the variables and equations necessary to deal with the joint. Intutively, we have to add equations so that the revolute/sliding axis is the same as view from link1 and from link2.

Parameters

p	A set of parameters. They include, for instance the dummification level.
maxCoord	The maximum value for the variables in the system.
cs	The cuiksystem where to add the variables and equations.
j	The joint from where to generate the variables and equations.

Definition at line 1967 of file joint.c.

References AddCt2Monomial(), AddEquation2CS(), AddMonomial(), AddVariable2CS(), AddVariable2Monomial(), ApplyLinkRot(), CT_REPRESENTATION, DeleteEquation(), DeleteInterval(), DeleteMonomial(), DeleteVariable(), DOF_VAR, DotProduct(), DUMMY_VAR, EQU, Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GenerateDotProductEquation(), GenerateSaddleEquation(), GetJointDOF(), GetJointRangeN(), GetJointRangeTopology(), GetLinkName(), GetParameter(), HTransformGetElement(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, IN_PATCH_JOINT_SCALE_VAR, InitEquation(), InitMonomial(), IsGroundLink, Tjoint::link, Tjoint::linkID, NEW, NewInterval(), NewVariable(), NFUN, NO_UINT, Tjoint::normals, Tjoint::normRange, Tjoint::points, Tjoint::preT, PRS_JOINT, REP_JOINTS, ResetMonomial(), REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SECONDARY_VAR, SetEquationCmp(), SetEquationType(), SetEquationValue(), SetVariableInterval(), SetVariableTopology(), SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, SYSTEM_EQ, SYSTEM_VAR, Tjoint::t, TOPOLOGY_S, UNV_JOINT, and UNV_JOINT_VAR.

Referenced by InitWorldKinCS().

void RegenerateJointSolution	(	Tparameters *	p,
		TCuikSystem *	cs,
		double *	sol,
		Tjoint *	j
	)

Solution points only include values for the system variables (and secondary). However, in some formulations, the frame joint equations use not only system/secondary variables, but, for instance, dummy variable too. This function computes the values for the joint-related non-basis variables form the basic ones for a given joint.

Currently only the IN_PATCH_JOINT uses dummy variables but this can change in the future.

Parameters

p	The set of 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
j	The joint.

Definition at line 2291 of file joint.c.

References CT_REPRESENTATION, GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, Tjoint::link, NEW, REP_JOINTS, and Tjoint::t.

Referenced by RegenerateMechanismSolution().

void RegenerateJointBox	(	Tparameters *	p,
		TCuikSystem *	cs,
		Tbox *	b,
		Tjoint *	j
	)

This is the same as RegenerateJointSolution but working on solution boxes instead of on solution points (i.e. it is interval-based instead of floating point based)

Parameters

p	The set of 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.
b	The solution box with the values for the system variables. At the end of the function, this box also has values for dummy ones.
j	The joint.

Definition at line 2327 of file joint.c.

References CT_REPRESENTATION, GetBoxInterval(), GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, IntervalProduct(), Tjoint::link, NEW, REP_JOINTS, SetBoxInterval(), and Tjoint::t.

Referenced by RegenerateMechanismBox().

void GenerateJointSolution	(	Tparameters *	p,
		double *	dof,
		THTransform *	t1,
		THTransform *	t2,
		TCuikSystem *	cs,
		double *	sol,
		Tjoint *	j
	)

Generates the joint related variables in a solution from the homogeneous transforms of the two links involved in the joint and the values of the degrees of freedom for this joint.

This is part of the mechanism to transform from dof information to a solution point in the cuik form (so that in can be plotted, etc).

This is transforms2samples, but using the dofs to facilitate the computations. Actually the dofs could be computed from the transforms using GetJointTransform but having them facilitates the computation (see GetLinkTransformsFromSolution for the inverse operation, i.e samples2transforms).

Since the dofs are finally (marginally) used, this can also be seen as dof2samples (see GetJointDOFValues for the inverse). The rest of the dof2samples is implemented in GenerateLinkSolution.

Parameters

p	The parameter set.
dof	Values for the degrees of freedom of this joint.
t1	Homogenous transform given the pose of the first link in the joint.
t2	Homogenous transform given the pose of the second link in the joint.
cs	The cuik system to retrive the identifier of the variables for the joint.
sol	Solution vector where to store the output values.
j	The joint.

Definition at line 2365 of file joint.c.

References COS_VAR, COS_VAR_UNI, CT_REPRESENTATION, DotProduct(), Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::hasLimits, HTransformApplyRot(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, IN_PATCH_JOINT_SCALE_VAR, Tjoint::link, Tjoint::linkID, NEW, NO_UINT, Tjoint::normals, PRS_JOINT, PRS_JOINT_VAR, REP_JOINTS, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, ROT_JOINT_VAR_REF, SPH_JOINT, SPH_JOINT_VAR_REF, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, Tjoint::t, UNV_JOINT, UNV_JOINT_VAR, UNV_JOINT_VAR_REF, and Tjoint::vrange.

Referenced by WorldDOF2Sol().

void GetJointTransform	(	double *	dof,
		THTransform *	t,
		Tjoint *	j
	)

Computes the homogeneous trasform taking from the first link involved in the joint to the second link, considering a given value for the degrees of freedom of the joint.

This is the dof2transforms (see GetJointDOFValues for the inverse).

Parameters

dof	Values for the DOF of the joint.
t	The output transform.
j	The joint.

Definition at line 2537 of file joint.c.

References CrossProduct(), Error(), FIX_JOINT, FREE_JOINT, HTransformAcumRot(), HTransformCopy(), HTransformDelete(), HTransformFromVectors(), HTransformInverse(), HTransformProduct(), HTransformRx(), HTransformRy(), HTransformRz(), HTransformTxyz(), IN_PATCH_JOINT, M_PI, Normalize(), Tjoint::normals, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, RX, RY, RZ, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, SumVectorScale(), Tjoint::t, and UNV_JOINT.

Referenced by GetLinkTransformsFromDOF().

void GetJointDOFValues	(	Tparameters *	p,
		THTransform *	t1,
		THTransform *	t2,
		double *	dof,
		Tjoint *	j
	)

Recovers the joint DOFs given the poses of the two links connected by the joint.

This function recovers the joint parameters such that t2=t1*T where T is the transform sequence resulting from GetJointTransSeq.

This is the transforms2dof (the inverse of GetJointTransform)

Since the transforms are computed from samples, this is also samples2dof, i.e., the inverse of GenerateJointSolution + GenerateLinkSolution. Note that dofs are only associated with joints and, thus there is no need for a GetLinkDOFValues to complete the inverse.

Parameters

p	The set of parameters.
t1	The transform giving the absolute pose of the first link involved in the joint.
t2	The transform giving the absolute pose of the second link involved in the joint.
dof	Space where to store the computed values.
j	The joint to query.

Definition at line 2638 of file joint.c.

References AXIS_H, AXIS_X, AXIS_Y, AXIS_Z, Tjoint::coupled, CrossProduct(), Det2x2(), Det3x3(), DifferenceVector(), DotProduct(), Error(), FIX_JOINT, FREE_JOINT, GetYawPitchRoll(), HTransformCopy(), HTransformDelete(), HTransformFromVectors(), HTransformGetElement(), HTransformInverse(), HTransformProduct(), HTransformSetElement(), IN_PATCH_JOINT, Norm(), Normalize(), Tjoint::normals, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, SumVectorScale(), Tjoint::t, UNV_JOINT, and ZERO.

Referenced by GetMechanismDOFsFromTransforms().

void GetJointTransSeq	(	Tparameters *	p,
		TCuikSystem *	cs,
		TTransSeq *	ts,
		Tjoint *	j
	)

Build the sequence of transforms taking from one link to the next via the joint.

Parameters

p	The set of parameters.
cs	The cuiksysem on which the dof variables are defined.
ts	The resulting sequence of transforms.
j	The joint.

Definition at line 2829 of file joint.c.

References AddCtTrans2TransSeq(), AddDispTrans2TransSeq(), AddPatchTrans2TransSeq(), AddVarTrans2TransSeq(), Tjoint::coupled, CT_REPRESENTATION, DOF_VAR, Error(), FIX_JOINT, FREE_JOINT, GetCSVariableID(), GetLinkName(), GetParameter(), HTransformRx2(), HTransformRz2(), Tjoint::id, IN_PATCH_JOINT, InitTransSeq(), Tjoint::link, NEW, NO_UINT, Tjoint::normals, PA, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, REP_JOINTS, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, RX, RY, RZ, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, SumVector(), Tjoint::t, and UNV_JOINT.

Referenced by GenerateMEquationFromBranch().

void PrintJointAxes	(	Tparameters *	p,
		FILE *	f,
		TCuikSystem *	cs,
		double *	sol,
		double *	r,
		Tjoint *	j
	)

Print a point and a vector for the joint axis. This is latter used to generate the Jacobian of the mechanism at a given point.

Parameters

p	The set of parameters.
f	The file where to store the data.
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	Array with the translation from the ground link to each link.
j	The joint to query.

Definition at line 3014 of file joint.c.

References Error(), FREE_JOINT, GetTransform2Link(), HTransformApply(), HTransformApplyRot(), IsGroundLink, Tjoint::link, Tjoint::linkID, Tjoint::normals, Tjoint::points, PRS_JOINT, REV_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by PrintWorldAxes().

void PrintJoint	(	FILE *	f,
		Tjoint *	j
	)

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

Parameters

f	The file where to store the information.
j	The joint to print.

Definition at line 3054 of file joint.c.

References Tjoint::avoidLimits, Error(), FIX_JOINT, FREE_JOINT, GetLinkName(), Tjoint::hasLimits, HTransformPrettyPrint(), IN_PATCH_JOINT, Tjoint::link, Tjoint::points, Tjoint::preT, PrintInterval(), PRS_JOINT, Tjoint::range, Tjoint::range2, REV_JOINT, REV_LOW_JOINT, REV_UP_JOINT, SPH_JOINT, SPH_SPH_LOW_JOINT, SPH_SPH_UP_JOINT, SumVector(), Tjoint::t, UNV_JOINT, and Tjoint::vrange.

Referenced by PrintMechanism().

void DeleteJoint

(

Tjoint *

j

)

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

Parameters

j	The joint to delete.

Definition at line 3190 of file joint.c.

References DeleteInterval(), HTransformDelete(), Tjoint::postT, Tjoint::preT, Tjoint::range, and Tjoint::range2.

Referenced by AddLeg2World(), DeleteMechanism(), GenerateKinTree(), InitWorldFromMolecule(), and main().