1 /*
2  *_________________________________________________________________________*
3  *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
4  *      DESCRIPTION: SEE READ-ME                                           *
5  *      FILE NAME: onbody.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 ONBODY_H
19 #define ONBODY_H
20 
21 #include "poemslist.h"
22 #include "matrix.h"
23 #include "vect6.h"
24 #include "mat6x6.h"
25 
26 // emumerated type
27 enum Direction {
28   BACKWARD = 0,
29   FORWARD= 1
30 };
31 
32 class Body;
33 class InertialFrame;
34 class Joint;
35 class OnSolver;
36 
37 class OnBody {
38        Body* system_body;
39        Joint* system_joint;
40        OnBody* parent;
41        List<OnBody> children;
42 
43        Direction joint_dir;
44        void (Joint::*kinfun)();          // kinematics function
45        void (Joint::*updatesP)(Matrix&); // sP update function
46        Vect3* gamma;                     // pointer to gamma vector
47        Mat3x3* pk_C_k;                   // pointer to transformation
48 
49 
50        Mat6x6 sI;      // spatial inertias
51        Mat6x6 sIhat;   // recursive spatial inertias
52        Mat6x6 sSC;     // spatial shift
53        Mat6x6 sT;      // spatial triangularization
54 
55        Vect6 sF;       // spatial forces
56        Vect6 sFhat;    // recursive spatial forces
57        Vect6 sAhat;    // recursive spatial acceleration
58 
59        Matrix sP;      // spatial partial velocities
60        Matrix sM;      // triangularized mass matrix diagonal elements
61        Matrix sMinv;   // inverse of sM
62        Matrix sPsMinv;
63        Matrix sIhatsP;
64 
65        // states and state derivatives
66        ColMatrix* q;
67        ColMatrix* u;
68        ColMatrix* qdot;
69        ColMatrix* udot;
70        ColMatrix* qdotdot;
71 
72        ColMatrix* r;
73        ColMatrix* acc;
74        ColMatrix* ang;
75 
76   // friend classes
77   friend class OnSolver;
78 
79 
80 public:
81        OnBody();
82        ~OnBody();
83        int RecursiveSetup(InertialFrame* basebody);
84        int RecursiveSetup(int ID, OnBody* parentbody, Joint* sys_joint);
85        void RecursiveKinematics();
86        void RecursiveTriangularization();
87        void RecursiveForwardSubstitution();
88        Mat3x3 GetN_C_K();
89        Vect3 LocalCart();
90        int GetBodyID();
91        void CalculateAcceleration();
92        void Setup();
93        void SetupInertialFrame();
94        void LocalKinematics();
95                  void LocalTriangularization(Vect3& Torque, Vect3& Force);
96        void LocalForwardSubstitution();
97 };
98 
99 #endif
100