1 /*
2  *_________________________________________________________________________*
3  *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
4  *      DESCRIPTION: SEE READ-ME                                           *
5  *      FILE NAME: joint.h                                                 *
6  *      AUTHORS: See Author List                                           *
7  *      GRANTS: See Grants List                                            *
8  *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
9  *      LICENSE: Please see License Agreement                              *
10  *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
11  *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
12  *                     Computational Dynamics Lab                          *
13  *                     Rensselaer Polytechnic Institute                    *
14  *                     110 8th St. Troy NY 12180                           *
15  *      CONTACT:        anderk5@rpi.edu                                    *
16  *_________________________________________________________________________*/
17 
18 #ifndef JOINT_H
19 #define JOINT_H
20 
21 #include "poemsobject.h"
22 #include <iostream>
23 #include "matrices.h"
24 
25 enum JointType {
26   XYZJOINT = 0,
27   FREEBODYJOINT = 1,
28   REVOLUTEJOINT = 2,
29   PRISMATICJOINT = 3,
30   SPHERICALJOINT = 4,
31   BODY23JOINT = 5,
32   MIXEDJOINT = 6
33 };
34 
35 class Body;
36 class Point;
37 
38 class Joint : public POEMSObject {
39 protected:
40   Body* body1;
41   Body* body2;
42   Point* point1;
43   Point* point2;
44 
45   ColMatrix qo;     // generalized coordinates (initial value)
46   ColMatrix uo;     // generalized speeds (initial value)
47   ColMatrix qdoto;     // generalized speeds (initial value)
48 
49   ColMatrix q;      // generalized coordinates
50   ColMatrix u;      // generalized speeds
51   ColMatrix qdot;   // generalized coordinate derivatives
52   ColMatrix udot;   // generalized speed derivatives
53   ColMatrix qdotdot;
54 
55   Mat3x3 pk_C_ko;   // transformation relationship for q = 0
56 
57   Mat3x3 pk_C_k;  // local transform
58   Mat3x3 k_C_pk;
59 
60   Vect3 r12;
61   Vect3 r21;
62 
63 public:
64 
65   Joint();
66   virtual ~Joint();
67   virtual JointType GetType() = 0;
68 
69   void SetBodies(Body* b1, Body* b2);
70   void SetPoints(Point* p1, Point* p2);
71 
72   int GetBodyID1();
73   int GetBodyID2();
74   int GetPointID1();
75   int GetPointID2();
76 
77   ColMatrix* GetQ();
78   ColMatrix* GetU();
79   ColMatrix* GetQdot();
80   ColMatrix* GetUdot();
81   ColMatrix* GetQdotdot();
82   /*ColMatrix* GetAcc();
83   ColMatrix* GetAng();*/
84 
85   void DimQandU(int i);
86   void DimQandU(int i, int j);
87 
88   Body* GetBody1();
89   Body* GetBody2();
90   Body* OtherBody(Body* body);
91 
92   Vect3* GetR12();
93   Vect3* GetR21();
94   Mat3x3* Get_pkCk();
95   Mat3x3* Get_kCpk();
96 
97   //void SetInitialState(VirtualMatrix& q, VirtualMatrix& u);
98   void SetInitialState(ColMatrix& q, ColMatrix& u);
99   void SetZeroOrientation(VirtualMatrix& C);
100   void ResetQdot();
101   void ResetQ();
102   bool ReadIn(std::istream& in);
103   void WriteOut(std::ostream& out);
104 
105   virtual void WriteOutJointData(std::ostream& out) = 0;
106   virtual bool ReadInJointData(std::istream& in) = 0;
107   virtual Matrix GetForward_sP();
108   virtual Matrix GetBackward_sP();
109   virtual void UpdateForward_sP(Matrix& sP);
110   virtual void UpdateBackward_sP(Matrix& sP);
111   virtual void ComputeForwardTransforms();
112   virtual void ComputeBackwardTransforms();
113   virtual void ComputeLocalTransform()=0;
114   virtual void ComputeForwardGlobalTransform();
115   virtual void ComputeBackwardGlobalTransform();
116   virtual void ForwardKinematics()=0;
117   virtual void BackwardKinematics()=0;
118 };
119 
120 // global joint functions
121 Joint* NewJoint(int type);
122 
123 #endif
124