1 /*****************************************************************************
2 * DynaMechs: A Multibody Dynamic Simulation Library
3 *
4 * Copyright (C) 1994-2001 Scott McMillan All Rights Reserved.
5 *
6 * This library is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This library is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 * General Public License for more details.
15 *
16 * You should have received a copy of the GNU Library General Public
17 * License along with this library; if not, write to the Free
18 * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 *****************************************************************************
20 * File: dmRigidBody.cpp
21 * Author: Scott McMillan
22 * Summary: Class declaration for rigid bodies
23 *****************************************************************************/
24
25 #include <dm.h>
26 #include <dmRigidBody.hpp>
27 #include <dmForce.hpp>
28 #include <dmEnvironment.hpp>
29
30 //============================================================================
31 // class dmRigidBody
32 //============================================================================
33
34 //----------------------------------------------------------------------------
35 // Summary: default constructor - builds a generic rigid body with arbitrary
36 // (but valid) inertia parameters.
37 // Parameters:
38 // Returns:
39 //----------------------------------------------------------------------------
dmRigidBody()40 dmRigidBody::dmRigidBody() : dmLink()
41 {
42 m_mass = 1.0;
43
44 m_I_bar[0][0] = 1.0; m_I_bar[0][1] = 0.0; m_I_bar[0][2] = 0.0;
45 m_I_bar[1][0] = 0.0; m_I_bar[1][1] = 1.0; m_I_bar[1][2] = 0.0;
46 m_I_bar[2][0] = 0.0; m_I_bar[2][1] = 0.0; m_I_bar[2][2] = 1.0;
47
48 m_cg_pos[0] = m_cg_pos[1] = m_cg_pos[2] = 0.0;
49
50 for (int k=0; k<6; k++) m_external_force[k] = 0.0;
51
52 #ifdef DM_HYDRODYNAMICS
53 m_displaced_fluid_vol = 0.0;
54 m_displaced_fluid_mass = 0.0;
55 m_cb_pos[0] = m_cb_pos[1] = m_cb_pos[2] = 0.0;
56
57 for (int i=0; i<6; i++)
58 {
59 m_Cd_A_p[i] = 0.0;
60
61 for (int j=0; j<6; j++)
62 m_I_added_mass[i][j] = 0.0;
63 }
64
65 m_axis = 0;
66 m_x0 = 0.0;
67 m_length = 0.0;
68 m_radius = 0.0;
69 m_Ca = 0.0;
70 m_C2rl = 0.0;
71 #endif
72 }
73
74
75 //----------------------------------------------------------------------------
~dmRigidBody()76 dmRigidBody::~dmRigidBody()
77 {
78 }
79
80 //----------------------------------------------------------------------------
setInertiaParameters(Float mass,CartesianTensor I_bar,CartesianVector cg_pos)81 bool dmRigidBody::setInertiaParameters(Float mass,
82 CartesianTensor I_bar,
83 CartesianVector cg_pos)
84 {
85 register int i,j;
86
87 // Verify positive-definiteness of spatial Inertia matrix before going on:
88 SpatialTensor I_star;
89
90 for (j = 0; j < 6; j++)
91 for (i = 0; i < 6; i++)
92 I_star[j][i] = 0.0;
93
94 for (j = 0; j < 3; j++)
95 {
96 I_star[j + 3][j + 3] = mass;
97 for (i = 0; i < 3; i++)
98 I_star[j][i] = I_bar[j][i];
99 }
100
101 // off diagonal block of spatial inertia:
102 I_star[2][4] = I_star[4][2] = mass*cg_pos[0];
103 I_star[1][5] = I_star[5][1] = -mass*cg_pos[0];
104 I_star[0][5] = I_star[5][0] = mass*cg_pos[1];
105 I_star[2][3] = I_star[3][2] = -mass*cg_pos[1];
106 I_star[1][3] = I_star[3][1] = mass*cg_pos[2];
107 I_star[0][4] = I_star[4][0] = -mass*cg_pos[2];
108
109 // Perform the root-free cholesky decomposition (IStar = L D L').
110 for (int k = 0; k < 5; k++)
111 {
112 for (i = 5; i > k; i--)
113 {
114 Float tem = I_star[i][k]/I_star[k][k];
115
116 for (j = i; j > k; j--)
117 {
118 I_star[i][j] -= I_star[j][k]*tem;
119 }
120 I_star[i][k] = tem;
121 }
122 }
123
124 #ifdef DEBUG
125 cout << "dmRigidBody constructor: PD check" << endl;
126 #endif
127 bool pd_flag = true;
128 for (i=0; i<6; i++)
129 {
130 if (I_star[i][i] <= 0.0) pd_flag = false;
131 #ifdef DEBUG
132 cout << ' ' << I_star[i][i];
133 #endif
134 }
135 #ifdef DEBUG
136 cout << endl;
137 #endif
138
139 // regardless of whether or not the matrix is PD, go ahead and set the
140 // member variables
141 for (j = 0; j < 6; j++)
142 for (i = 0; i < 6; i++)
143 m_SpInertia[j][i] = 0.0;
144
145 m_mass = mass;
146
147 for (i = 0; i < 3; i++)
148 {
149 m_SpInertia[i + 3][i + 3] = m_mass;
150
151 m_cg_pos[i] = cg_pos[i];
152 m_h[i] = m_mass*m_cg_pos[i];
153
154 for (j = 0; j < 3; j++)
155 {
156 m_I_bar[i][j] = I_bar[i][j];
157 m_SpInertia[i][j] = m_I_bar[i][j];
158 }
159 }
160
161 // off diagonal block of spatial inertia:
162 m_SpInertia[2][4] = m_SpInertia[4][2] = m_h[0];
163 m_SpInertia[1][5] = m_SpInertia[5][1] = -m_h[0];
164 m_SpInertia[0][5] = m_SpInertia[5][0] = m_h[1];
165 m_SpInertia[2][3] = m_SpInertia[3][2] = -m_h[1];
166 m_SpInertia[1][3] = m_SpInertia[3][1] = m_h[2];
167 m_SpInertia[0][4] = m_SpInertia[4][0] = -m_h[2];
168
169 initABVars();
170
171 // Let the user know if the matrices were PD or not (Note under some very
172 // specific conditions the simulation can withstand a positive semidefinite
173 // inertia matrix
174 if (!pd_flag)
175 {
176 cerr << "Error: Rigid Body inertia is not positive definite." << endl;
177 }
178
179 return pd_flag;
180 }
181
182 //----------------------------------------------------------------------------
getInertiaParameters(Float & mass,CartesianTensor I_bar,CartesianVector cg_pos) const183 void dmRigidBody::getInertiaParameters(Float &mass,
184 CartesianTensor I_bar,
185 CartesianVector cg_pos) const
186 {
187 mass = m_mass;
188
189 for (int i=0; i<3; i++)
190 {
191 cg_pos[i] = m_cg_pos[i];
192 for (int j=0; j<3; j++)
193 I_bar[i][j] = m_I_bar[i][j];
194 }
195 }
196
197 #ifdef DM_HYDRODYNAMICS
198 //----------------------------------------------------------------------------
setHydrodynamicParameters(Float volume,SpatialTensor I_added_mass,CartesianVector cb_pos,int drag_axis,Float cyl_min,Float cyl_max,Float cyl_radius,Float C_d)199 void dmRigidBody::setHydrodynamicParameters(Float volume,
200 SpatialTensor I_added_mass,
201 CartesianVector cb_pos,
202 int drag_axis,
203 Float cyl_min,
204 Float cyl_max,
205 Float cyl_radius,
206 Float C_d)
207 {
208 register int i, j;
209
210 Float density = dmEnvironment::getEnvironment()->getFluidDensity();
211
212 // hydrodynamic properties:
213 m_displaced_fluid_vol = volume;
214 m_displaced_fluid_mass = m_displaced_fluid_vol*density;
215
216 for (i = 0; i < 6; i++) {
217 for (j = 0; j < 6; j++) {
218 m_I_added_mass[i][j] = I_added_mass[i][j];
219 m_SpInertia[i][j] += m_I_added_mass[i][j];
220 }
221 }
222
223 for (i=0; i<3; i++)
224 {
225 m_cb_pos[i] = cb_pos[i];
226 }
227
228 // drag parameters
229 m_axis = drag_axis;
230
231 /* FIXME - oops this is no longer supported
232 if (m_axis == -1) {
233 readConfigParameterLabel(cfg_ptr, "Drag_Coefficients");
234 for (j = 0; j < 6; j++)
235 cfg_ptr >> m_Cd_A_p[j];
236 }
237 else
238 {
239 */
240
241 m_x0 = cyl_min;
242 m_length = cyl_max - m_x0;
243 m_radius = cyl_radius;
244
245 Float area = M_PI*m_radius*m_radius;
246 Float C2 = density*C_d;
247 m_Ca = 0.5*C2*area;
248 m_C2rl = C2*m_radius*m_length;
249 }
250 #endif
251
252 //----------------------------------------------------------------------------
253 // Summary: add a force computation object to the rigid body.
254 // This function does not check to see if this force object
255 // has been added previously (error).
256 // Parameters: pointer to the force.
257 // Returns: true if the operation is successful, else false
258 //----------------------------------------------------------------------------
addForce(dmForce * force)259 bool dmRigidBody::addForce(dmForce *force)
260 {
261 if (force == NULL)
262 {
263 cerr << "dmRigidBody::addForce error: NULL force pointer."
264 << endl;
265 return false;
266 }
267
268 m_force.push_back(force);
269
270 return true;
271 }
272
273 //----------------------------------------------------------------------------
274 // Summary: get a pointer to a particular force as specified by an
275 // index (in the order they are added starting at zero).
276 // Parameters: index from 0 to m_num_forces-1
277 // Returns: a pointer to the desired force, or NULL if the index is
278 // out of range.
279 //----------------------------------------------------------------------------
getForce(unsigned int index) const280 dmForce *dmRigidBody::getForce(unsigned int index) const
281 {
282 if (index >= m_force.size())
283 {
284 cerr << "dmRigidBody::getForce error: index out of range "
285 << index << endl;
286 return NULL;
287 }
288 return m_force[index];
289 }
290
291 //----------------------------------------------------------------------------
292 // Summary: get the index of the force pointed to by the parameter
293 // provided it has been added to this rigid body
294 // Parameters: pointer to the force
295 // Returns: index of the force, or -1 if it is not contained in this
296 // rigid body.
297 //----------------------------------------------------------------------------
getForceIndex(dmForce * force) const298 int dmRigidBody::getForceIndex(dmForce *force) const
299 {
300 for (unsigned int i=0; i<m_force.size(); i++)
301 {
302 if (force == m_force[i])
303 {
304 return (int) i;
305 }
306 }
307
308 return -1;
309 }
310
311 //----------------------------------------------------------------------------
removeForce(dmForce * force)312 bool dmRigidBody::removeForce(dmForce *force)
313 {
314 int index = getForceIndex(force);
315
316 if (index < 0)
317 {
318 cerr << "dmRigidBody::removeForce(force) error: force not added before."
319 << endl;
320 return false;
321 }
322
323 m_force.erase(m_force.begin() + index);
324
325 return true;
326 }
327
328 //----------------------------------------------------------------------------
removeForce(unsigned int index)329 bool dmRigidBody::removeForce(unsigned int index)
330 {
331 if (index >= m_force.size())
332 {
333 cerr << "dmRigidBody::removeForce(index) error: index out of range."
334 << endl;
335 return false;
336 }
337
338 m_force.erase(m_force.begin() + index);
339
340 return true;
341 }
342
343 //----------------------------------------------------------------------------
344 // Summary: saves the state of all attached force objects
345 // Parameters: none
346 // Returns: none
347 //----------------------------------------------------------------------------
pushForceStates()348 void dmRigidBody::pushForceStates()
349 {
350 for (unsigned int i=0; i<m_force.size(); i++)
351 m_force[i]->pushState();
352 }
353
354 //----------------------------------------------------------------------------
355 // Summary: restores state of all attached force objects from saved state
356 // Parameters: none
357 // Returns: none
358 //----------------------------------------------------------------------------
popForceStates()359 void dmRigidBody::popForceStates()
360 {
361 for (unsigned int i=0; i<m_force.size(); i++)
362 m_force[i]->popState();
363 }
364
365 //----------------------------------------------------------------------------
366 // Summary: Computes and returns the gravitational potential energy of
367 // the rigid body link. The potential energy zero is defined
368 // by the plane passing through the origin of the inertial
369 // coordinate system with the gravity vector defining the normal.
370 // Parameters: link_val - pointer to struct filled with the kinematic
371 // parameters of the rigid body link
372 // a_gravity - gravitational acceleration vector (w.r.t. ICS)
373 // Returns: potential energy of the rigid body link
374 //----------------------------------------------------------------------------
getPotentialEnergy(const dmABForKinStruct & link_val,CartesianVector a_gravity) const375 Float dmRigidBody::getPotentialEnergy(const dmABForKinStruct &link_val,
376 CartesianVector a_gravity) const
377 {
378 CartesianVector p_COM; // link center of mass wrt ICS
379
380 for (int i = 0; i < 3; i++)
381 p_COM[i] = (link_val.p_ICS[i]
382 + link_val.R_ICS[i][0]*m_cg_pos[0]
383 + link_val.R_ICS[i][1]*m_cg_pos[1]
384 + link_val.R_ICS[i][2]*m_cg_pos[2]);
385
386 Float potentialEnergy = - m_mass*(a_gravity[0]*p_COM[0] +
387 a_gravity[1]*p_COM[1] +
388 a_gravity[2]*p_COM[2]);
389
390 return potentialEnergy;
391 }
392
393 //----------------------------------------------------------------------------
394 // Summary: Computes and returns the kinetic energy of the rigid body link.
395 // Parameters: link_val - pointer to struct filled with the kinematic
396 // parameters of the rigid body link
397 // Returns: kinetic energy of the rigid body link
398 //----------------------------------------------------------------------------
getKineticEnergy(const dmABForKinStruct & link_val) const399 Float dmRigidBody::getKineticEnergy(const dmABForKinStruct &link_val) const
400 {
401 CartesianVector Ibar_w;
402 for (int i = 0; i < 3; i++)
403 Ibar_w[i] = (m_I_bar[i][0]*link_val.v[0] +
404 m_I_bar[i][1]*link_val.v[1] +
405 m_I_bar[i][2]*link_val.v[2]);
406
407 Float w_Ibar_w = (link_val.v[0]*Ibar_w[0] +
408 link_val.v[1]*Ibar_w[1] +
409 link_val.v[2]*Ibar_w[2]);
410
411 Float m_v_v = m_mass*(link_val.v[3]*link_val.v[3] +
412 link_val.v[4]*link_val.v[4] +
413 link_val.v[5]*link_val.v[5]);
414
415
416 CartesianVector w_cross_cg;
417 crossproduct(&link_val.v[0], m_cg_pos, w_cross_cg);
418
419 Float m_v_w_cg = m_mass*(link_val.v[3]*w_cross_cg[0] +
420 link_val.v[4]*w_cross_cg[1] +
421 link_val.v[5]*w_cross_cg[2]);
422
423 Float kineticEnergy = 0.5*(w_Ibar_w + m_v_v) + m_v_w_cg;
424
425 return kineticEnergy;
426 }
427
428 //----------------------------------------------------------------------------
429 // Summary: compute the state-dependent bias force term need for all rigid
430 // body objects
431 // Parameters: omega - angular velocity of body express in body's CS
432 // Returns: beta - spatial bias force vector
433 //----------------------------------------------------------------------------
computeBeta(const dmABForKinStruct & link_val_curr,SpatialVector beta)434 void dmRigidBody::computeBeta(const dmABForKinStruct &link_val_curr,
435 SpatialVector beta)
436 {
437 unsigned int i;
438 CartesianVector tem;
439
440 for (i = 0; i < 3; i++)
441 {
442 tem[i] = m_I_bar[i][0]*link_val_curr.v[0] +
443 m_I_bar[i][1]*link_val_curr.v[1] +
444 m_I_bar[i][2]*link_val_curr.v[2];
445 }
446 crossproduct(tem, link_val_curr.v, &beta[0]); // beta_ref[0..2] = Iw x w;
447
448 crossproduct(link_val_curr.v, m_h, tem);
449 crossproduct(tem, link_val_curr.v, &beta[3]); // beta_ref[3..5] = (wxh)xw;
450
451 #ifdef DM_HYDRODYNAMICS
452 SpatialVector beta_H;
453 computeHydrodynamicBias(link_val_curr, beta_H);
454 for (i = 0; i < 6; i++)
455 {
456 m_beta[i] += beta_H[i];
457 }
458 #endif
459 }
460
461 #ifdef DM_HYDRODYNAMICS
462 //----------------------------------------------------------------------------
463 // Summary: compute the hydrodynamic drag on the submerged body
464 // Parameters: v_rel - spatial velocity relative to the fluid
465 // Returns: f_D - spatial drag force expressed wrt body's CS
466 //----------------------------------------------------------------------------
computeDrag(SpatialVector v_rel,SpatialVector f_D)467 void dmRigidBody::computeDrag(SpatialVector v_rel, SpatialVector f_D)
468 {
469 register int k;
470 register Float x, vn_mag, tmp;
471 CartesianVector vn, tem;
472
473 static Float gqx[4] = {0.069431844, // Gauss-quadrature constants.
474 0.330009478,
475 0.669990521,
476 0.930568155};
477 static Float gqk[4] = {0.1739274225687,
478 0.3260725774312,
479 0.3260725774312,
480 0.1739274225687};
481
482 for (k = 0; k < 6; k++)
483 f_D[k] = 0.0;
484
485 if (m_axis == -1)
486 {
487 for (k = 0; k < 6; k++)
488 f_D[k] = -m_Cd_A_p[k]*v_rel[k]*fabs(v_rel[k]);
489 }
490 else if (m_axis == 0)
491 {
492 // x-axis moment and force
493 f_D[3] = -m_Ca*v_rel[3]*fabs(v_rel[3]);
494
495 // y,z components - using Gauss-quadrature
496 for (k = 0; k < 4; k++)
497 {
498 x = m_x0 + m_length*gqx[k];
499 vn[1] = v_rel[4] + v_rel[2]*x;
500 vn[2] = v_rel[5] - v_rel[1]*x;
501
502 vn_mag = sqrt(vn[1]*vn[1] + vn[2]*vn[2]);
503
504 tmp = gqk[k]*vn_mag;
505 tem[1] = tmp*vn[1];
506 tem[2] = tmp*vn[2];
507
508 f_D[1] += (-x*tem[2]);
509 f_D[2] += x*tem[1];
510 f_D[4] += tem[1];
511 f_D[5] += tem[2];
512 }
513 f_D[1] *= -m_C2rl;
514 f_D[2] *= -m_C2rl;
515 f_D[4] *= -m_C2rl;
516 f_D[5] *= -m_C2rl;
517 }
518 else if (m_axis == 1)
519 {
520 // y-axis moment and force
521 f_D[4] = -m_Ca*v_rel[4]*fabs(v_rel[4]);
522
523 // z,x components
524 for (k = 0; k < 4; k++)
525 {
526 x = m_x0 + m_length*gqx[k];
527 vn[0] = v_rel[3] - v_rel[2]*x;
528 vn[2] = v_rel[5] + v_rel[0]*x;
529
530 vn_mag = sqrt(vn[2]*vn[2] + vn[0]*vn[0]);
531
532 tmp = gqk[k]*vn_mag;
533 tem[0] = tmp*vn[0];
534 tem[2] = tmp*vn[2];
535
536 f_D[0] += (x*tem[2]);
537 f_D[2] += -x*tem[0];
538 f_D[3] += tem[0];
539 f_D[5] += tem[2];
540 }
541 f_D[0] *= -m_C2rl;
542 f_D[2] *= -m_C2rl;
543 f_D[3] *= -m_C2rl;
544 f_D[5] *= -m_C2rl;
545 }
546 else if (m_axis == 2)
547 {
548 // z-axis moment and force
549 f_D[5] = -m_Ca*v_rel[5]*fabs(v_rel[5]);
550
551 // y,x components
552 for (k = 0; k < 4; k++)
553 {
554 x = m_x0 + m_length*gqx[k];
555 vn[0] = v_rel[3] + v_rel[1]*x;
556 vn[1] = v_rel[4] - v_rel[0]*x;
557
558 vn_mag = sqrt(vn[1]*vn[1] + vn[0]*vn[0]);
559
560 tmp = gqk[k]*vn_mag;
561 tem[0] = tmp*vn[0];
562 tem[1] = tmp*vn[1];
563
564 f_D[0] += (-x*tem[1]);
565 f_D[1] += x*tem[0];
566 f_D[3] += tem[0];
567 f_D[4] += tem[1];
568 }
569 f_D[0] *= -m_C2rl;
570 f_D[1] *= -m_C2rl;
571 f_D[3] *= -m_C2rl;
572 f_D[4] *= -m_C2rl;
573 }
574 }
575 #endif
576
577 #ifdef DM_HYDRODYNAMICS
578 //----------------------------------------------------------------------------
579 // Summary: compute the hydrodynamic portion of the bias (state-dependent)
580 // force.
581 // Parameters: val - the struct containing all the necessary parameters to
582 // compute this force, including all of the hydrodynamic
583 // terms.
584 // Returns: beta_H - spatial hydro bias force
585 //----------------------------------------------------------------------------
computeHydrodynamicBias(const dmABForKinStruct & val,SpatialVector beta_H)586 void dmRigidBody::computeHydrodynamicBias(const dmABForKinStruct &val,
587 SpatialVector beta_H)
588 {
589 register int i;
590 CartesianVector tem1, tem2, tem3;
591 SpatialVector tema, v_rel, f_TB, f_D;
592
593 // 2.4
594 v_rel[0] = val.v[0];
595 v_rel[1] = val.v[1];
596 v_rel[2] = val.v[2];
597 v_rel[3] = val.v[3] - val.v_f[0];
598 v_rel[4] = val.v[4] - val.v_f[1];
599 v_rel[5] = val.v[5] - val.v_f[2];
600
601 // 2.8 compute total bouyancy force.
602 f_TB[3] = m_displaced_fluid_mass*val.a_fg[0];
603 f_TB[4] = m_displaced_fluid_mass*val.a_fg[1];
604 f_TB[5] = m_displaced_fluid_mass*val.a_fg[2];
605 crossproduct(m_cb_pos, &f_TB[3], f_TB);
606
607 // drag force.
608 computeDrag(v_rel, f_D);
609
610 // added mass bias force.
611 crossproduct(&val.v[0], &v_rel[3], tem1);
612 tem1[0] += val.a_fg[0];
613 tem1[1] += val.a_fg[1];
614 tem1[2] += val.a_fg[2];
615
616 for (i = 0; i < 6; i++)
617 {
618 beta_H[i] = m_I_added_mass[i][3]*tem1[0] +
619 m_I_added_mass[i][4]*tem1[1] +
620 m_I_added_mass[i][5]*tem1[2];
621 tema[i] = m_I_added_mass[i][0]*v_rel[0] + m_I_added_mass[i][3]*v_rel[3] +
622 m_I_added_mass[i][1]*v_rel[1] + m_I_added_mass[i][4]*v_rel[4] +
623 m_I_added_mass[i][2]*v_rel[2] + m_I_added_mass[i][5]*v_rel[5];
624 }
625
626 crossproduct(&val.v[0], tema, tem1);
627 crossproduct(&v_rel[3], &tema[3], tem2);
628 crossproduct(&val.v[0], &tema[3], tem3);
629
630 beta_H[0] -= (tem1[0] + tem2[0]);
631 beta_H[1] -= (tem1[1] + tem2[1]);
632 beta_H[2] -= (tem1[2] + tem2[2]);
633 beta_H[3] -= tem3[0];
634 beta_H[4] -= tem3[1];
635 beta_H[5] -= tem3[2];
636
637 // accumulate all forces into _beta
638 for (i = 0; i < 6; i++)
639 {
640 beta_H[i] += f_TB[i] + f_D[i];
641 }
642 }
643 #endif
644