joint.h
Go to the documentation of this file.
1 #ifndef JOINTH
2 #define JOINTH
3 
4 #include "interval.h"
5 #include "link.h"
6 #include "boolean.h"
7 #include "cuiksystem.h"
8 #include "color.h"
9 #include "htransform.h"
10 #include "eqvector.h"
11 
12 #include <stdlib.h>
13 
14 
24 /************************************************************************************/
31 #define NO_JOINT NO_UINT
32 
40 #define FREE_JOINT 0
41 
51 #define FIX_JOINT 1
52 
59 #define REV_JOINT 2
60 
67 #define UNV_JOINT 3
68 
75 #define SPH_JOINT 4
76 
98 #define SPH_SPH_JOINT 5
99 
119 #define SPH_PRS_SPH_JOINT 6
120 
127 #define PRS_JOINT 7
128 
142 #define IN_PATCH_JOINT 8
143 
144 /************************************************************************************/
145 
177 typedef struct {
178  unsigned int t;
181  unsigned int id;
186  unsigned int linkID[2];
187  Tlink *link[2];
189  double points[6][3];
203  double normals[3][3];
212  boolean hasLimits;
213  void *coupled;
216  boolean avoidLimits;
223  double offset;
226  double offset2;
227  double vrange[2][3];
230  unsigned int obj3d[2];
234  double maxCoord;
235  double rad;
239  double length;
252 } Tjoint;
253 
274 void NewFreeJoint(unsigned int id,
275  unsigned int linkID1,Tlink *link1,
276  unsigned int linkID2,Tlink *link2,
277  Tjoint *j);
278 
296 void NewFixJoint(unsigned int id,
297  unsigned int linkID1,Tlink *link1,
298  unsigned int linkID2,Tlink *link2,
299  THTransform *t,
300  Tjoint *j);
345 void NewRevoluteJoint(unsigned int id,unsigned int r,
346  unsigned int linkID1,Tlink *link1,
347  unsigned int linkID2,Tlink *link2,
348  double **points,
349  boolean hasLimits,Tinterval *range,double **rPoints,
350  boolean avoidLimits,double avoidLimitsWeight,
351  Tjoint *coupled,
352  Tjoint *j);
387 void NewUniversalJoint(unsigned int id,unsigned int r,
388  unsigned int linkID1,Tlink *link1,
389  unsigned int linkID2,Tlink *link2,
390  double **points,
391  boolean hasLimits,Tinterval *range1,Tinterval *range2,double **rPoints,
392  boolean avoidLimits,double avoidLimitsWeight,
393  Tjoint *j);
394 
428 void NewSphericalJoint(unsigned int id,
429  unsigned int linkID1,Tlink *link1,
430  unsigned int linkID2,Tlink *link2,
431  double **points,
432  boolean hasLimits,double range,double **rPoints,
433  boolean avoidLimits,double avoidLimitsWeight,
434  Tjoint *j);
435 
464 void NewPrismaticJoint(unsigned int id,
465  unsigned int linkID1,Tlink *link1,
466  unsigned int linkID2,Tlink *link2,
467  double **points,
468  Tinterval *range,
469  boolean avoidLimits,double avoidLimitsWeight,
470  Tjoint *j);
493 void NewSphSphJoint(unsigned int id,
494  unsigned int linkID1,Tlink *link1,
495  unsigned int linkID2,Tlink *link2,
496  double **points,
497  double l,double r,
498  Tcolor *color,
499  Tjoint *j);
520 void NewSphPrsSphJoint(unsigned int id,
521  unsigned int linkID1,Tlink *link1,
522  unsigned int linkID2,Tlink *link2,
523  double **points,
524  Tinterval *range,double r,
525  Tcolor *color,
526  Tjoint *j);
527 
557 void NewInPatchJoint(unsigned int id,
558  unsigned int linkID1,Tlink *link1,
559  unsigned int linkID2,Tlink *link2,
560  double **points,
561  double **patch,
562  boolean avoidLimits,double avoidLimitsWeight,
563  Tjoint *j);
564 
573 void CopyJoint(Tjoint *j_dst,Tjoint *j_src);
574 
584 unsigned int GetJointType(Tjoint *j);
585 
595 unsigned int GetJointID(Tjoint *j);
596 
606 unsigned int JointFromID(Tjoint *j);
607 
617 Tlink *JointFrom(Tjoint *j);
618 
628 unsigned int JointToID(Tjoint *j);
629 
630 
640 Tlink *JointTo(Tjoint *j);
641 
653 void GetJointName(char **name,Tjoint *j);
654 
667 boolean IsJointAllSpheres(Tjoint *j);
668 
683 void GetJointPoint(unsigned int link,unsigned int point,double *p,Tjoint *j);
684 
685 
695 boolean LimitedJoint(Tjoint *j);
696 
697 
709 
723 
724 
735 void GetJointRangeN(unsigned int nr,double mt,Tinterval *r,Tjoint *j);
736 
747 unsigned int GetJointRangeTopology(unsigned int nr,Tjoint *j);
748 
769 unsigned int CoupledWith(Tjoint *j);
770 
782 signed int GetJointDOF(Tjoint *j);
783 
796 double GetJointLength(Tjoint *j);
797 
798 
812 
830 
831 
856  Tequation *eq,Tjoint *j);
857 
871 void GenerateJointEquations(Tparameters *p,double maxCoord,
872  TCuikSystem *cs,Tjoint *j);
873 
874 
896 void RegenerateJointSolution(Tparameters *p,TCuikSystem *cs,double *sol,Tjoint *j);
897 
915 
943 void GenerateJointSolution(Tparameters *p,double *dof,THTransform *t1,THTransform *t2,
944  TCuikSystem *cs,double *sol,Tjoint *j);
945 
946 
960 void GetJointTransform(double *dof,THTransform *t,Tjoint *j);
961 
988 void GetJointDOFValues(Tparameters *p,THTransform *t1,THTransform *t2,double *dof,
989  Tjoint *j);
990 
991 
1004 
1018 double GetJointMaxCoordinate(Tjoint *j);
1019 
1030 void PlotJoint(Tplot3d *pt,Tjoint *j);
1031 
1032 
1059  Tplot3d *pt,THTransform *t1,THTransform *t2,Tjoint *j);
1060 
1078 void PrintJointAxes(Tparameters *p,FILE *f,TCuikSystem *cs,double *sol,
1079  double *r,Tjoint *j);
1080 
1090 void PrintJoint(FILE *f,Tjoint *j);
1091 
1099 void DeleteJoint(Tjoint *j);
1100 
1101 #endif
Definition of the boolean type.
void GetJointTransform(double *dof, THTransform *t, Tjoint *j)
Computes the transform induced by the joint.
Definition: joint.c:2527
void RegenerateJointSolution(Tparameters *p, TCuikSystem *cs, double *sol, Tjoint *j)
Computes the values for the dummy variables used in the joint equations.
Definition: joint.c:2256
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.
Definition: joint.c:1012
signed int GetJointDOF(Tjoint *j)
Computes the degrees of freedom allowed by a given joint.
Definition: joint.c:1224
double GetJointMaxCoordinate(Tjoint *j)
Returns the maximum coordinate value for all the objects in the joint.
Definition: joint.c:3130
void GetJointDOFValues(Tparameters *p, THTransform *t1, THTransform *t2, double *dof, Tjoint *j)
Recovers the joint DOFs from the absolute poses of the links.
Definition: joint.c:2644
double maxCoord
Definition: joint.h:234
Relation between two links.
Definition: joint.h:177
boolean avoidLimits
Definition: joint.h:216
A homgeneous transform in R^3.
boolean hasLimits
Definition: joint.h:212
Tinterval * GetJointSecondRange(Tjoint *j)
Checks the second limit of a universal joint.
Definition: joint.c:1032
Tlink * JointFrom(Tjoint *j)
Gets a pointer to the first link involved in the joint.
Definition: joint.c:946
unsigned int CoupledWith(Tjoint *j)
Returns the identifier of the joint coupled with the query joint.
Definition: joint.c:1216
double offset2
Definition: joint.h:226
A color.
Definition: color.h:23
Definition of the TEqVEctor type and the associated functions.
double length
Definition: joint.h:239
void GenerateJointEquationsInBranch(Tparameters *p, double s, TCuikSystem *cs, Tequation *eq, Tjoint *j)
Generate the constraints of a joint in a sequence.
Definition: joint.c:1755
void GetJointName(char **name, Tjoint *j)
Returns a string identifying the joint.
Definition: joint.c:961
Tlink * JointTo(Tjoint *j)
Gets a pointer to the second link involved in the joint.
Definition: joint.c:956
unsigned int GetJointID(Tjoint *j)
Gets the joint identifier.
Definition: joint.c:936
A 3D plot.
Definition: plot3d.h:54
Tinterval range
Definition: joint.h:220
void NewFreeJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, Tjoint *j)
Constructor.
Definition: joint.c:101
unsigned int id
Definition: joint.h:181
An equation.
Definition: equation.h:236
void * coupled
Definition: joint.h:213
THTransform postT
Definition: joint.h:248
unsigned int GetJointRangeTopology(unsigned int nr, Tjoint *j)
Returns the topology of one of the ranges of the joint.
Definition: joint.c:1137
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.
Definition: joint.c:132
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.
Definition: joint.c:288
void PrintJointAxes(Tparameters *p, FILE *f, TCuikSystem *cs, double *sol, double *r, Tjoint *j)
Prints the joint axis in world coordinates.
Definition: joint.c:3271
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.
Definition: joint.c:709
boolean LimitedJoint(Tjoint *j)
Checks if a joint has limits.
Definition: joint.c:1022
Tcolor color
Definition: joint.h:241
unsigned int GetJointType(Tjoint *j)
Gets the joint type.
Definition: joint.c:931
A table of parameters.
Definition of the TCuikSystem type and the associated functions.
THTransform preT
Definition: joint.h:244
Definition of the THTransform type and the associated functions.
void MoveJointFromTransforms(Tparameters *p, Tplot3d *pt, THTransform *t1, THTransform *t2, Tjoint *j)
Displaces a joint in a 3d scene.
Definition: joint.c:3199
A sequence of transforms.
Definition: trans_seq.h:296
void NewFixJoint(unsigned int id, unsigned int linkID1, Tlink *link1, unsigned int linkID2, Tlink *link2, THTransform *t, Tjoint *j)
Constructor.
Definition: joint.c:110
void GetJointTransSeq(Tparameters *p, TCuikSystem *cs, TTransSeq *ts, Tjoint *j)
Build the sequence of transforms passing through the joint.
Definition: joint.c:2895
A box.
Definition: box.h:83
void PlotJoint(Tplot3d *pt, Tjoint *j)
Adds a joint to a 3d scene.
Definition: joint.c:3135
void PrintJoint(FILE *f, Tjoint *j)
Stores the joint information into a file.
Definition: joint.c:3310
A cuiksystem, i.e., a set of variables and equations defining a position analysis problem...
Definition: cuiksystem.h:181
Definition of the Tcolor type and the associated functions.
Tinterval normRange
Definition: joint.h:209
unsigned int JointFromID(Tjoint *j)
Gets the identifier of the first link involved in the joint.
Definition: joint.c:941
void GenerateJointSolution(Tparameters *p, double *dof, THTransform *t1, THTransform *t2, TCuikSystem *cs, double *sol, Tjoint *j)
Generates a solution point from degrees of freedom.
Definition: joint.c:2330
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.
Definition: joint.c:652
double rad
Definition: joint.h:235
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.
Definition: joint.c:452
double avoidLimitsWeight
Definition: joint.h:218
unsigned int t
Definition: joint.h:178
double offset
Definition: joint.h:223
boolean IsJointAllSpheres(Tjoint *j)
Identifies joint formed only by spheres.
Definition: joint.c:1004
void GenerateJointRangeSingularityEquations(Tparameters *p, TCuikSystem *cs, Tjoint *j)
Modifies the cuik system to detect end range singularities.
Definition: joint.c:1276
double GetJointLength(Tjoint *j)
Returns the length of a spherical-spherical joint.
Definition: joint.c:1268
Tinterval range2
Definition: joint.h:221
Defines a interval.
Definition: interval.h:33
Tinterval * GetJointRange(Tjoint *j)
Checks the limits of a joint.
Definition: joint.c:1027
void DeleteJoint(Tjoint *j)
Destructor.
Definition: joint.c:3461
void GenerateJointRangeEquations(Tparameters *p, TCuikSystem *cs, Tjoint *j)
Generate the constraints related with the joint limits.
Definition: joint.c:1463
void CopyJoint(Tjoint *j_dst, Tjoint *j_src)
Copy constructor.
Definition: joint.c:873
void GenerateJointEquations(Tparameters *p, double maxCoord, TCuikSystem *cs, Tjoint *j)
Generate the constraints related with the joint.
Definition: joint.c:1920
void GetJointRangeN(unsigned int nr, double mt, Tinterval *r, Tjoint *j)
Returns one of the ranges of the joint.
Definition: joint.c:1037
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.
Definition: joint.c:541
unsigned int JointToID(Tjoint *j)
Gets the identifier of the second link involved in the joint.
Definition: joint.c:951
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.
Definition: joint.c:609
Definition of the Tinterval type and the associated functions.
void RegenerateJointBox(Tparameters *p, TCuikSystem *cs, Tbox *b, Tjoint *j)
Computes the values for the dummy variables used in the joint equations.
Definition: joint.c:2292