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 //cout<<" what is system_body->mass "<< system_body->mass <<endl;
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,BTtorque;
249 Iw.Zeros();wIw.Zeros();wIwIalpha.Zeros();ma.Zeros();
250 Bforce.Zeros();Bforce_ma.Zeros();Btorque.Zeros();Btorque_wIwIalpha.Zeros(),BTtorque.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 FastAdd(system_body->btorque,system_body->ttorque,BTtorque);//added the body bending and twisting torques here,
264 FastAdd(Btorque,BTtorque,Btorque);//varified and validated by mingqiu already
265 //cout<<" what is Btorque= "<< Btorque <<endl;
266 FastAdd(Bforce,ma,Bforce_ma);
267 FastAdd(Btorque,wIwIalpha,Btorque_wIwIalpha);
268 OnPopulateSVect(Btorque_wIwIalpha,Bforce_ma,sF);
269
270
271 sIhat = sI;
272 sFhat = sF;
273 Mat6x6 sPsMinvsPT;
274 Mat6x6 sIhatsPsMsPT;
275 Mat6x6 sUsIhatsPsMsPT;
276 Mat6x6 sTsIhat;
277 Mat6x6 sTsIhatsSCT;
278 Vect6 sTsFhat;
279 Mat6x6 sU;
280 sU.Identity();
281
282 OnBody* child;
283 ListElement<OnBody>* ele = children.GetHeadElement();
284
285 while(ele){
286 child = ele->value;
287
288 FastMultT(child->sIhatsP,child->sPsMinv,sIhatsPsMsPT);
289 FastSubt(sU,sIhatsPsMsPT,sUsIhatsPsMsPT);
290 FastMult(child->sSC,sUsIhatsPsMsPT,child->sT);
291
292 FastMult(child->sT,child->sIhat,sTsIhat);
293 FastMultT(sTsIhat,child->sSC,sTsIhatsSCT);
294 FastAdd(sIhat,sTsIhatsSCT,sIhat);
295
296 FastMult(child->sT,child->sFhat,sTsFhat);
297 FastAdd(sFhat,sTsFhat,sFhat);
298 ele = ele->next;
299 }
300
301 FastMult(sIhat,sP,sIhatsP);
302 FastTMult(sP,sIhatsP,sM);
303 sMinv=SymInverse(sM);
304 FastMult(sP,sMinv,sPsMinv);
305 }
306
LocalForwardSubstitution()307 void OnBody::LocalForwardSubstitution(){
308 Vect6 sSCTsAhat;
309 Vect6 sIhatsSCTsAhat;
310 Vect6 sFsIhatsSCTsAhat;
311 Vect6 sPudot;
312 int type = system_joint->GetType();
313 //cout<<" what is type= "<< type <<endl;
314 if(type == 1){
315 FastTMult(sSC,parent->sAhat,sSCTsAhat);
316 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
317 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
318 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
319
320 ColMatrix result1=*udot;
321 ColMatrix result2=*qdot;
322 ColMatrix result3=*q;
323 int num=result1.GetNumRows();
324 ColMatrix result4(num+1);
325 result4.Zeros();
326 EPdotdot_udot(result1, result2, result3, result4);
327 FastAssign(result4, *qdotdot);
328 FastMult(sP,*udot,sPudot);
329 FastAdd(sSCTsAhat,sPudot,sAhat);
330 }
331 else if (type == 4){
332 FastTMult(sSC,parent->sAhat,sSCTsAhat);
333 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
334 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
335 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
336
337 ColMatrix result1=*udot;
338 ColMatrix result2=*qdot;
339 ColMatrix result3=*q;
340 int num=result1.GetNumRows();
341 ColMatrix result4(num+1);
342 result4.Zeros();
343
344 EPdotdot_udot(result1, result2, result3, result4);
345 FastAssign(result4, *qdotdot);
346 FastMult(sP,*udot,sPudot);
347 FastAdd(sSCTsAhat,sPudot,sAhat);
348 }
349 else if (type == 5){
350 FastTMult(sSC,parent->sAhat,sSCTsAhat);
351 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
352 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
353 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
354 ColMatrix temp_u = *udot;
355
356 ColMatrix result1(3);
357 result1(1)=0.0;
358 result1(2)=temp_u(1);
359 result1(3)=temp_u(2);
360 ColMatrix result2=*qdot;
361 ColMatrix result3=*q;
362 int num=result1.GetNumRows();
363 ColMatrix result4(num+1);
364 result4.Zeros();
365
366 EPdotdot_udot(result1, result2, result3, result4);
367 FastAssign(result4, *qdotdot);
368 FastMult(sP,*udot,sPudot);
369 FastAdd(sSCTsAhat,sPudot,sAhat);
370 }
371 else if (type == 6){
372 FastTMult(sSC,parent->sAhat,sSCTsAhat);
373 FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);
374 FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
375 FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
376 ColMatrix temp_u = *udot;
377 int tt = temp_u.GetNumRows() + 1;
378 ColMatrix result1(tt);
379 result1(1)=0.0;
380 for (int k =2; k<=tt; k++){
381 result1(k)=temp_u(k-1);
382 }
383 ColMatrix result2=*qdot;
384 ColMatrix result3=*q;
385 int num=result1.GetNumRows();
386 ColMatrix result4(num+1);
387 result4.Zeros();
388
389 EPdotdot_udot(result1, result2, result3, result4);
390 FastAssign(result4, *qdotdot);
391 FastMult(sP,*udot,sPudot);
392 FastAdd(sSCTsAhat,sPudot,sAhat);
393 }
394 else{
395 cout<<"Joint type not recognized in onbody.cpp LocalForwardSubsitution() "<<type<<endl;
396 exit(-1);
397 }
398 CalculateAcceleration();
399
400 }
401
402
CalculateAcceleration()403 void OnBody::CalculateAcceleration(){
404 }
405