1 /*
2 *_________________________________________________________________________*
3 * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
4 * DESCRIPTION: SEE READ-ME *
5 * FILE NAME: onbody.cpp *
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 #include "onbody.h"
19 #include "body.h"
20 #include "inertialframe.h"
21 #include "joint.h"
22 #include "onfunctions.h"
23 #include "virtualmatrix.h"
24 #include "matrixfun.h"
25 #include <iostream>
26 #include "norm.h"
27 #include "eulerparameters.h"
28
29 using namespace std;
30
31
OnBody()32 OnBody::OnBody(){
33 system_body = 0;
34 system_joint = 0;
35 parent = 0;
36
37 // these terms have zeros which are NEVER overwritten
38 sI.Zeros();
39 sSC.Zeros();
40 }
41
~OnBody()42 OnBody::~OnBody(){
43 children.DeleteValues();
44 }
45
RecursiveSetup(InertialFrame * basebody)46 int OnBody::RecursiveSetup (InertialFrame* basebody){
47 int ID = 0;
48 system_body = basebody;
49
50 // record that the traversal algorithm has been here
51 if( system_body->GetID() ) return 0;
52 ID++;
53 system_body->SetID(ID);
54
55 // setup inertial frame
56 SetupInertialFrame();
57
58 Joint* joint;
59 OnBody* child;
60 int tempID;
61
62 // loop through children calling the recursive setup function
63 ListElement<Joint>* ele = system_body->joints.GetHeadElement();
64 while(ele){
65 joint = ele->value;
66 child = new OnBody;
67
68 tempID = child->RecursiveSetup(ID,this,joint);
69 if( tempID ){
70 children.Append(child);
71 ID = tempID;
72 }
73 else delete child;
74
75 ele = ele->next;
76 }
77
78 return ID;
79 }
80
RecursiveSetup(int ID,OnBody * parentbody,Joint * sys_joint)81 int OnBody::RecursiveSetup(int ID, OnBody* parentbody, Joint* sys_joint){
82 // initialize the topology and system analogs
83 parent = parentbody;
84 system_joint = sys_joint;
85 system_body = system_joint->OtherBody(parentbody->system_body);
86
87
88 // record that the traversal algorithm has been here
89 if( system_body->GetID() ) return 0;
90 ID++;
91
92 // perform the local setup operations
93 Setup();
94
95 Joint* joint;
96 OnBody* child;
97 int tempID;
98
99 // loop through children calling the recursive setup function
100 ListElement<Joint>* ele = system_body->joints.GetHeadElement();
101 while(ele){
102 joint = ele->value;
103 if(joint != sys_joint){
104 child = new OnBody;
105
106 tempID = child->RecursiveSetup(ID,this,joint);
107 if( tempID ){
108 children.Append(child);
109 ID = tempID;
110 }
111 else delete child;
112 }
113
114 ele = ele->next;
115 }
116
117 return ID;
118 }
119
SetupInertialFrame()120 void OnBody::SetupInertialFrame(){
121 // error check
122 if(system_body->GetType() != INERTIALFRAME){
123 cerr << "ERROR: attempting to setup inertial frame for non-inertial body" << endl;
124 exit(1);
125 }
126
127 // setup gravity
128 Vect3 neg_gravity = -((InertialFrame*) system_body)->GetGravity();
129 sAhat.Zeros();
130 Set6DLinearVector(sAhat,neg_gravity);
131
132 }
133
134
Setup()135 void OnBody::Setup(){
136
137 // get the direction of the joint
138 if( system_joint->GetBody2() == system_body ) joint_dir = FORWARD;
139 else joint_dir = BACKWARD;
140
141 // set the function pointers for the joint direction
142 if( joint_dir == FORWARD ){
143 kinfun = &Joint::ForwardKinematics;
144 updatesP = &Joint::UpdateForward_sP;
145 gamma = system_joint->GetR12();
146 pk_C_k = system_joint->Get_pkCk();
147 } else {
148 kinfun = &Joint::BackwardKinematics;
149 updatesP = &Joint::UpdateBackward_sP;
150 gamma = system_joint->GetR21();
151 pk_C_k = system_joint->Get_kCpk();
152 }
153
154 // initialize variables and dimensions
155
156 // sI
157 OnPopulateSI(system_body->inertia, system_body->mass, sI);
158
159 // sP
160 if( joint_dir == FORWARD )
161 sP = system_joint->GetForward_sP();
162 else
163 sP = system_joint->GetBackward_sP();
164
165 // dimension these quantities
166 sM = T(sP)*sP;
167 sMinv = sM;
168 sPsMinv = sP;
169 sIhatsP = sP;
170
171
172 // get the state and state derivative column matrix pointers
173 q = system_joint->GetQ();
174 u = system_joint->GetU();
175 qdot = system_joint->GetQdot();
176 udot = system_joint->GetUdot();
177 qdotdot = system_joint->GetQdotdot();
178 }
179
RecursiveKinematics()180 void OnBody::RecursiveKinematics(){
181 OnBody* child;
182 // Perform local kinematics recursively outward
183 ListElement<OnBody>* ele = children.GetHeadElement();
184 while(ele){
185 child = ele->value;
186 child->LocalKinematics();
187 child->RecursiveKinematics();
188 Mat3x3 result=*child->pk_C_k;
189 ele = ele->next;
190 }
191
192 }
193
RecursiveTriangularization()194 void OnBody::RecursiveTriangularization(){
195 OnBody* child;
196
197 // Perform local triangularization recursively inward
198 ListElement<OnBody>* ele = children.GetHeadElement();
199 while(ele){
200 child = ele->value;
201 child->RecursiveTriangularization();
202 //child->LocalTriangularization();
203 ele = ele->next;
204 }
205
206 }
207
RecursiveForwardSubstitution()208 void OnBody::RecursiveForwardSubstitution(){
209 OnBody* child;
210 // Perform local forward substitution recursively outward
211 ListElement<OnBody>* ele = children.GetHeadElement();
212 while(ele){
213 child = ele->value;
214 // child->LocalForwardSubstitution();
215 child->RecursiveForwardSubstitution();
216 ele = ele->next;
217 }
218 }
219
LocalKinematics()220 void OnBody::LocalKinematics(){
221 // do the directional kinematics
222 (system_joint->*kinfun)();
223 (system_joint->*updatesP)( sP );
224 OnPopulateSC( *gamma, *pk_C_k, sSC );
225 }
226
GetN_C_K()227 Mat3x3 OnBody::GetN_C_K(){
228 Mat3x3 nck=system_body->n_C_k;
229 return nck;
230 }
231
232
GetBodyID()233 int OnBody::GetBodyID(){
234 int ID=system_body->GetID();
235 return ID;
236 }
237
LocalCart()238 Vect3 OnBody::LocalCart(){
239 (system_joint->*kinfun)();
240 Vect3 RR=system_body->r;
241 return RR;
242 }
243
244
245
LocalTriangularization(Vect3 & Torque,Vect3 & Force)246 void OnBody::LocalTriangularization(Vect3& Torque, Vect3& Force){
247
248 Vect3 Iw,wIw,Ialpha,wIwIalpha,ma,Bforce,Bforce_ma,Btorque,Btorque_wIwIalpha;
249 Iw.Zeros();wIw.Zeros();wIwIalpha.Zeros();ma.Zeros();
250 Bforce.Zeros();Bforce_ma.Zeros();Btorque.Zeros();Btorque_wIwIalpha.Zeros();
251
252 FastMult(system_body->inertia,system_body->omega_k,Iw);
253 FastCross(Iw,system_body->omega_k,wIw);
254
255 FastMult(system_body->inertia, system_body->alpha_t, Ialpha);
256 FastSubt(wIw,Ialpha,wIwIalpha);
257 FastMult(-system_body->mass,system_body->a_t,ma);
258
259
260 Mat3x3 k_C_n=T(system_body->n_C_k);
261 Bforce=k_C_n*Force;
262 Btorque=k_C_n*Torque;
263
264 FastAdd(Bforce,ma,Bforce_ma);
265 FastAdd(Btorque,wIwIalpha,Btorque_wIwIalpha);
266 OnPopulateSVect(Btorque_wIwIalpha,Bforce_ma,sF);
267
268
269 sIhat = sI;
270 sFhat = sF;
271 Mat6x6 sPsMinvsPT;
272 Mat6x6 sIhatsPsMsPT;
273 Mat6x6 sUsIhatsPsMsPT;
274 Mat6x6 sTsIhat;
275 Mat6x6 sTsIhatsSCT;
276 Vect6 sTsFhat;
277 Mat6x6 sU;
278 sU.Identity();
279
280 OnBody* child;
281 ListElement<OnBody>* ele = children.GetHeadElement();
282
283 while(ele){
284 child = ele->value;
285
286 FastMultT(child->sIhatsP,child->sPsMinv,sIhatsPsMsPT);
287 FastSubt(sU,sIhatsPsMsPT,sUsIhatsPsMsPT);
288 FastMult(child->sSC,sUsIhatsPsMsPT,child->sT);
289
290 FastMult(child->sT,child->sIhat,sTsIhat);
291 FastMultT(sTsIhat,child->sSC,sTsIhatsSCT);
292 FastAdd(sIhat,sTsIhatsSCT,sIhat);
293
294 FastMult(child->sT,child->sFhat,sTsFhat);
295 FastAdd(sFhat,sTsFhat,sFhat);
296 ele = ele->next;
297 }
298
299 FastMult(sIhat,sP,sIhatsP);
300 FastTMult(sP,sIhatsP,sM);
301 sMinv=SymInverse(sM);
302 FastMult(sP,sMinv,sPsMinv);
303 }
304
LocalForwardSubstitution()305 void OnBody::LocalForwardSubstitution(){
306 Vect6 sSCTsAhat;
307 Vect6 sIhatsSCTsAhat;
308 Vect6 sFsIhatsSCTsAhat;
309 Vect6 sPudot;
310 int type = system_joint->GetType();
311 if(type == 1){
312 FastTMult(sSC,parent->sAhat,sSCTsAhat);
313 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
314 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
315 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
316
317 ColMatrix result1=*udot;
318 ColMatrix result2=*qdot;
319 ColMatrix result3=*q;
320 int num=result1.GetNumRows();
321 ColMatrix result4(num+1);
322 result4.Zeros();
323 EPdotdot_udot(result1, result2, result3, result4);
324 FastAssign(result4, *qdotdot);
325 FastMult(sP,*udot,sPudot);
326 FastAdd(sSCTsAhat,sPudot,sAhat);
327 }
328 else if (type == 4){
329 FastTMult(sSC,parent->sAhat,sSCTsAhat);
330 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
331 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
332 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
333
334 ColMatrix result1=*udot;
335 ColMatrix result2=*qdot;
336 ColMatrix result3=*q;
337 int num=result1.GetNumRows();
338 ColMatrix result4(num+1);
339 result4.Zeros();
340
341 EPdotdot_udot(result1, result2, result3, result4);
342 FastAssign(result4, *qdotdot);
343 FastMult(sP,*udot,sPudot);
344 FastAdd(sSCTsAhat,sPudot,sAhat);
345 }
346 else if (type == 5){
347 FastTMult(sSC,parent->sAhat,sSCTsAhat);
348 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
349 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
350 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
351 ColMatrix temp_u = *udot;
352
353 ColMatrix result1(3);
354 result1(1)=0.0;
355 result1(2)=temp_u(1);
356 result1(3)=temp_u(2);
357 ColMatrix result2=*qdot;
358 ColMatrix result3=*q;
359 int num=result1.GetNumRows();
360 ColMatrix result4(num+1);
361 result4.Zeros();
362
363 EPdotdot_udot(result1, result2, result3, result4);
364 FastAssign(result4, *qdotdot);
365 FastMult(sP,*udot,sPudot);
366 FastAdd(sSCTsAhat,sPudot,sAhat);
367 }
368 else if (type == 6){
369 FastTMult(sSC,parent->sAhat,sSCTsAhat);
370 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
371 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
372 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
373 ColMatrix temp_u = *udot;
374 int tt = temp_u.GetNumRows() + 1;
375 ColMatrix result1(tt);
376 result1(1)=0.0;
377 for (int k =2; k<=tt; k++){
378 result1(k)=temp_u(k-1);
379 }
380 ColMatrix result2=*qdot;
381 ColMatrix result3=*q;
382 int num=result1.GetNumRows();
383 ColMatrix result4(num+1);
384 result4.Zeros();
385
386 EPdotdot_udot(result1, result2, result3, result4);
387 FastAssign(result4, *qdotdot);
388 FastMult(sP,*udot,sPudot);
389 FastAdd(sSCTsAhat,sPudot,sAhat);
390 }
391 else{
392 cout<<"Joint type not recognized in onbody.cpp LocalForwardSubsitution() "<<type<<endl;
393 exit(-1);
394 }
395 CalculateAcceleration();
396
397 }
398
399
CalculateAcceleration()400 void OnBody::CalculateAcceleration(){
401 }
402