dali-physics/third-party/bullet3/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

   1 #include "MultiBodyTreeImpl.hpp"
   2
   3 namespace btInverseDynamics
   4 {
   5 MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
   6         : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
   7 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
   8           ,
   9           m_m3x(3, m_num_dofs)
  10 #endif
  11 {
  12 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
  13         resize(m_m3x, m_num_dofs);
  14 #endif
  15         m_body_list.resize(num_bodies_);
  16         m_parent_index.resize(num_bodies_);
  17         m_child_indices.resize(num_bodies_);
  18         m_user_int.resize(num_bodies_);
  19         m_user_ptr.resize(num_bodies_);
  20
  21         m_world_gravity(0) = 0.0;
  22         m_world_gravity(1) = 0.0;
  23         m_world_gravity(2) = -9.8;
  24 }
  25
  26 const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const
  27 {
  28         switch (type)
  29         {
  30                 case FIXED:
  31                         return "fixed";
  32                 case REVOLUTE:
  33                         return "revolute";
  34                 case PRISMATIC:
  35                         return "prismatic";
  36                 case FLOATING:
  37                         return "floating";
  38                 case SPHERICAL:
  39                         return "spherical";
  40         }
  41         return "error: invalid";
  42 }
  43
  44 inline void indent(const int &level)
  45 {
  46         for (int j = 0; j < level; j++)
  47                 id_printf("  ");  // indent
  48 }
  49
  50 void MultiBodyTree::MultiBodyImpl::printTree()
  51 {
  52         id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type));
  53         printTree(0, 0);
  54 }
  55
  56 void MultiBodyTree::MultiBodyImpl::printTreeData()
  57 {
  58         for (idArrayIdx i = 0; i < m_body_list.size(); i++)
  59         {
  60                 RigidBody &body = m_body_list[i];
  61                 id_printf("body: %d\n", static_cast<int>(i));
  62                 id_printf("type: %s\n", jointTypeToString(body.m_joint_type));
  63                 id_printf("q_index= %d\n", body.m_q_index);
  64                 id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2));
  65                 id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2));
  66
  67                 id_printf("mass = %f\n", body.m_mass);
  68                 id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1),
  69                                   body.m_body_mass_com(2));
  70                 id_printf(
  71                         "I_o= [%f %f %f;\n"
  72                         "         %f %f %f;\n"
  73                         "         %f %f %f]\n",
  74                         body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
  75                         body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
  76                         body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
  77
  78                 id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0),
  79                                   body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2));
  80         }
  81 }
  82 int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
  83 {
  84         switch (type)
  85         {
  86                 case FIXED:
  87                         return 0;
  88                 case REVOLUTE:
  89                 case PRISMATIC:
  90                         return 1;
  91                 case FLOATING:
  92                         return 6;
  93                 case SPHERICAL:
  94                         return 3;
  95         }
  96         bt_id_error_message("unknown joint type %d\n", type);
  97         return 0;
  98 }
  99
 100 void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation)
 101 {
 102         // this is adapted from URDF2Bullet.
 103         // TODO: fix this and print proper graph (similar to git --log --graph)
 104         int num_children = m_child_indices[index].size();
 105
 106         indentation += 2;
 107         int count = 0;
 108
 109         for (int i = 0; i < num_children; i++)
 110         {
 111                 int child_index = m_child_indices[index][i];
 112                 indent(indentation);
 113                 id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index,
 114                                   jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1,
 115                                   m_body_list[index].m_q_index,
 116                                   m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type));
 117                 // first grandchild
 118                 printTree(child_index, indentation);
 119         }
 120 }
 121
 122 int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity)
 123 {
 124         m_world_gravity = gravity;
 125         return 0;
 126 }
 127
 128 int MultiBodyTree::MultiBodyImpl::generateIndexSets()
 129 {
 130         m_body_revolute_list.resize(0);
 131         m_body_prismatic_list.resize(0);
 132         int q_index = 0;
 133         for (idArrayIdx i = 0; i < m_body_list.size(); i++)
 134         {
 135                 RigidBody &body = m_body_list[i];
 136                 body.m_q_index = -1;
 137                 switch (body.m_joint_type)
 138                 {
 139                         case REVOLUTE:
 140                                 m_body_revolute_list.push_back(i);
 141                                 body.m_q_index = q_index;
 142                                 q_index++;
 143                                 break;
 144                         case PRISMATIC:
 145                                 m_body_prismatic_list.push_back(i);
 146                                 body.m_q_index = q_index;
 147                                 q_index++;
 148                                 break;
 149                         case FIXED:
 150                                 // do nothing
 151                                 break;
 152                         case FLOATING:
 153                                 m_body_floating_list.push_back(i);
 154                                 body.m_q_index = q_index;
 155                                 q_index += 6;
 156                                 break;
 157                         case SPHERICAL:
 158                                 m_body_spherical_list.push_back(i);
 159                                 body.m_q_index = q_index;
 160                                 q_index += 3;
 161                                 break;
 162                         default:
 163                                 bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
 164                                 return -1;
 165                 }
 166         }
 167         // sanity check
 168         if (q_index != m_num_dofs)
 169         {
 170                 bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
 171                 return -1;
 172         }
 173
 174         m_child_indices.resize(m_body_list.size());
 175
 176         for (idArrayIdx child = 1; child < m_parent_index.size(); child++)
 177         {
 178                 const int &parent = m_parent_index[child];
 179                 if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1))
 180                 {
 181                         m_child_indices[parent].push_back(child);
 182                 }
 183                 else
 184                 {
 185                         if (-1 == parent)
 186                         {
 187                                 // multiple bodies are directly linked to the environment, ie, not a single root
 188                                 bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
 189                         }
 190                         else
 191                         {
 192                                 // should never happen
 193                                 bt_id_error_message(
 194                                         "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
 195                                         child, parent, static_cast<int>(m_parent_index.size()));
 196                         }
 197                         return -1;
 198                 }
 199         }
 200
 201         return 0;
 202 }
 203
 204 void MultiBodyTree::MultiBodyImpl::calculateStaticData()
 205 {
 206         // relative kinematics that are not a function of q, u, dot_u
 207         for (idArrayIdx i = 0; i < m_body_list.size(); i++)
 208         {
 209                 RigidBody &body = m_body_list[i];
 210                 switch (body.m_joint_type)
 211                 {
 212                         case REVOLUTE:
 213                                 body.m_parent_vel_rel(0) = 0;
 214                                 body.m_parent_vel_rel(1) = 0;
 215                                 body.m_parent_vel_rel(2) = 0;
 216                                 body.m_parent_acc_rel(0) = 0;
 217                                 body.m_parent_acc_rel(1) = 0;
 218                                 body.m_parent_acc_rel(2) = 0;
 219                                 body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
 220                                 break;
 221                         case PRISMATIC:
 222                                 body.m_body_T_parent = body.m_body_T_parent_ref;
 223                                 body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT;
 224                                 body.m_body_ang_vel_rel(0) = 0;
 225                                 body.m_body_ang_vel_rel(1) = 0;
 226                                 body.m_body_ang_vel_rel(2) = 0;
 227                                 body.m_body_ang_acc_rel(0) = 0;
 228                                 body.m_body_ang_acc_rel(1) = 0;
 229                                 body.m_body_ang_acc_rel(2) = 0;
 230                                 break;
 231                         case FIXED:
 232                                 body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
 233                                 body.m_body_T_parent = body.m_body_T_parent_ref;
 234                                 body.m_body_ang_vel_rel(0) = 0;
 235                                 body.m_body_ang_vel_rel(1) = 0;
 236                                 body.m_body_ang_vel_rel(2) = 0;
 237                                 body.m_parent_vel_rel(0) = 0;
 238                                 body.m_parent_vel_rel(1) = 0;
 239                                 body.m_parent_vel_rel(2) = 0;
 240                                 body.m_body_ang_acc_rel(0) = 0;
 241                                 body.m_body_ang_acc_rel(1) = 0;
 242                                 body.m_body_ang_acc_rel(2) = 0;
 243                                 body.m_parent_acc_rel(0) = 0;
 244                                 body.m_parent_acc_rel(1) = 0;
 245                                 body.m_parent_acc_rel(2) = 0;
 246                                 break;
 247                         case FLOATING:
 248                                 // no static data
 249                                 break;
 250                         case SPHERICAL:
 251                                 //todo: review
 252                                 body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
 253                                 body.m_parent_vel_rel(0) = 0;
 254                                 body.m_parent_vel_rel(1) = 0;
 255                                 body.m_parent_vel_rel(2) = 0;
 256                                 body.m_parent_acc_rel(0) = 0;
 257                                 body.m_parent_acc_rel(1) = 0;
 258                                 body.m_parent_acc_rel(2) = 0;
 259                                 break;
 260                 }
 261
 262                         // resize & initialize jacobians to zero.
 263 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 264                 body.m_body_dot_Jac_T_u(0) = 0.0;
 265                 body.m_body_dot_Jac_T_u(1) = 0.0;
 266                 body.m_body_dot_Jac_T_u(2) = 0.0;
 267                 body.m_body_dot_Jac_R_u(0) = 0.0;
 268                 body.m_body_dot_Jac_R_u(1) = 0.0;
 269                 body.m_body_dot_Jac_R_u(2) = 0.0;
 270                 resize(body.m_body_Jac_T, m_num_dofs);
 271                 resize(body.m_body_Jac_R, m_num_dofs);
 272                 body.m_body_Jac_T.setZero();
 273                 body.m_body_Jac_R.setZero();
 274 #endif  //
 275         }
 276 }
 277
 278 int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u,
 279                                                                                                                    const vecx &dot_u, vecx *joint_forces)
 280 {
 281         if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
 282                 joint_forces->size() != m_num_dofs)
 283         {
 284                 bt_id_error_message(
 285                         "wrong vector dimension. system has %d DOFs,\n"
 286                         "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
 287                         m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
 288                         static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
 289                 return -1;
 290         }
 291         // 1. relative kinematics
 292         if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION))
 293         {
 294                 bt_id_error_message("error in calculateKinematics\n");
 295                 return -1;
 296         }
 297         // 2. update contributions to equations of motion for every body.
 298         for (idArrayIdx i = 0; i < m_body_list.size(); i++)
 299         {
 300                 RigidBody &body = m_body_list[i];
 301                 // 3.4 update dynamic terms (rate of change of angular & linear momentum)
 302                 body.m_eom_lhs_rotational =
 303                         body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
 304                         body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
 305                         body.m_body_moment_user;
 306                 body.m_eom_lhs_translational =
 307                         body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
 308                         body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
 309                         body.m_body_force_user;
 310         }
 311
 312         // 3. calculate full set of forces at parent joint
 313         // (not directly calculating the joint force along the free direction
 314         // simplifies inclusion of fixed joints.
 315         // An alternative would be to fuse bodies in a pre-processing step,
 316         // but that would make changing masses online harder (eg, payload masses
 317         // added with fixed  joints to a gripper)
 318         // Also, this enables adding zero weight bodies as a way to calculate frame poses
 319         // for force elements, etc.
 320
 321         for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--)
 322         {
 323                 // sum of forces and moments acting on this body from its children
 324                 vec3 sum_f_children;
 325                 vec3 sum_m_children;
 326                 setZero(sum_f_children);
 327                 setZero(sum_m_children);
 328                 for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
 329                          child_list_idx++)
 330                 {
 331                         const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
 332                         vec3 child_joint_force_in_this_frame =
 333                                 child.m_body_T_parent.transpose() * child.m_force_at_joint;
 334                         sum_f_children -= child_joint_force_in_this_frame;
 335                         sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
 336                                                           child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
 337                 }
 338                 RigidBody &body = m_body_list[body_idx];
 339
 340                 body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
 341                 body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
 342         }
 343
 344         // 4. Calculate Joint forces.
 345         // These are the components of force_at_joint/moment_at_joint
 346         // in the free directions given by Jac_JT/Jac_JR
 347         // 4.1 revolute joints
 348         for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
 349         {
 350                 RigidBody &body = m_body_list[m_body_revolute_list[i]];
 351                 // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
 352                 (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
 353         }
 354         // 4.2 for prismatic joints
 355         for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
 356         {
 357                 RigidBody &body = m_body_list[m_body_prismatic_list[i]];
 358                 // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
 359                 (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
 360         }
 361         // 4.3 floating bodies (6-DoF joints)
 362         for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
 363         {
 364                 RigidBody &body = m_body_list[m_body_floating_list[i]];
 365                 (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
 366                 (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
 367                 (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
 368
 369                 (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
 370                 (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
 371                 (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
 372         }
 373
 374         // 4.4 spherical bodies (3-DoF joints)
 375         for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
 376         {
 377                 //todo: review
 378                 RigidBody &body = m_body_list[m_body_spherical_list[i]];
 379                 (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
 380                 (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
 381                 (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
 382         }
 383         return 0;
 384 }
 385
 386 int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u,
 387                                                                                                           const KinUpdateType type)
 388 {
 389         if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs)
 390         {
 391                 bt_id_error_message(
 392                         "wrong vector dimension. system has %d DOFs,\n"
 393                         "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
 394                         m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
 395                         static_cast<int>(dot_u.size()));
 396                 return -1;
 397         }
 398         if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION)
 399         {
 400                 bt_id_error_message("invalid type %d\n", type);
 401                 return -1;
 402         }
 403
 404         // 1. update relative kinematics
 405         // 1.1 for revolute
 406         for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
 407         {
 408                 RigidBody &body = m_body_list[m_body_revolute_list[i]];
 409                 mat33 T;
 410                 bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
 411                 body.m_body_T_parent = T * body.m_body_T_parent_ref;
 412                 if (type >= POSITION_VELOCITY)
 413                 {
 414                         body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
 415                 }
 416                 if (type >= POSITION_VELOCITY_ACCELERATION)
 417                 {
 418                         body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
 419                 }
 420         }
 421         // 1.2 for prismatic
 422         for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
 423         {
 424                 RigidBody &body = m_body_list[m_body_prismatic_list[i]];
 425                 body.m_parent_pos_parent_body =
 426                         body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
 427                 if (type >= POSITION_VELOCITY)
 428                 {
 429                         body.m_parent_vel_rel =
 430                                 body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
 431                 }
 432                 if (type >= POSITION_VELOCITY_ACCELERATION)
 433                 {
 434                         body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
 435                 }
 436         }
 437         // 1.3 fixed joints: nothing to do
 438         // 1.4 6dof joints:
 439         for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
 440         {
 441                 RigidBody &body = m_body_list[m_body_floating_list[i]];
 442
 443                 body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
 444                                                            transformY(q(body.m_q_index + 1)) *
 445                                                            transformX(q(body.m_q_index));
 446                 body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
 447                 body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
 448                 body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
 449                 body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 450
 451                 if (type >= POSITION_VELOCITY)
 452                 {
 453                         body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
 454                         body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
 455                         body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
 456
 457                         body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
 458                         body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
 459                         body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
 460
 461                         body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
 462                 }
 463                 if (type >= POSITION_VELOCITY_ACCELERATION)
 464                 {
 465                         body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
 466                         body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
 467                         body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
 468
 469                         body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
 470                         body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
 471                         body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
 472
 473                         body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
 474                 }
 475         }
 476
 477         for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
 478         {
 479                 //todo: review
 480                 RigidBody &body = m_body_list[m_body_spherical_list[i]];
 481
 482                 mat33 T;
 483
 484                 T = transformX(q(body.m_q_index)) *
 485                                 transformY(q(body.m_q_index + 1)) *
 486                                 transformZ(q(body.m_q_index + 2));
 487                 body.m_body_T_parent = T * body.m_body_T_parent_ref;
 488
 489                 body.m_parent_pos_parent_body(0)=0;
 490                 body.m_parent_pos_parent_body(1)=0;
 491                 body.m_parent_pos_parent_body(2)=0;
 492
 493                 body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 494
 495                 if (type >= POSITION_VELOCITY)
 496                 {
 497                         body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
 498                         body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
 499                         body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
 500                         body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
 501                 }
 502                 if (type >= POSITION_VELOCITY_ACCELERATION)
 503                 {
 504                         body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
 505                         body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
 506                         body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
 507                         body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
 508                 }
 509         }
 510
 511         // 2. absolute kinematic quantities (vector valued)
 512         // NOTE: this should be optimized by specializing for different body types
 513         // (e.g., relative rotation is always zero for prismatic joints, etc.)
 514
 515         // calculations for root body
 516         {
 517                 RigidBody &body = m_body_list[0];
 518                 // 3.1 update absolute positions and orientations:
 519                 // will be required if we add force elements (eg springs between bodies,
 520                 // or contacts)
 521                 // not required right now, added here for debugging purposes
 522                 body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
 523                 body.m_body_T_world = body.m_body_T_parent;
 524
 525                 if (type >= POSITION_VELOCITY)
 526                 {
 527                         // 3.2 update absolute velocities
 528                         body.m_body_ang_vel = body.m_body_ang_vel_rel;
 529                         body.m_body_vel = body.m_parent_vel_rel;
 530                 }
 531                 if (type >= POSITION_VELOCITY_ACCELERATION)
 532                 {
 533                         // 3.3 update absolute accelerations
 534                         // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
 535                         body.m_body_ang_acc = body.m_body_ang_acc_rel;
 536                         body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
 537                         // add gravitational acceleration to root body
 538                         // this is an efficient way to add gravitational terms,
 539                         // but it does mean that the kinematics are no longer
 540                         // correct at the acceleration level
 541                         // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
 542                         body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
 543                 }
 544         }
 545
 546         for (idArrayIdx i = 1; i < m_body_list.size(); i++)
 547         {
 548                 RigidBody &body = m_body_list[i];
 549                 RigidBody &parent = m_body_list[m_parent_index[i]];
 550                 // 2.1 update absolute positions and orientations:
 551                 // will be required if we add force elements (eg springs between bodies,
 552                 // or contacts)  not required right now added here for debugging purposes
 553                 body.m_body_pos =
 554                         body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
 555                 body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
 556
 557                 if (type >= POSITION_VELOCITY)
 558                 {
 559                         // 2.2 update absolute velocities
 560                         body.m_body_ang_vel =
 561                                 body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
 562
 563                         body.m_body_vel =
 564                                 body.m_body_T_parent *
 565                                 (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
 566                                  body.m_parent_vel_rel);
 567                 }
 568                 if (type >= POSITION_VELOCITY_ACCELERATION)
 569                 {
 570                         // 2.3 update absolute accelerations
 571                         // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
 572                         body.m_body_ang_acc =
 573                                 body.m_body_T_parent * parent.m_body_ang_acc -
 574                                 body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
 575                                 body.m_body_ang_acc_rel;
 576                         body.m_body_acc =
 577                                 body.m_body_T_parent *
 578                                 (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
 579                                  parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
 580                                  2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
 581                 }
 582         }
 583
 584         return 0;
 585 }
 586
 587 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 588
 589 void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
 590 {
 591         const int &idx = body.m_q_index;
 592         switch (body.m_joint_type)
 593         {
 594                 case FIXED:
 595                         break;
 596                 case REVOLUTE:
 597                         setMat3xElem(0, idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
 598                         setMat3xElem(1, idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
 599                         setMat3xElem(2, idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
 600                         break;
 601                 case PRISMATIC:
 602                         setMat3xElem(0, idx, body.m_body_T_parent_ref(0, 0) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 0) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 0) * body.m_Jac_JT(2),
 603                                                  &body.m_body_Jac_T);
 604                         setMat3xElem(1, idx, body.m_body_T_parent_ref(0, 1) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 1) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 1) * body.m_Jac_JT(2),
 605                                                  &body.m_body_Jac_T);
 606                         setMat3xElem(2, idx, body.m_body_T_parent_ref(0, 2) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 2) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 2) * body.m_Jac_JT(2),
 607                                                  &body.m_body_Jac_T);
 608                         break;
 609                 case FLOATING:
 610                         setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
 611                         setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
 612                         setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
 613                         // body_Jac_T = body_T_parent.transpose();
 614                         setMat3xElem(0, idx + 3, body.m_body_T_parent(0, 0), &body.m_body_Jac_T);
 615                         setMat3xElem(0, idx + 4, body.m_body_T_parent(1, 0), &body.m_body_Jac_T);
 616                         setMat3xElem(0, idx + 5, body.m_body_T_parent(2, 0), &body.m_body_Jac_T);
 617
 618                         setMat3xElem(1, idx + 3, body.m_body_T_parent(0, 1), &body.m_body_Jac_T);
 619                         setMat3xElem(1, idx + 4, body.m_body_T_parent(1, 1), &body.m_body_Jac_T);
 620                         setMat3xElem(1, idx + 5, body.m_body_T_parent(2, 1), &body.m_body_Jac_T);
 621
 622                         setMat3xElem(2, idx + 3, body.m_body_T_parent(0, 2), &body.m_body_Jac_T);
 623                         setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
 624                         setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
 625
 626                         break;
 627                 case SPHERICAL:
 628                         //todo: review
 629                         setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
 630                         setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
 631                         setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
 632                         break;
 633         }
 634 }
 635
 636 int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type)
 637 {
 638         if (q.size() != m_num_dofs || u.size() != m_num_dofs)
 639         {
 640                 bt_id_error_message(
 641                         "wrong vector dimension. system has %d DOFs,\n"
 642                         "but dim(q)= %d, dim(u)= %d\n",
 643                         m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
 644                 return -1;
 645         }
 646         if (type != POSITION_ONLY && type != POSITION_VELOCITY)
 647         {
 648                 bt_id_error_message("invalid type %d\n", type);
 649                 return -1;
 650         }
 651
 652         addRelativeJacobianComponent(m_body_list[0]);
 653         for (idArrayIdx i = 1; i < m_body_list.size(); i++)
 654         {
 655                 RigidBody &body = m_body_list[i];
 656                 RigidBody &parent = m_body_list[m_parent_index[i]];
 657
 658                 mul(body.m_body_T_parent, parent.m_body_Jac_R, &body.m_body_Jac_R);
 659                 body.m_body_Jac_T = parent.m_body_Jac_T;
 660                 mul(tildeOperator(body.m_parent_pos_parent_body), parent.m_body_Jac_R, &m_m3x);
 661                 sub(body.m_body_Jac_T, m_m3x, &body.m_body_Jac_T);
 662
 663                 addRelativeJacobianComponent(body);
 664                 mul(body.m_body_T_parent, body.m_body_Jac_T, &body.m_body_Jac_T);
 665
 666                 if (type >= POSITION_VELOCITY)
 667                 {
 668                         body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
 669                                                                           body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
 670                         body.m_body_dot_Jac_T_u = body.m_body_T_parent *
 671                                                                           (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
 672                                                                            parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
 673                                                                            2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
 674                 }
 675         }
 676         return 0;
 677 }
 678 #endif
 679
 680 static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
 681 {
 682         switch (dof)
 683         {
 684                 // rotational part
 685                 case 0:
 686                         Jac_JR(0) = 1;
 687                         Jac_JR(1) = 0;
 688                         Jac_JR(2) = 0;
 689                         setZero(Jac_JT);
 690                         break;
 691                 case 1:
 692                         Jac_JR(0) = 0;
 693                         Jac_JR(1) = 1;
 694                         Jac_JR(2) = 0;
 695                         setZero(Jac_JT);
 696                         break;
 697                 case 2:
 698                         Jac_JR(0) = 0;
 699                         Jac_JR(1) = 0;
 700                         Jac_JR(2) = 1;
 701                         setZero(Jac_JT);
 702                         break;
 703         }
 704 }
 705
 706 static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
 707 {
 708         switch (dof)
 709         {
 710                 // rotational part
 711                 case 0:
 712                         Jac_JR(0) = 1;
 713                         Jac_JR(1) = 0;
 714                         Jac_JR(2) = 0;
 715                         setZero(Jac_JT);
 716                         break;
 717                 case 1:
 718                         Jac_JR(0) = 0;
 719                         Jac_JR(1) = 1;
 720                         Jac_JR(2) = 0;
 721                         setZero(Jac_JT);
 722                         break;
 723                 case 2:
 724                         Jac_JR(0) = 0;
 725                         Jac_JR(1) = 0;
 726                         Jac_JR(2) = 1;
 727                         setZero(Jac_JT);
 728                         break;
 729                 // translational part
 730                 case 3:
 731                         setZero(Jac_JR);
 732                         Jac_JT(0) = 1;
 733                         Jac_JT(1) = 0;
 734                         Jac_JT(2) = 0;
 735                         break;
 736                 case 4:
 737                         setZero(Jac_JR);
 738                         Jac_JT(0) = 0;
 739                         Jac_JT(1) = 1;
 740                         Jac_JT(2) = 0;
 741                         break;
 742                 case 5:
 743                         setZero(Jac_JR);
 744                         Jac_JT(0) = 0;
 745                         Jac_JT(1) = 0;
 746                         Jac_JT(2) = 1;
 747                         break;
 748         }
 749 }
 750
 751 static inline int jointNumDoFs(const JointType &type)
 752 {
 753         switch (type)
 754         {
 755                 case FIXED:
 756                         return 0;
 757                 case REVOLUTE:
 758                 case PRISMATIC:
 759                         return 1;
 760                 case FLOATING:
 761                         return 6;
 762                 case SPHERICAL:
 763                         return 3;
 764         }
 765         // this should never happen
 766         bt_id_error_message("invalid joint type\n");
 767         // TODO add configurable abort/crash function
 768         abort();
 769         return 0;
 770 }
 771
 772 int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
 773                                                                                                           const bool initialize_matrix,
 774                                                                                                           const bool set_lower_triangular_matrix,
 775                                                                                                           matxx *mass_matrix)
 776 {
 777         // This calculates the joint space mass matrix for the multibody system.
 778         // The algorithm is essentially an implementation of "method 3"
 779         // in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
 780         // (Later named "Composite Rigid Body Algorithm" by Featherstone).
 781         //
 782         // This implementation, however, handles branched systems and uses a formulation centered
 783         // on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
 784
 785         if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
 786                 mass_matrix->cols() != m_num_dofs)
 787         {
 788                 bt_id_error_message(
 789                         "Dimension error. System has %d DOFs,\n"
 790                         "but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
 791                         m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
 792                         static_cast<int>(mass_matrix->cols()));
 793                 return -1;
 794         }
 795
 796         // TODO add optimized zeroing function?
 797         if (initialize_matrix)
 798         {
 799                 for (int i = 0; i < m_num_dofs; i++)
 800                 {
 801                         for (int j = 0; j < m_num_dofs; j++)
 802                         {
 803                                 setMatxxElem(i, j, 0.0, mass_matrix);
 804                         }
 805                 }
 806         }
 807
 808         if (update_kinematics)
 809         {
 810                 // 1. update relative kinematics
 811                 // 1.1 for revolute joints
 812                 for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
 813                 {
 814                         RigidBody &body = m_body_list[m_body_revolute_list[i]];
 815                         // from reference orientation (q=0) of body-fixed frame to current orientation
 816                         mat33 body_T_body_ref;
 817                         bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref);
 818                         body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
 819                 }
 820                 // 1.2 for prismatic joints
 821                 for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
 822                 {
 823                         RigidBody &body = m_body_list[m_body_prismatic_list[i]];
 824                         // body.m_body_T_parent= fixed
 825                         body.m_parent_pos_parent_body =
 826                                 body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
 827                 }
 828                 // 1.3 fixed joints: nothing to do
 829                 // 1.4 6dof joints:
 830                 for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
 831                 {
 832                         RigidBody &body = m_body_list[m_body_floating_list[i]];
 833
 834                         body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
 835                                                                    transformY(q(body.m_q_index + 1)) *
 836                                                                    transformX(q(body.m_q_index));
 837                         body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
 838                         body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
 839                         body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
 840
 841                         body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 842                 }
 843
 844                 for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
 845                 {
 846                         //todo: review
 847                         RigidBody &body = m_body_list[m_body_spherical_list[i]];
 848
 849                         mat33 T;
 850
 851                         T = transformX(q(body.m_q_index)) *
 852                                 transformY(q(body.m_q_index + 1)) *
 853                                 transformZ(q(body.m_q_index + 2));
 854                         body.m_body_T_parent = T * body.m_body_T_parent_ref;
 855
 856                         body.m_parent_pos_parent_body(0)=0;
 857                         body.m_parent_pos_parent_body(1)=0;
 858                         body.m_parent_pos_parent_body(2)=0;
 859
 860                         body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 861                 }
 862         }
 863         for (int i = m_body_list.size() - 1; i >= 0; i--)
 864         {
 865                 RigidBody &body = m_body_list[i];
 866                 // calculate mass, center of mass and inertia of "composite rigid body",
 867                 // ie, sub-tree starting at current body
 868                 body.m_subtree_mass = body.m_mass;
 869                 body.m_body_subtree_mass_com = body.m_body_mass_com;
 870                 body.m_body_subtree_I_body = body.m_body_I_body;
 871
 872                 for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++)
 873                 {
 874                         RigidBody &child = m_body_list[m_child_indices[i][c]];
 875                         mat33 body_T_child = child.m_body_T_parent.transpose();
 876
 877                         body.m_subtree_mass += child.m_subtree_mass;
 878                         body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com +
 879                                                                                         child.m_parent_pos_parent_body * child.m_subtree_mass;
 880                         body.m_body_subtree_I_body +=
 881                                 body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent;
 882
 883                         if (child.m_subtree_mass > 0)
 884                         {
 885                                 // Shift the reference point for the child subtree inertia using the
 886                                 // Huygens-Steiner ("parallel axis") theorem.
 887                                 // (First shift from child origin to child com, then from there to this body's
 888                                 // origin)
 889                                 vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass;
 890                                 mat33 tilde_r_child_com = tildeOperator(r_com);
 891                                 mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com);
 892                                 body.m_body_subtree_I_body +=
 893                                         child.m_subtree_mass *
 894                                         (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com);
 895                         }
 896                 }
 897         }
 898
 899         for (int i = m_body_list.size() - 1; i >= 0; i--)
 900         {
 901                 const RigidBody &body = m_body_list[i];
 902
 903                 // determine DoF-range for body
 904                 const int q_index_min = body.m_q_index;
 905                 const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1;
 906                 // loop over the DoFs used by this body
 907                 // local joint jacobians (ok as is for 1-DoF joints)
 908                 vec3 Jac_JR = body.m_Jac_JR;
 909                 vec3 Jac_JT = body.m_Jac_JT;
 910                 for (int col = q_index_max; col >= q_index_min; col--)
 911                 {
 912                         // set jacobians for 6-DoF joints
 913                         if (FLOATING == body.m_joint_type)
 914                         {
 915                                 setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
 916                         }
 917                         if (SPHERICAL == body.m_joint_type)
 918                         {
 919                                 //todo: review
 920                                 setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
 921                         }
 922
 923                         vec3 body_eom_rot =
 924                                 body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
 925                         vec3 body_eom_trans =
 926                                 body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
 927                         setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
 928
 929                         // rest of the mass matrix column upwards
 930                         {
 931                                 // 1. for multi-dof joints, rest of the dofs of this body
 932                                 for (int row = col - 1; row >= q_index_min; row--)
 933                                 {
 934                                         if (SPHERICAL == body.m_joint_type)
 935                                         {
 936                                                 //todo: review
 937                                                 setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
 938                                                 const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
 939                                                 setMatxxElem(col, row, Mrc, mass_matrix);
 940                                         }
 941                                         if (FLOATING == body.m_joint_type)
 942                                         {
 943                                                 setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
 944                                                 const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
 945                                                 setMatxxElem(col, row, Mrc, mass_matrix);
 946                                         }
 947                                 }
 948                                 // 2. ancestor dofs
 949                                 int child_idx = i;
 950                                 int parent_idx = m_parent_index[i];
 951                                 while (parent_idx >= 0)
 952                                 {
 953                                         const RigidBody &child_body = m_body_list[child_idx];
 954                                         const RigidBody &parent_body = m_body_list[parent_idx];
 955
 956                                         const mat33 parent_T_child = child_body.m_body_T_parent.transpose();
 957                                         body_eom_rot = parent_T_child * body_eom_rot;
 958                                         body_eom_trans = parent_T_child * body_eom_trans;
 959                                         body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans);
 960
 961                                         const int parent_body_q_index_min = parent_body.m_q_index;
 962                                         const int parent_body_q_index_max =
 963                                                 parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
 964                                         vec3 Jac_JR = parent_body.m_Jac_JR;
 965                                         vec3 Jac_JT = parent_body.m_Jac_JT;
 966                                         for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
 967                                         {
 968                                                 if (SPHERICAL == parent_body.m_joint_type)
 969                                                 {
 970                                                         //todo: review
 971                                                         setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
 972                                                 }
 973                                                 // set jacobians for 6-DoF joints
 974                                                 if (FLOATING == parent_body.m_joint_type)
 975                                                 {
 976                                                         setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
 977                                                 }
 978                                                 const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
 979                                                 setMatxxElem(col, row, Mrc, mass_matrix);
 980                                         }
 981
 982                                         child_idx = parent_idx;
 983                                         parent_idx = m_parent_index[child_idx];
 984                                 }
 985                         }
 986                 }
 987         }
 988
 989         if (set_lower_triangular_matrix)
 990         {
 991                 for (int col = 0; col < m_num_dofs; col++)
 992                 {
 993                         for (int row = 0; row < col; row++)
 994                         {
 995                                 setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
 996                         }
 997                 }
 998         }
 999         return 0;
1000 }
1001
1002 // utility macro
1003 #define CHECK_IF_BODY_INDEX_IS_VALID(index)                                                  \
1004         do                                                                                       \
1005         {                                                                                        \
1006                 if (index < 0 || index >= m_num_bodies)                                              \
1007                 {                                                                                    \
1008                         bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
1009                         return -1;                                                                       \
1010                 }                                                                                    \
1011         } while (0)
1012
1013 int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p)
1014 {
1015         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1016         *p = m_parent_index[body_index];
1017         return 0;
1018 }
1019
1020 int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const
1021 {
1022         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1023         *user_int = m_user_int[body_index];
1024         return 0;
1025 }
1026 int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const
1027 {
1028         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1029         *user_ptr = m_user_ptr[body_index];
1030         return 0;
1031 }
1032
1033 int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int)
1034 {
1035         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1036         m_user_int[body_index] = user_int;
1037         return 0;
1038 }
1039
1040 int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr)
1041 {
1042         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1043         m_user_ptr[body_index] = user_ptr;
1044         return 0;
1045 }
1046
1047 int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const
1048 {
1049         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1050         const RigidBody &body = m_body_list[body_index];
1051         *world_origin = body.m_body_T_world.transpose() * body.m_body_pos;
1052         return 0;
1053 }
1054
1055 int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const
1056 {
1057         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1058         const RigidBody &body = m_body_list[body_index];
1059         if (body.m_mass > 0)
1060         {
1061                 *world_com = body.m_body_T_world.transpose() *
1062                                          (body.m_body_pos + body.m_body_mass_com / body.m_mass);
1063         }
1064         else
1065         {
1066                 *world_com = body.m_body_T_world.transpose() * (body.m_body_pos);
1067         }
1068         return 0;
1069 }
1070
1071 int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const
1072 {
1073         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1074         const RigidBody &body = m_body_list[body_index];
1075         *world_T_body = body.m_body_T_world.transpose();
1076         return 0;
1077 }
1078 int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const
1079 {
1080         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1081         const RigidBody &body = m_body_list[body_index];
1082         *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel;
1083         return 0;
1084 }
1085 int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
1086                                                                                                                 vec3 *world_velocity) const
1087 {
1088         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1089         const RigidBody &body = m_body_list[body_index];
1090         *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel;
1091         return 0;
1092 }
1093
1094 int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
1095                                                                                                                    vec3 *world_velocity) const
1096 {
1097         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1098         const RigidBody &body = m_body_list[body_index];
1099         vec3 com;
1100         if (body.m_mass > 0)
1101         {
1102                 com = body.m_body_mass_com / body.m_mass;
1103         }
1104         else
1105         {
1106                 com(0) = 0;
1107                 com(1) = 0;
1108                 com(2) = 0;
1109         }
1110
1111         *world_velocity =
1112                 body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com));
1113         return 0;
1114 }
1115
1116 int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index,
1117                                                                                                                          vec3 *world_dot_omega) const
1118 {
1119         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1120         const RigidBody &body = m_body_list[body_index];
1121         *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc;
1122         return 0;
1123 }
1124 int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index,
1125                                                                                                                         vec3 *world_acceleration) const
1126 {
1127         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1128         const RigidBody &body = m_body_list[body_index];
1129         *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc;
1130         return 0;
1131 }
1132
1133 int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const
1134 {
1135         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1136         *joint_type = m_body_list[body_index].m_joint_type;
1137         return 0;
1138 }
1139
1140 int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
1141                                                                                                   const char **joint_type) const
1142 {
1143         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1144         *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type);
1145         return 0;
1146 }
1147
1148 int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3 *r) const
1149 {
1150         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1151         *r = m_body_list[body_index].m_parent_pos_parent_body_ref;
1152         return 0;
1153 }
1154
1155 int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33 *T) const
1156 {
1157         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1158         *T = m_body_list[body_index].m_body_T_parent_ref;
1159         return 0;
1160 }
1161
1162 int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
1163 {
1164         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1165         if (m_body_list[body_index].m_joint_type == REVOLUTE)
1166         {
1167                 *axis = m_body_list[body_index].m_Jac_JR;
1168                 return 0;
1169         }
1170         if (m_body_list[body_index].m_joint_type == PRISMATIC)
1171         {
1172                 *axis = m_body_list[body_index].m_Jac_JT;
1173                 return 0;
1174         }
1175         setZero(*axis);
1176         return 0;
1177 }
1178
1179 int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const
1180 {
1181         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1182         *q_index = m_body_list[body_index].m_q_index;
1183         return 0;
1184 }
1185
1186 int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass)
1187 {
1188         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1189         m_body_list[body_index].m_mass = mass;
1190         return 0;
1191 }
1192
1193 int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index,
1194                                                                                                                  const vec3 &first_mass_moment)
1195 {
1196         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1197         m_body_list[body_index].m_body_mass_com = first_mass_moment;
1198         return 0;
1199 }
1200 int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index,
1201                                                                                                                   const mat33 &second_mass_moment)
1202 {
1203         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1204         m_body_list[body_index].m_body_I_body = second_mass_moment;
1205         return 0;
1206 }
1207 int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const
1208 {
1209         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1210         *mass = m_body_list[body_index].m_mass;
1211         return 0;
1212 }
1213 int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index,
1214                                                                                                                  vec3 *first_mass_moment) const
1215 {
1216         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1217         *first_mass_moment = m_body_list[body_index].m_body_mass_com;
1218         return 0;
1219 }
1220 int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index,
1221                                                                                                                   mat33 *second_mass_moment) const
1222 {
1223         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1224         *second_mass_moment = m_body_list[body_index].m_body_I_body;
1225         return 0;
1226 }
1227
1228 void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments()
1229 {
1230         for (int index = 0; index < m_num_bodies; index++)
1231         {
1232                 RigidBody &body = m_body_list[index];
1233                 setZero(body.m_body_force_user);
1234                 setZero(body.m_body_moment_user);
1235         }
1236 }
1237
1238 int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force)
1239 {
1240         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1241         m_body_list[body_index].m_body_force_user += body_force;
1242         return 0;
1243 }
1244
1245 int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment)
1246 {
1247         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1248         m_body_list[body_index].m_body_moment_user += body_moment;
1249         return 0;
1250 }
1251
1252 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
1253 int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
1254 {
1255         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1256         const RigidBody &body = m_body_list[body_index];
1257         *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
1258         return 0;
1259 }
1260
1261 int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
1262 {
1263         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1264         const RigidBody &body = m_body_list[body_index];
1265         *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
1266         return 0;
1267 }
1268
1269 int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
1270 {
1271         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1272         const RigidBody &body = m_body_list[body_index];
1273         mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
1274         return 0;
1275 }
1276
1277 int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
1278 {
1279         CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1280         const RigidBody &body = m_body_list[body_index];
1281         mul(body.m_body_T_world.transpose(), body.m_body_Jac_R, world_jac_rot);
1282         return 0;
1283 }
1284
1285 #endif
1286 }  // namespace btInverseDynamics