1 #include "MultiBodyTree.hpp"
2
3 #include <cmath>
4 #include <limits>
5 #include <vector>
6
7 #include "IDMath.hpp"
8 #include "details/MultiBodyTreeImpl.hpp"
9 #include "details/MultiBodyTreeInitCache.hpp"
10
11 namespace btInverseDynamics
12 {
MultiBodyTree()13 MultiBodyTree::MultiBodyTree()
14 : m_is_finalized(false),
15 m_mass_parameters_are_valid(true),
16 m_accept_invalid_mass_parameters(false),
17 m_impl(0x0),
18 m_init_cache(0x0)
19 {
20 m_init_cache = new InitCache();
21 }
22
~MultiBodyTree()23 MultiBodyTree::~MultiBodyTree()
24 {
25 delete m_impl;
26 delete m_init_cache;
27 }
28
setAcceptInvalidMassParameters(bool flag)29 void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
30 {
31 m_accept_invalid_mass_parameters = flag;
32 }
33
getAcceptInvalidMassProperties() const34 bool MultiBodyTree::getAcceptInvalidMassProperties() const
35 {
36 return m_accept_invalid_mass_parameters;
37 }
38
getBodyOrigin(const int body_index,vec3 * world_origin) const39 int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
40 {
41 return m_impl->getBodyOrigin(body_index, world_origin);
42 }
43
getBodyCoM(const int body_index,vec3 * world_com) const44 int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
45 {
46 return m_impl->getBodyCoM(body_index, world_com);
47 }
48
getBodyTransform(const int body_index,mat33 * world_T_body) const49 int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
50 {
51 return m_impl->getBodyTransform(body_index, world_T_body);
52 }
getBodyAngularVelocity(const int body_index,vec3 * world_omega) const53 int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
54 {
55 return m_impl->getBodyAngularVelocity(body_index, world_omega);
56 }
getBodyLinearVelocity(const int body_index,vec3 * world_velocity) const57 int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
58 {
59 return m_impl->getBodyLinearVelocity(body_index, world_velocity);
60 }
61
getBodyLinearVelocityCoM(const int body_index,vec3 * world_velocity) const62 int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
63 {
64 return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
65 }
66
getBodyAngularAcceleration(const int body_index,vec3 * world_dot_omega) const67 int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
68 {
69 return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
70 }
getBodyLinearAcceleration(const int body_index,vec3 * world_acceleration) const71 int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
72 {
73 return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
74 }
75
getParentRParentBodyRef(const int body_index,vec3 * r) const76 int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
77 {
78 return m_impl->getParentRParentBodyRef(body_index, r);
79 }
80
getBodyTParentRef(const int body_index,mat33 * T) const81 int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
82 {
83 return m_impl->getBodyTParentRef(body_index, T);
84 }
85
getBodyAxisOfMotion(const int body_index,vec3 * axis) const86 int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
87 {
88 return m_impl->getBodyAxisOfMotion(body_index, axis);
89 }
90
printTree()91 void MultiBodyTree::printTree() { m_impl->printTree(); }
printTreeData()92 void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
93
numBodies() const94 int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
95
numDoFs() const96 int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
97
calculateInverseDynamics(const vecx & q,const vecx & u,const vecx & dot_u,vecx * joint_forces)98 int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
99 vecx *joint_forces)
100 {
101 if (false == m_is_finalized)
102 {
103 bt_id_error_message("system has not been initialized\n");
104 return -1;
105 }
106 if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces))
107 {
108 bt_id_error_message("error in inverse dynamics calculation\n");
109 return -1;
110 }
111 return 0;
112 }
113
calculateMassMatrix(const vecx & q,const bool update_kinematics,const bool initialize_matrix,const bool set_lower_triangular_matrix,matxx * mass_matrix)114 int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
115 const bool initialize_matrix,
116 const bool set_lower_triangular_matrix, matxx *mass_matrix)
117 {
118 if (false == m_is_finalized)
119 {
120 bt_id_error_message("system has not been initialized\n");
121 return -1;
122 }
123 if (-1 ==
124 m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
125 set_lower_triangular_matrix, mass_matrix))
126 {
127 bt_id_error_message("error in mass matrix calculation\n");
128 return -1;
129 }
130 return 0;
131 }
132
calculateMassMatrix(const vecx & q,matxx * mass_matrix)133 int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
134 {
135 return calculateMassMatrix(q, true, true, true, mass_matrix);
136 }
137
calculateKinematics(const vecx & q,const vecx & u,const vecx & dot_u)138 int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u)
139 {
140 vec3 world_gravity(m_impl->m_world_gravity);
141 // temporarily set gravity to zero, to ensure we get the actual accelerations
142 setZero(m_impl->m_world_gravity);
143
144 if (false == m_is_finalized)
145 {
146 bt_id_error_message("system has not been initialized\n");
147 return -1;
148 }
149 if (-1 == m_impl->calculateKinematics(q, u, dot_u,
150 MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION))
151 {
152 bt_id_error_message("error in kinematics calculation\n");
153 return -1;
154 }
155
156 m_impl->m_world_gravity = world_gravity;
157 return 0;
158 }
159
calculatePositionKinematics(const vecx & q)160 int MultiBodyTree::calculatePositionKinematics(const vecx &q)
161 {
162 if (false == m_is_finalized)
163 {
164 bt_id_error_message("system has not been initialized\n");
165 return -1;
166 }
167 if (-1 == m_impl->calculateKinematics(q, q, q,
168 MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
169 {
170 bt_id_error_message("error in kinematics calculation\n");
171 return -1;
172 }
173 return 0;
174 }
175
calculatePositionAndVelocityKinematics(const vecx & q,const vecx & u)176 int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u)
177 {
178 if (false == m_is_finalized)
179 {
180 bt_id_error_message("system has not been initialized\n");
181 return -1;
182 }
183 if (-1 == m_impl->calculateKinematics(q, u, u,
184 MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
185 {
186 bt_id_error_message("error in kinematics calculation\n");
187 return -1;
188 }
189 return 0;
190 }
191
192 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
calculateJacobians(const vecx & q,const vecx & u)193 int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u)
194 {
195 if (false == m_is_finalized)
196 {
197 bt_id_error_message("system has not been initialized\n");
198 return -1;
199 }
200 if (-1 == m_impl->calculateJacobians(q, u,
201 MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
202 {
203 bt_id_error_message("error in jacobian calculation\n");
204 return -1;
205 }
206 return 0;
207 }
208
calculateJacobians(const vecx & q)209 int MultiBodyTree::calculateJacobians(const vecx &q)
210 {
211 if (false == m_is_finalized)
212 {
213 bt_id_error_message("system has not been initialized\n");
214 return -1;
215 }
216 if (-1 == m_impl->calculateJacobians(q, q,
217 MultiBodyTree::MultiBodyImpl::POSITION_ONLY))
218 {
219 bt_id_error_message("error in jacobian calculation\n");
220 return -1;
221 }
222 return 0;
223 }
224
getBodyDotJacobianTransU(const int body_index,vec3 * world_dot_jac_trans_u) const225 int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
226 {
227 return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u);
228 }
229
getBodyDotJacobianRotU(const int body_index,vec3 * world_dot_jac_rot_u) const230 int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
231 {
232 return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u);
233 }
234
getBodyJacobianTrans(const int body_index,mat3x * world_jac_trans) const235 int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
236 {
237 return m_impl->getBodyJacobianTrans(body_index, world_jac_trans);
238 }
239
getBodyJacobianRot(const int body_index,mat3x * world_jac_rot) const240 int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
241 {
242 return m_impl->getBodyJacobianRot(body_index, world_jac_rot);
243 }
244
245 #endif
246
addBody(int body_index,int parent_index,JointType joint_type,const vec3 & parent_r_parent_body_ref,const mat33 & body_T_parent_ref,const vec3 & body_axis_of_motion_,idScalar mass,const vec3 & body_r_body_com,const mat33 & body_I_body,const int user_int,void * user_ptr)247 int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
248 const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
249 const vec3 &body_axis_of_motion_, idScalar mass,
250 const vec3 &body_r_body_com, const mat33 &body_I_body,
251 const int user_int, void *user_ptr)
252 {
253 if (body_index < 0)
254 {
255 bt_id_error_message("body index must be positive (got %d)\n", body_index);
256 return -1;
257 }
258 vec3 body_axis_of_motion(body_axis_of_motion_);
259 switch (joint_type)
260 {
261 case REVOLUTE:
262 case PRISMATIC:
263 // check if axis is unit vector
264 if (!isUnitVector(body_axis_of_motion))
265 {
266 bt_id_warning_message(
267 "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
268 body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
269 idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
270 BT_ID_POW(body_axis_of_motion(1), 2) +
271 BT_ID_POW(body_axis_of_motion(2), 2));
272 if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min()))
273 {
274 bt_id_error_message("axis of motion vector too short (%e)\n", length);
275 return -1;
276 }
277 body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
278 }
279 break;
280 case FIXED:
281 break;
282 case FLOATING:
283 break;
284 case SPHERICAL:
285 break;
286 default:
287 bt_id_error_message("unknown joint type %d\n", joint_type);
288 return -1;
289 }
290
291 // sanity check for mass properties. Zero mass is OK.
292 if (mass < 0)
293 {
294 m_mass_parameters_are_valid = false;
295 bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
296 if (!m_accept_invalid_mass_parameters)
297 {
298 return -1;
299 }
300 }
301
302 if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type))
303 {
304 m_mass_parameters_are_valid = false;
305 // error message printed in function call
306 if (!m_accept_invalid_mass_parameters)
307 {
308 return -1;
309 }
310 }
311
312 if (!isValidTransformMatrix(body_T_parent_ref))
313 {
314 return -1;
315 }
316
317 return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref,
318 body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com,
319 body_I_body, user_int, user_ptr);
320 }
321
getParentIndex(const int body_index,int * parent_index) const322 int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
323 {
324 return m_impl->getParentIndex(body_index, parent_index);
325 }
326
getUserInt(const int body_index,int * user_int) const327 int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
328 {
329 return m_impl->getUserInt(body_index, user_int);
330 }
331
getUserPtr(const int body_index,void ** user_ptr) const332 int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
333 {
334 return m_impl->getUserPtr(body_index, user_ptr);
335 }
336
setUserInt(const int body_index,const int user_int)337 int MultiBodyTree::setUserInt(const int body_index, const int user_int)
338 {
339 return m_impl->setUserInt(body_index, user_int);
340 }
341
setUserPtr(const int body_index,void * const user_ptr)342 int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
343 {
344 return m_impl->setUserPtr(body_index, user_ptr);
345 }
346
finalize()347 int MultiBodyTree::finalize()
348 {
349 const int &num_bodies = m_init_cache->numBodies();
350 const int &num_dofs = m_init_cache->numDoFs();
351
352 if (num_dofs < 0)
353 {
354 bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
355 //return -1;
356 }
357
358 // 1 allocate internal MultiBody structure
359 m_impl = new MultiBodyImpl(num_bodies, num_dofs);
360
361 // 2 build new index set assuring index(parent) < index(child)
362 if (-1 == m_init_cache->buildIndexSets())
363 {
364 return -1;
365 }
366 m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
367
368 // 3 setup internal kinematic and dynamic data
369 for (int index = 0; index < num_bodies; index++)
370 {
371 InertiaData inertia;
372 JointData joint;
373 if (-1 == m_init_cache->getInertiaData(index, &inertia))
374 {
375 return -1;
376 }
377 if (-1 == m_init_cache->getJointData(index, &joint))
378 {
379 return -1;
380 }
381
382 RigidBody &rigid_body = m_impl->m_body_list[index];
383
384 rigid_body.m_mass = inertia.m_mass;
385 rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com;
386 rigid_body.m_body_I_body = inertia.m_body_I_body;
387 rigid_body.m_joint_type = joint.m_type;
388 rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
389 rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref;
390 rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
391 rigid_body.m_joint_type = joint.m_type;
392
393 int user_int;
394 if (-1 == m_init_cache->getUserInt(index, &user_int))
395 {
396 return -1;
397 }
398 if (-1 == m_impl->setUserInt(index, user_int))
399 {
400 return -1;
401 }
402
403 void *user_ptr;
404 if (-1 == m_init_cache->getUserPtr(index, &user_ptr))
405 {
406 return -1;
407 }
408 if (-1 == m_impl->setUserPtr(index, user_ptr))
409 {
410 return -1;
411 }
412
413 // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
414 // matrices.
415 switch (rigid_body.m_joint_type)
416 {
417 case REVOLUTE:
418 rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
419 rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
420 rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2);
421 rigid_body.m_Jac_JT(0) = 0.0;
422 rigid_body.m_Jac_JT(1) = 0.0;
423 rigid_body.m_Jac_JT(2) = 0.0;
424 break;
425 case PRISMATIC:
426 rigid_body.m_Jac_JR(0) = 0.0;
427 rigid_body.m_Jac_JR(1) = 0.0;
428 rigid_body.m_Jac_JR(2) = 0.0;
429 rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0);
430 rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1);
431 rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
432 break;
433 case FIXED:
434 // NOTE/TODO: dimension really should be zero ..
435 rigid_body.m_Jac_JR(0) = 0.0;
436 rigid_body.m_Jac_JR(1) = 0.0;
437 rigid_body.m_Jac_JR(2) = 0.0;
438 rigid_body.m_Jac_JT(0) = 0.0;
439 rigid_body.m_Jac_JT(1) = 0.0;
440 rigid_body.m_Jac_JT(2) = 0.0;
441 break;
442 case SPHERICAL:
443 // NOTE/TODO: this is not really correct.
444 // the Jacobians should be 3x3 matrices here !
445 rigid_body.m_Jac_JR(0) = 0.0;
446 rigid_body.m_Jac_JR(1) = 0.0;
447 rigid_body.m_Jac_JR(2) = 0.0;
448 rigid_body.m_Jac_JT(0) = 0.0;
449 rigid_body.m_Jac_JT(1) = 0.0;
450 rigid_body.m_Jac_JT(2) = 0.0;
451 break;
452 case FLOATING:
453 // NOTE/TODO: this is not really correct.
454 // the Jacobians should be 3x3 matrices here !
455 rigid_body.m_Jac_JR(0) = 0.0;
456 rigid_body.m_Jac_JR(1) = 0.0;
457 rigid_body.m_Jac_JR(2) = 0.0;
458 rigid_body.m_Jac_JT(0) = 0.0;
459 rigid_body.m_Jac_JT(1) = 0.0;
460 rigid_body.m_Jac_JT(2) = 0.0;
461 break;
462 default:
463 bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
464 return -1;
465 }
466 }
467
468 // 4 assign degree of freedom indices & build per-joint-type index arrays
469 if (-1 == m_impl->generateIndexSets())
470 {
471 bt_id_error_message("generating index sets\n");
472 return -1;
473 }
474
475 // 5 do some pre-computations ..
476 m_impl->calculateStaticData();
477
478 // 6. make sure all user forces are set to zero, as this might not happen
479 // in the vector ctors.
480 m_impl->clearAllUserForcesAndMoments();
481
482 m_is_finalized = true;
483 return 0;
484 }
485
setGravityInWorldFrame(const vec3 & gravity)486 int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
487 {
488 return m_impl->setGravityInWorldFrame(gravity);
489 }
490
getJointType(const int body_index,JointType * joint_type) const491 int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
492 {
493 return m_impl->getJointType(body_index, joint_type);
494 }
495
getJointTypeStr(const int body_index,const char ** joint_type) const496 int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
497 {
498 return m_impl->getJointTypeStr(body_index, joint_type);
499 }
500
getDoFOffset(const int body_index,int * q_offset) const501 int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
502 {
503 return m_impl->getDoFOffset(body_index, q_offset);
504 }
505
setBodyMass(const int body_index,idScalar mass)506 int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
507 {
508 return m_impl->setBodyMass(body_index, mass);
509 }
510
setBodyFirstMassMoment(const int body_index,const vec3 & first_mass_moment)511 int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment)
512 {
513 return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
514 }
515
setBodySecondMassMoment(const int body_index,const mat33 & second_mass_moment)516 int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment)
517 {
518 return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
519 }
520
getBodyMass(const int body_index,idScalar * mass) const521 int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
522 {
523 return m_impl->getBodyMass(body_index, mass);
524 }
525
getBodyFirstMassMoment(const int body_index,vec3 * first_mass_moment) const526 int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const
527 {
528 return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
529 }
530
getBodySecondMassMoment(const int body_index,mat33 * second_mass_moment) const531 int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const
532 {
533 return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
534 }
535
clearAllUserForcesAndMoments()536 void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
537
addUserForce(const int body_index,const vec3 & body_force)538 int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
539 {
540 return m_impl->addUserForce(body_index, body_force);
541 }
542
addUserMoment(const int body_index,const vec3 & body_moment)543 int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment)
544 {
545 return m_impl->addUserMoment(body_index, body_moment);
546 }
547
548 } // namespace btInverseDynamics
549