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