joint.h File Reference

Detailed Description

Definition of the Tjoint type and the associated functions.

See Also
Tjoint, joint.c.

Definition in file joint.h.

Data Structures

struct  Tjoint
 Relation between two links. More...
 

Macros

#define NO_JOINT   NO_UINT
 One of the possible type of joints. More...
 
#define FREE_JOINT   0
 One of the possible type of joints. More...
 
#define FIX_JOINT   1
 One of the possible type of joints. More...
 
#define REV_JOINT   2
 One of the possible type of joints. More...
 
#define UNV_JOINT   3
 One of the possible type of joints. More...
 
#define SPH_JOINT   4
 One of the possible type of joints. More...
 
#define SPH_SPH_JOINT   5
 One of the possible type of joints. More...
 
#define SPH_PRS_SPH_JOINT   6
 One of the possible type of joints. More...
 
#define PRS_JOINT   7
 One of the possible type of joints. More...
 
#define IN_PATCH_JOINT   8
 One of the possible type of joints. More...
 

Functions

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 NewSphSphJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, double l, double r, Tcolor *color, Tjoint *j)
 Constructor. More...
 
void NewSphPrsSphJoint (unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, double **points, Tinterval *range, double r, Tcolor *color, 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...
 
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...
 
TlinkJointFrom (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...
 
TlinkJointTo (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...
 
boolean IsJointAllSpheres (Tjoint *j)
 Identifies joint formed only by spheres. 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...
 
TintervalGetJointRange (Tjoint *j)
 Checks the limits of a joint. More...
 
TintervalGetJointSecondRange (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...
 
signed int GetJointDOF (Tjoint *j)
 Computes the degrees of freedom allowed by a given joint. More...
 
double GetJointLength (Tjoint *j)
 Returns the length of a spherical-spherical 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 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...
 
double GetJointMaxCoordinate (Tjoint *j)
 Returns the maximum coordinate value for all the objects in the joint. More...
 
void PlotJoint (Tplot3d *pt, Tjoint *j)
 Adds a joint to a 3d scene. More...
 
void MoveJointFromTransforms (Tparameters *p, Tplot3d *pt, THTransform *t1, THTransform *t2, Tjoint *j)
 Displaces a joint in a 3d scene. 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...
 

Macro Definition Documentation

#define NO_JOINT   NO_UINT

One of the possible type of joints. This is used for uninitialized joints.

See Also
Tjoint

Definition at line 31 of file joint.h.

#define FREE_JOINT   0

One of the possible type of joints. A free joint: imposes no constraint between two links. Used to define free flying robots.

See Also
Tjoint

Definition at line 40 of file joint.h.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GenerateJointSolution(), GetJointDOF(), GetJointDOFValues(), GetJointName(), GetJointRangeN(), GetJointRangeTopology(), GetJointTransform(), GetJointTransSeq(), NewFreeJoint(), PrintJoint(), and PrintJointAxes().

#define FIX_JOINT   1

One of the possible type of joints. A fix joint: fixes the translation and the orientation of one link with respect to another link using an homogeneous transform.

See Also
Tjoint

Definition at line 51 of file joint.h.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GenerateJointSolution(), GetJointDOF(), GetJointDOFValues(), GetJointName(), GetJointRangeN(), GetJointRangeTopology(), GetJointTransform(), GetJointTransSeq(), NewFixJoint(), and PrintJoint().

#define SPH_SPH_JOINT   5

One of the possible type of joints. A spherical-spherical composite joint.

This type of joint define a cylinder of a given length between the two links connected by this joint. A single vector represents this cylinder (gives its orientation w.r.t. the global frame). The position is deduced from the anchor point of the link and the rotation around the cylinder is not taken into account. By using a spherical-spherical composite joint we do not have to represent a full reference frame for the cylindre connecting the links and we save variables and equations.

This type of joint is particularly taylored to represent legs of parallel platforms with fixed lenght.

Todo:
Spherical-Spherical joints implicitly define a cylinder between the two extremes of the joint. Right now, there is no way to include this cylinder in the collision checking.
See Also
Tjoint

Definition at line 98 of file joint.h.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GenerateJointSolution(), GetJointDOF(), GetJointDOFValues(), GetJointLength(), GetJointName(), GetJointRangeN(), GetJointRangeTopology(), GetJointTransform(), GetJointTransSeq(), IsJointAllSpheres(), MoveJointFromTransforms(), NewSphSphJoint(), PlotJoint(), and PrintJoint().

#define SPH_PRS_SPH_JOINT   6

One of the possible type of joints. A spherical-prismatic-spherical composite joint.

This type of joint define a prismatic joint in between two spherical joints. It is a generalization of the SPH_SPH_JOINT but here the distance between the two spherical joints can vary inside a given range. This variable length is modeled (and graphically represented) as a primatic joint (two cylinders with different radii that can slide one smaller inside the larger).

This type of joint is particularly taylored to represent legs of parallel platform with variable lenght.

Todo:
Spherical-Prismatic-Spherical joints implicitly define two cylinders between the two extremes of the joint. Right now, there is no way to include this cylinder in the collision checking.
See Also
Tjoint

Definition at line 119 of file joint.h.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GenerateJointRangeSingularityEquations(), GenerateJointSolution(), GetJointDOF(), GetJointDOFValues(), GetJointName(), GetJointRangeN(), GetJointRangeTopology(), GetJointTransform(), GetJointTransSeq(), IsJointAllSpheres(), MoveJointFromTransforms(), NewSphPrsSphJoint(), PlotJoint(), and PrintJoint().

#define IN_PATCH_JOINT   8

Defines a generalized plannar joint where a point in the first link is constrainted to be inside a patch in the second link and the normal of the first link at the given point is aligned with the normal of the patch.

The patch is defined as a order-one Bezier patch that is given by 4 points. If those for points are co-planar this joint is a plannar joint with a plane limited by the 4 given points.

See Also
Tjoint

Definition at line 142 of file joint.h.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GenerateJointRangeSingularityEquations(), GenerateJointSolution(), GetJointDOF(), GetJointDOFValues(), GetJointName(), GetJointRangeN(), GetJointRangeTopology(), GetJointTransform(), GetJointTransSeq(), NewInPatchJoint(), PrintJoint(), RegenerateJointBox(), and RegenerateJointSolution().

Function Documentation

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
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
jThe joint to initialize.

Definition at line 101 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
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
tHomogeneous transform defining the relative position between the two links.
jThe joint to initialize.

Definition at line 110 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
idUnique identifier given to the joint.
rType of represention used. Value for CT_REPRESENTATION.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsFour 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.
hasLimitsTRUE if the joint to create include limits for the rotation movement.
rangeIf hasLimits is TRUE, this is the valid range for the rotation (in radiants).
rPointsIf 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].
avoidLimitsTRUE 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.
avoidLimitsWeightIf avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
coupledPointer 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).
jThe joint to initialize.

Definition at line 132 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(), and main().

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
idUnique identifier given to the joint.
rType of represention used. Value for CT_REPRESENTATION.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsFour 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
hasLimitsTRUE if the joint to create include limits.
range1If hasLimits is TRUE, this is the valid range for the first rotation (in radiants).
range2If hasLimits is TRUE, this is the valid range for the second rotation (in radiants).
rPointspoints[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.
avoidLimitsTRUE 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.
avoidLimitsWeightIf avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
jThe joint to initialize.

Definition at line 288 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
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsTwo 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.
hasLimitsTRUE if the joint to create include limits.
rangeIf hasLimits is TRUE, the valid value of rotation angle between the two links is [-range,range] .
rPointspoints[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.
avoidLimitsTRUE 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.
avoidLimitsWeightIf avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
jThe joint to initialize.

Definition at line 452 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.

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
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsFour 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.
rangeValid range of values for the prismatic joint.
avoidLimitsTRUE 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.
avoidLimitsWeightIf avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
jThe joint to intialize.

Definition at line 541 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 NewSphSphJoint ( unsigned int  id,
unsigned int  linkID1,
Tlink link1,
unsigned int  linkID2,
Tlink link2,
double **  points,
double  l,
double  r,
Tcolor color,
Tjoint j 
)

Defines a new spherical-spherical composite joint between two links.

Parameters
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsTwo 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.
lLength of the spherical-spherical joint. Distance between the two extremes of the joint.
rRadius of the cylinder in between the extremes of the spherical-spherical joint.
colorThe color for the cylinder in between the extremes of the spherical-spherical joint.
jThe joint to initialize.

Definition at line 609 of file joint.c.

References ADJUST_REAL, Tjoint::color, CopyColor(), HTransformInverse(), HTransformTxyz(), InitJoint(), Tjoint::length, Tjoint::maxCoord, Norm(), Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::rad, and SPH_SPH_JOINT.

void NewSphPrsSphJoint ( unsigned int  id,
unsigned int  linkID1,
Tlink link1,
unsigned int  linkID2,
Tlink link2,
double **  points,
Tinterval range,
double  r,
Tcolor color,
Tjoint j 
)

Defines a new spherical-prismatic-spherical composite joint between two links.

Parameters
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsTwo 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.
rangeValid range of lenghts between the two spherical joints..
rRadius of the cylinder in between the extremes of the spherical joints.
colorThe color for the cylinder in between the extremes of the spherical joints.
jThe joint to initialize.

Definition at line 652 of file joint.c.

References ADJUST_REAL, Tjoint::color, CopyColor(), CopyInterval(), Error(), HTransformInverse(), HTransformTxyz(), InitJoint(), IntervalSize(), Tjoint::length, LowerLimit(), Tjoint::maxCoord, Norm(), Tjoint::offset, Tjoint::points, Tjoint::postT, Tjoint::preT, Tjoint::rad, Tjoint::range, and SPH_PRS_SPH_JOINT.

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
idUnique identifier given to the joint.
linkID1Identifier of the first link to relate.
link1Pointer to the first link to relate.
linkID2Identifier of the second link to relate.
link2Pointer to the first link to relate.
pointsA 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.
patchFour points defining a first-order Bezier patch in the second link.
avoidLimitsTRUE 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.
avoidLimitsWeightIf avoidLimits is TRUE, this is the strength of the limit avoidance for this joint.
jThe joint to initialize.

Definition at line 709 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 
)
unsigned int GetJointType ( Tjoint j)

Gets the joint type.

Parameters
jThe joint to query.
Returns
The joint type.

Definition at line 931 of file joint.c.

References Tjoint::t.

Referenced by AllRevolute(), and IsRevoluteBinaryLink().

unsigned int GetJointID ( Tjoint j)

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

Parameters
jThe joint to query.
Returns
The joint identifier.

Definition at line 936 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
jThe joint to query.
Returns
The identifier of the first link involved in the joint.

Definition at line 941 of file joint.c.

References Tjoint::linkID.

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

Tlink* JointFrom ( Tjoint j)

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

Parameters
jThe joint to query.
Returns
The pointer to the first link involved in the joint.

Definition at line 946 of file joint.c.

References Tjoint::link.

unsigned int JointToID ( Tjoint j)

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

Parameters
jThe joint to query.
Returns
The identifier of the second link involved in the joint.

Definition at line 951 of file joint.c.

References Tjoint::linkID.

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

Tlink* JointTo ( Tjoint j)

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

Parameters
jThe joint to query.
Returns
The pointer to the second link involved in the joint.

Definition at line 956 of file joint.c.

References Tjoint::link.

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
nameThe joint name. The space for this string is allocated inside this function but must be deallocated externally.
jThe joint to query.

Definition at line 961 of file joint.c.

References Error(), FIX_JOINT, FREE_JOINT, GetLinkName(), IN_PATCH_JOINT, Tjoint::link, NEW, PRS_JOINT, REV_JOINT, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, Tjoint::t, and UNV_JOINT.

Referenced by GetWorldDOFLabel(), and GetWorldJointLabel().

boolean IsJointAllSpheres ( Tjoint j)

Returns TRUE the bodies in the joint, if any, are all spheres.

Joints typically do not include any body, but if they do, this functions identifies if they are spheres (typically they are not).

Parameters
jThe joint to query.
Returns
TRUE if all joint bodies are spheres.

Definition at line 1004 of file joint.c.

References SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, and Tjoint::t.

Referenced by AddJoint2Mechanism().

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
link0 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.
point0/1 according if we want to get the origin of the axis or the end.
pAn array where to store the requested point.
jThe joint to query.

Definition at line 1012 of file joint.c.

References Tjoint::points.

Referenced by IsRevoluteBinaryLink().

boolean LimitedJoint ( Tjoint j)

Checks if a joint has limits.

Parameters
jThe joint to query.
Returns
TRUE if there joint jas limits.

Definition at line 1022 of file joint.c.

References Tjoint::hasLimits.

Tinterval* GetJointRange ( Tjoint j)

Checks the limits of a joint.

Parameters
jThe 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 1027 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
jThe 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 1032 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
nrNumber of degree of freedom.
mtMaximum value for non-constrained translations.
rOutput range.
jThe joint to query.

Definition at line 1037 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, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_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
nrNumber of degree of freedom.
jThe joint to query.
Returns
The topology (TOPOLOGY_R or TOPOLOGY_S).

Definition at line 1137 of file joint.c.

References Error(), FIX_JOINT, FREE_JOINT, IN_PATCH_JOINT, PRS_JOINT, REV_JOINT, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, Tjoint::t, TOPOLOGY_R, TOPOLOGY_S, and UNV_JOINT.

Referenced by GenerateJointEquations(), and WorldDOF2Sol().

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
jThe joint to query.
Returns
The identifier of the coupled joint of NO_UINT if the joint is not coupled.

Definition at line 1216 of file joint.c.

References Tjoint::coupled, and NO_UINT.

Referenced by WorldInitDOFInfo().

signed 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
jThe joint to query.
Returns
The degrees of freedom allowed by the joint.

Definition at line 1224 of file joint.c.

References Tjoint::coupled, Error(), FIX_JOINT, FREE_JOINT, IN_PATCH_JOINT, PRS_JOINT, REV_JOINT, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, Tjoint::t, and UNV_JOINT.

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

double GetJointLength ( Tjoint j)

Returns the distance between the two spherical joints in a spherical-spherical joint.

For other types of joints this function triggers and error.

Parameters
jThe joint to query.
Returns
The length of a spherical-spherical joint.

Definition at line 1268 of file joint.c.

References Error(), Tjoint::length, SPH_SPH_JOINT, and Tjoint::t.

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
pA set of parameters. They include, for instance the dummification level.
csThe cuiksystem where to add the variables and equations.
jThe joint from where to generate the variables and equations.

Definition at line 1276 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, SPH_PRS_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
pA set of parameters. They include, for instance the dummification level.
csThe cuiksystem where to add the variables and equations.
jThe joint from where to generate the variables and equations.

Definition at line 1463 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, ROT_JOINT_VAR_REF, SECONDARY_VAR, SetEquationCmp(), SetEquationType(), SetEquationValue(), SetVariableInterval(), SPH_JOINT, SPH_JOINT_VAR_REF, SPH_PRS_SPH_JOINT, SPH_SPH_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
pThe set of parameters.
sOrientation of the joint in the sequence (1 for positive and -1 for negative).
csThe cuiksystem where the joints are defined.
eqThree equations (X, Y, and Z) where to add the joint constraints.
jThe joint from where to generate the variables and equations.

Definition at line 1755 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::length, Tjoint::link, Tjoint::linkID, NEW, NFUN, NO_UINT, Tjoint::normals, Tjoint::points, PRS_JOINT, PRS_JOINT_VAR, REP_JOINTS, ResetMonomial(), REV_JOINT, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, SPH_SPH_JOINT_VAR, Tjoint::t, and UNV_JOINT.

Referenced by GenerateTransEquationsFromBranch().

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
pA set of parameters. They include, for instance the dummification level.
maxCoordThe maximum value for the variables in the system.
csThe cuiksystem where to add the variables and equations.
jThe joint from where to generate the variables and equations.

Definition at line 1920 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(), GenerateNormEquation(), 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, SECONDARY_VAR, SetEquationCmp(), SetEquationType(), SetEquationValue(), SetVariableInterval(), SetVariableTopology(), SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, SPH_SPH_JOINT_VAR, 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 dummy variable too. This function computes the values for the joint-related dummy variables form the non-dummy ones for a given joint.

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

Parameters
pThe set of parameters.
csThe 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.
solThe solution point with the values for the system variables. At the end of the function, this vector also has values for
jThe joint.

Definition at line 2256 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
pThe set of parameters.
csThe 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.
bThe solution box with the values for the system variables. At the end of the function, this box also has values for dummy ones.
jThe joint.

Definition at line 2292 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
pThe parameter set.
dofValues for the degrees of freedom of this joint.
t1Homogenous transform given the pose of the first link in the joint.
t2Homogenous transform given the pose of the second link in the joint.
csThe cuik system to retrive the identifier of the variables for the joint.
solSolution vector where to store the output values.
jThe joint.

Definition at line 2330 of file joint.c.

References COS_VAR, COS_VAR_UNI, CT_REPRESENTATION, DifferenceVector(), DotProduct(), Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GetCSVariableID(), GetLinkName(), GetParameter(), Tjoint::hasLimits, HTransformApply(), HTransformApplyRot(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, IN_PATCH_JOINT_SCALE_VAR, Tjoint::link, Tjoint::linkID, NEW, NO_UINT, Normalize(), Tjoint::normals, Tjoint::points, PRS_JOINT, PRS_JOINT_VAR, REP_JOINTS, REV_JOINT, ROT_JOINT_VAR_REF, SPH_JOINT, SPH_JOINT_VAR_REF, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, SPH_SPH_JOINT_VAR, 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
dofValues for the DOF of the joint.
tThe output transform.
jThe joint.

Definition at line 2527 of file joint.c.

References CrossProduct(), Error(), FIX_JOINT, FREE_JOINT, HTransformAcumRot(), HTransformCopy(), HTransformDelete(), HTransformFromVectors(), HTransformInverse(), HTransformProduct(), HTransformRx(), HTransformRy(), HTransformRz(), HTransformTx(), HTransformTxyz(), IN_PATCH_JOINT, Tjoint::length, M_PI, Normalize(), Tjoint::normals, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, REV_JOINT, RX, RZ, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_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
pThe set of parameters.
t1The transform giving the absolute pose of the first link involved in the joint.
t2The transform giving the absolute pose of the second link involved in the joint.
dofSpace where to store the computed values.
jThe joint to query.

Definition at line 2644 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(), HTransformApply(), HTransformCopy(), HTransformDelete(), HTransformFromVectors(), HTransformGetElement(), HTransformInverse(), HTransformProduct(), HTransformSetElement(), HTransformTx(), HTransformX2Vect(), IN_PATCH_JOINT, Norm(), Normalize(), Tjoint::normals, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, REV_JOINT, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, SumVectorScale(), Tjoint::t, UNV_JOINT, and ZERO.

Referenced by GetMechanismDOFsFromTransforms(), and MoveJointFromTransforms().

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
pThe set of parameters.
csThe cuiksysem on which the dof variables are defined.
tsThe resulting sequence of transforms.
jThe joint.

Definition at line 2895 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(), HTransformTx(), Tjoint::id, IN_PATCH_JOINT, InitTransSeq(), Tjoint::length, Tjoint::link, NEW, NO_UINT, Tjoint::normals, PA, Tjoint::points, Tjoint::postT, Tjoint::preT, PRS_JOINT, REP_JOINTS, REV_JOINT, RX, RY, RZ, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, SumVector(), Tjoint::t, TX, and UNV_JOINT.

Referenced by GenerateMEquationFromBranch().

double GetJointMaxCoordinate ( Tjoint j)

Returns the maximum coordinate value for all the objects in the joint.

Only spherical-spherical joints have an associated object. In this case the function returns the length of the link in between the two spherical joints. Otherwise this function returns 0.

Parameters
jThe joint to query.
Returns
The the maximum coordinate value used in the joint.

Definition at line 3130 of file joint.c.

References Tjoint::maxCoord.

Referenced by AddJoint2Mechanism().

void PlotJoint ( Tplot3d pt,
Tjoint j 
)

Adds a link to a 3d scene. Nothing is added for all joints but for spherical-spherical joints where we add a cylinder aligned with the X axis.

Parameters
ptThe 3d geometry where to add the link.
jThe joint to add to the scene.

Definition at line 3135 of file joint.c.

References Close3dObject(), Tjoint::color, CopyColor(), DeleteColor(), IntervalSize(), Tjoint::length, LowerLimit(), NO_UINT, Tjoint::obj3d, PlotCylinder(), PlotSphere(), Tjoint::rad, Tjoint::range, ScaleColor(), SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, StartNew3dObject(), and Tjoint::t.

Referenced by PlotMechanism().

void MoveJointFromTransforms ( Tparameters p,
Tplot3d pt,
THTransform t1,
THTransform t2,
Tjoint j 
)

Displaces a joint previously added to a 3d scene.

Actually, only in the case of composite joints (spherical-spherical joints) there is an effect for this function.

Composite joints (spherical-spherical joints) generate a cylinder connecting the two spherical joints. This function moves this cylinder to the position given by the start-end points of the spherical-spherical joint extracted from the link definitions, the link translation equations, and the link rotation matrixes taken from the given solution point.

Note that the rotation around the principal axis of the cylinder does not matters, that is the effect of a spherical-spherical joint.

Parameters
pThe set of parameters. Only used for debug.
ptThe 3d scene where the apply the displacement.
t1The transform giving the pose of the first link involved in the joint.
t2The transform giving the pose of the second link involved in the joint.
jThe joint to move.

Definition at line 3199 of file joint.c.

References DifferenceVector(), GetJointDOFValues(), HTransformAcumRot2(), HTransformAcumTrans(), HTransformApply(), HTransformDelete(), HTransformProduct(), HTransformX2Vect(), Move3dObject(), Norm(), Tjoint::obj3d, Tjoint::offset, Tjoint::points, Tjoint::preT, RY, RZ, SPH_PRS_SPH_JOINT, SPH_SPH_JOINT, SumVectorScale(), and Tjoint::t.

Referenced by MoveMechanismFromTransforms().

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
pThe set of parameters.
fThe file where to store the data.
csThe 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.
solThe solution point. Only values for system variables are used.
rArray with the translation from the ground link to each link.
jThe joint to query.

Definition at line 3271 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
fThe file where to store the information.
jThe joint to print.

Definition at line 3310 of file joint.c.

References Tjoint::avoidLimits, Tjoint::color, Error(), FIX_JOINT, FREE_JOINT, GetBlue(), GetGreen(), GetLinkName(), GetRed(), Tjoint::hasLimits, HTransformPrettyPrint(), IN_PATCH_JOINT, Tjoint::length, Tjoint::link, Tjoint::points, Tjoint::preT, PrintInterval(), PRS_JOINT, Tjoint::rad, Tjoint::range, Tjoint::range2, REV_JOINT, SPH_JOINT, SPH_PRS_SPH_JOINT, SPH_SPH_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
jThe joint to delete.

Definition at line 3461 of file joint.c.

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

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