dali-physics/third-party/bullet3/src/BulletInverseDynamics/MultiBodyTree.cpp

   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 {
  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
  23 MultiBodyTree::~MultiBodyTree()
  24 {
  25         delete m_impl;
  26         delete m_init_cache;
  27 }
  28
  29 void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
  30 {
  31         m_accept_invalid_mass_parameters = flag;
  32 }
  33
  34 bool MultiBodyTree::getAcceptInvalidMassProperties() const
  35 {
  36         return m_accept_invalid_mass_parameters;
  37 }
  38
  39 int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
  40 {
  41         return m_impl->getBodyOrigin(body_index, world_origin);
  42 }
  43
  44 int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
  45 {
  46         return m_impl->getBodyCoM(body_index, world_com);
  47 }
  48
  49 int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
  50 {
  51         return m_impl->getBodyTransform(body_index, world_T_body);
  52 }
  53 int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
  54 {
  55         return m_impl->getBodyAngularVelocity(body_index, world_omega);
  56 }
  57 int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
  58 {
  59         return m_impl->getBodyLinearVelocity(body_index, world_velocity);
  60 }
  61
  62 int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
  63 {
  64         return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
  65 }
  66
  67 int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
  68 {
  69         return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
  70 }
  71 int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
  72 {
  73         return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
  74 }
  75
  76 int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
  77 {
  78         return m_impl->getParentRParentBodyRef(body_index, r);
  79 }
  80
  81 int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
  82 {
  83         return m_impl->getBodyTParentRef(body_index, T);
  84 }
  85
  86 int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
  87 {
  88         return m_impl->getBodyAxisOfMotion(body_index, axis);
  89 }
  90
  91 void MultiBodyTree::printTree() { m_impl->printTree(); }
  92 void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
  93
  94 int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
  95
  96 int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
  97
  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
 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
 133 int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
 134 {
 135         return calculateMassMatrix(q, true, true, true, mass_matrix);
 136 }
 137
 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
 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
 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)
 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
 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
 225 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
 230 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
 235 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
 240 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
 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
 322 int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
 323 {
 324         return m_impl->getParentIndex(body_index, parent_index);
 325 }
 326
 327 int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
 328 {
 329         return m_impl->getUserInt(body_index, user_int);
 330 }
 331
 332 int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
 333 {
 334         return m_impl->getUserPtr(body_index, user_ptr);
 335 }
 336
 337 int MultiBodyTree::setUserInt(const int body_index, const int user_int)
 338 {
 339         return m_impl->setUserInt(body_index, user_int);
 340 }
 341
 342 int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
 343 {
 344         return m_impl->setUserPtr(body_index, user_ptr);
 345 }
 346
 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
 486 int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
 487 {
 488         return m_impl->setGravityInWorldFrame(gravity);
 489 }
 490
 491 int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
 492 {
 493         return m_impl->getJointType(body_index, joint_type);
 494 }
 495
 496 int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
 497 {
 498         return m_impl->getJointTypeStr(body_index, joint_type);
 499 }
 500
 501 int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
 502 {
 503         return m_impl->getDoFOffset(body_index, q_offset);
 504 }
 505
 506 int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
 507 {
 508         return m_impl->setBodyMass(body_index, mass);
 509 }
 510
 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
 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
 521 int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
 522 {
 523         return m_impl->getBodyMass(body_index, mass);
 524 }
 525
 526 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
 531 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
 536 void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
 537
 538 int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
 539 {
 540         return m_impl->addUserForce(body_index, body_force);
 541 }
 542
 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