1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
|
/*
*_________________________________________________________________________*
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: onbody.h *
* AUTHORS: See Author List *
* GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement *
* DOWNLOAD: Free at www.rpi.edu/~anderk5 *
* ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab *
* Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/
#ifndef ONBODY_H
#define ONBODY_H
#include "poemslist.h"
#include "matrix.h"
#include "vect6.h"
#include "mat6x6.h"
// emumerated type
enum Direction {
BACKWARD = 0,
FORWARD= 1
};
class Body;
class InertialFrame;
class Joint;
class OnSolver;
class OnBody {
Body* system_body;
Joint* system_joint;
OnBody* parent;
List<OnBody> children;
Direction joint_dir;
void (Joint::*kinfun)(); // kinematics function
void (Joint::*updatesP)(Matrix&); // sP update function
Vect3* gamma; // pointer to gamma vector
Mat3x3* pk_C_k; // pointer to transformation
Mat6x6 sI; // spatial inertias
Mat6x6 sIhat; // recursive spatial inertias
Mat6x6 sSC; // spatial shift
Mat6x6 sT; // spatial triangularization
Vect6 sF; // spatial forces
Vect6 sFhat; // recursive spatial forces
Vect6 sAhat; // recursive spatial acceleration
Matrix sP; // spatial partial velocities
Matrix sM; // triangularized mass matrix diagonal elements
Matrix sMinv; // inverse of sM
Matrix sPsMinv;
Matrix sIhatsP;
// states and state derivatives
ColMatrix* q;
ColMatrix* u;
ColMatrix* qdot;
ColMatrix* udot;
ColMatrix* qdotdot;
ColMatrix* r;
ColMatrix* acc;
ColMatrix* ang;
// friend classes
friend class OnSolver;
public:
OnBody();
~OnBody();
int RecursiveSetup(InertialFrame* basebody);
int RecursiveSetup(int ID, OnBody* parentbody, Joint* sys_joint);
void RecursiveKinematics();
void RecursiveTriangularization();
void RecursiveForwardSubstitution();
Mat3x3 GetN_C_K();
Vect3 LocalCart();
int GetBodyID();
void CalculateAcceleration();
void Setup();
void SetupInertialFrame();
void LocalKinematics();
void LocalTriangularization(Vect3& Torque, Vect3& Force);
void LocalForwardSubstitution();
};
#endif
|
