diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp')
| -rw-r--r-- | thirdparty/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp | 1028 |
1 files changed, 0 insertions, 1028 deletions
diff --git a/thirdparty/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp deleted file mode 100644 index b35c55df61..0000000000 --- a/thirdparty/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ /dev/null @@ -1,1028 +0,0 @@ -#include "MultiBodyTreeImpl.hpp" - -namespace btInverseDynamics { - -MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) - : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - ,m_m3x(3,m_num_dofs) -#endif -{ - -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - resize(m_m3x,m_num_dofs); -#endif - m_body_list.resize(num_bodies_); - m_parent_index.resize(num_bodies_); - m_child_indices.resize(num_bodies_); - m_user_int.resize(num_bodies_); - m_user_ptr.resize(num_bodies_); - - m_world_gravity(0) = 0.0; - m_world_gravity(1) = 0.0; - m_world_gravity(2) = -9.8; -} - -const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { - switch (type) { - case FIXED: - return "fixed"; - case REVOLUTE: - return "revolute"; - case PRISMATIC: - return "prismatic"; - case FLOATING: - return "floating"; - } - return "error: invalid"; -} - -inline void indent(const int &level) { - for (int j = 0; j < level; j++) - id_printf(" "); // indent -} - -void MultiBodyTree::MultiBodyImpl::printTree() { - id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); - printTree(0, 0); -} - -void MultiBodyTree::MultiBodyImpl::printTreeData() { - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - id_printf("body: %d\n", static_cast<int>(i)); - id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); - id_printf("q_index= %d\n", body.m_q_index); - id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); - id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); - - id_printf("mass = %f\n", body.m_mass); - id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), - body.m_body_mass_com(2)); - id_printf("I_o= [%f %f %f;\n" - " %f %f %f;\n" - " %f %f %f]\n", - body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), - body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), - body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); - - id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), - body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); - } -} -int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { - switch (type) { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - } - error_message("unknown joint type %d\n", type); - return 0; -} - -void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { - // this is adapted from URDF2Bullet. - // TODO: fix this and print proper graph (similar to git --log --graph) - int num_children = m_child_indices[index].size(); - - indentation += 2; - int count = 0; - - for (int i = 0; i < num_children; i++) { - int child_index = m_child_indices[index][i]; - indent(indentation); - id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, - jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, - m_body_list[index].m_q_index, - m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); - // first grandchild - printTree(child_index, indentation); - } -} - -int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { - m_world_gravity = gravity; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::generateIndexSets() { - m_body_revolute_list.resize(0); - m_body_prismatic_list.resize(0); - int q_index = 0; - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - body.m_q_index = -1; - switch (body.m_joint_type) { - case REVOLUTE: - m_body_revolute_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case PRISMATIC: - m_body_prismatic_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case FIXED: - // do nothing - break; - case FLOATING: - m_body_floating_list.push_back(i); - body.m_q_index = q_index; - q_index += 6; - break; - default: - error_message("unsupported joint type %d\n", body.m_joint_type); - return -1; - } - } - // sanity check - if (q_index != m_num_dofs) { - error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); - return -1; - } - - m_child_indices.resize(m_body_list.size()); - - for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { - const int &parent = m_parent_index[child]; - if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) { - m_child_indices[parent].push_back(child); - } else { - if (-1 == parent) { - // multiple bodies are directly linked to the environment, ie, not a single root - error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); - } else { - // should never happen - error_message( - "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", - child, parent, static_cast<int>(m_parent_index.size())); - } - return -1; - } - } - - return 0; -} - -void MultiBodyTree::MultiBodyImpl::calculateStaticData() { - // relative kinematics that are not a function of q, u, dot_u - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) { - case REVOLUTE: - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - break; - case PRISMATIC: - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - break; - case FIXED: - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - break; - case FLOATING: - // no static data - break; - } - - // resize & initialize jacobians to zero. -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - body.m_body_dot_Jac_T_u(0) = 0.0; - body.m_body_dot_Jac_T_u(1) = 0.0; - body.m_body_dot_Jac_T_u(2) = 0.0; - body.m_body_dot_Jac_R_u(0) = 0.0; - body.m_body_dot_Jac_R_u(1) = 0.0; - body.m_body_dot_Jac_R_u(2) = 0.0; - resize(body.m_body_Jac_T,m_num_dofs); - resize(body.m_body_Jac_R,m_num_dofs); - body.m_body_Jac_T.setZero(); - body.m_body_Jac_R.setZero(); -#endif // - } -} - -int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, - const vecx &dot_u, vecx *joint_forces) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || - joint_forces->size() != m_num_dofs) { - error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), - static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size())); - return -1; - } - // 1. relative kinematics - if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { - error_message("error in calculateKinematics\n"); - return -1; - } - // 2. update contributions to equations of motion for every body. - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - // 3.4 update dynamic terms (rate of change of angular & linear momentum) - body.m_eom_lhs_rotational = - body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + - body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - - body.m_body_moment_user; - body.m_eom_lhs_translational = - body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + - body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - - body.m_body_force_user; - } - - // 3. calculate full set of forces at parent joint - // (not directly calculating the joint force along the free direction - // simplifies inclusion of fixed joints. - // An alternative would be to fuse bodies in a pre-processing step, - // but that would make changing masses online harder (eg, payload masses - // added with fixed joints to a gripper) - // Also, this enables adding zero weight bodies as a way to calculate frame poses - // for force elements, etc. - - for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { - // sum of forces and moments acting on this body from its children - vec3 sum_f_children; - vec3 sum_m_children; - setZero(sum_f_children); - setZero(sum_m_children); - for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); - child_list_idx++) { - const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; - vec3 child_joint_force_in_this_frame = - child.m_body_T_parent.transpose() * child.m_force_at_joint; - sum_f_children -= child_joint_force_in_this_frame; - sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + - child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); - } - RigidBody &body = m_body_list[body_idx]; - - body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; - body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; - } - - // 4. Calculate Joint forces. - // These are the components of force_at_joint/moment_at_joint - // in the free directions given by Jac_JT/Jac_JR - // 4.1 revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); - } - // 4.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); - } - // 4.3 floating bodies (6-DoF joints) - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); - (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); - (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); - - (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); - (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); - (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); - } - - return 0; -} - -int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u, - const KinUpdateType type) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) { - error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), - static_cast<int>(dot_u.size())); - return -1; - } - if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { - error_message("invalid type %d\n", type); - return -1; - } - - // 1. update relative kinematics - // 1.1 for revolute - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - mat33 T; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); - body.m_body_T_parent = T * body.m_body_T_parent_ref; - if(type >= POSITION_VELOCITY) { - body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); - } - } - // 1.2 for prismatic - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - if(type >= POSITION_VELOCITY) { - body.m_parent_vel_rel = - body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); - } - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - - if(type >= POSITION_VELOCITY) { - body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); - body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); - body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); - - body.m_parent_vel_rel(0) = u(body.m_q_index + 3); - body.m_parent_vel_rel(1) = u(body.m_q_index + 4); - body.m_parent_vel_rel(2) = u(body.m_q_index + 5); - - body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); - body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); - body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); - - body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); - body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); - body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); - - body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; - } - } - - // 2. absolute kinematic quantities (vector valued) - // NOTE: this should be optimized by specializing for different body types - // (e.g., relative rotation is always zero for prismatic joints, etc.) - - // calculations for root body - { - RigidBody &body = m_body_list[0]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) - // not required right now, added here for debugging purposes - body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_body_T_world = body.m_body_T_parent; - - if(type >= POSITION_VELOCITY) { - // 3.2 update absolute velocities - body.m_body_ang_vel = body.m_body_ang_vel_rel; - body.m_body_vel = body.m_parent_vel_rel; - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = body.m_body_ang_acc_rel; - body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; - // add gravitational acceleration to root body - // this is an efficient way to add gravitational terms, - // but it does mean that the kinematics are no longer - // correct at the acceleration level - // NOTE: To get correct acceleration kinematics, just set world_gravity to zero - body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; - } - } - - for (idArrayIdx i = 1; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - RigidBody &parent = m_body_list[m_parent_index[i]]; - // 2.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) not required right now added here for debugging purposes - body.m_body_pos = - body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); - body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; - - if(type >= POSITION_VELOCITY) { - // 2.2 update absolute velocities - body.m_body_ang_vel = - body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; - - body.m_body_vel = - body.m_body_T_parent * - (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + - body.m_parent_vel_rel); - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - // 2.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = - body.m_body_T_parent * parent.m_body_ang_acc - - body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + - body.m_body_ang_acc_rel; - body.m_body_acc = - body.m_body_T_parent * - (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + - parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + - 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); - } - } - - return 0; -} - -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - -void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) { - const int& idx=body.m_q_index; - switch(body.m_joint_type) { - case FIXED: - break; - case REVOLUTE: - setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R); - setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R); - setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R); - break; - case PRISMATIC: - 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), - &body.m_body_Jac_T); - 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), - &body.m_body_Jac_T); - 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), - &body.m_body_Jac_T); - break; - case FLOATING: - setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R); - setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R); - setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R); - // body_Jac_T = body_T_parent.transpose(); - setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T); - setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T); - setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T); - - setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T); - setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T); - setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T); - - setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T); - setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T); - setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T); - - break; - } -} - -int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs) { - error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); - return -1; - } - if(type != POSITION_ONLY && type != POSITION_VELOCITY) { - error_message("invalid type %d\n", type); - return -1; - } - - addRelativeJacobianComponent(m_body_list[0]); - for (idArrayIdx i = 1; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - RigidBody &parent = m_body_list[m_parent_index[i]]; - - mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R); - body.m_body_Jac_T = parent.m_body_Jac_T; - mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x); - sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T); - - addRelativeJacobianComponent(body); - mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T); - - if(type >= POSITION_VELOCITY) { - body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u - - body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel); - body.m_body_dot_Jac_T_u = body.m_body_T_parent * - (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) + - parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + - 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel)); - } - } - return 0; -} -#endif - -static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { - switch (dof) { - // rotational part - case 0: - Jac_JR(0) = 1; - Jac_JR(1) = 0; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 1: - Jac_JR(0) = 0; - Jac_JR(1) = 1; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 2: - Jac_JR(0) = 0; - Jac_JR(1) = 0; - Jac_JR(2) = 1; - setZero(Jac_JT); - break; - // translational part - case 3: - setZero(Jac_JR); - Jac_JT(0) = 1; - Jac_JT(1) = 0; - Jac_JT(2) = 0; - break; - case 4: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 1; - Jac_JT(2) = 0; - break; - case 5: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 0; - Jac_JT(2) = 1; - break; - } -} - -static inline int jointNumDoFs(const JointType &type) { - switch (type) { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - } - // this should never happen - error_message("invalid joint type\n"); - // TODO add configurable abort/crash function - abort(); - return 0; -} - -int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, - const bool initialize_matrix, - const bool set_lower_triangular_matrix, - matxx *mass_matrix) { -// This calculates the joint space mass matrix for the multibody system. -// The algorithm is essentially an implementation of "method 3" -// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) -// (Later named "Composite Rigid Body Algorithm" by Featherstone). -// -// This implementation, however, handles branched systems and uses a formulation centered -// on the origin of the body-fixed frame to avoid re-computing various quantities at the com. - - if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || - mass_matrix->cols() != m_num_dofs) { - error_message("Dimension error. System has %d DOFs,\n" - "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()), - static_cast<int>(mass_matrix->cols())); - return -1; - } - - // TODO add optimized zeroing function? - if (initialize_matrix) { - for (int i = 0; i < m_num_dofs; i++) { - for (int j = 0; j < m_num_dofs; j++) { - setMatxxElem(i, j, 0.0, mass_matrix); - } - } - } - - if (update_kinematics) { - // 1. update relative kinematics - // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // from reference orientation (q=0) of body-fixed frame to current orientation - mat33 body_T_body_ref; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); - body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; - } - // 1.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * - transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - } - } - for (int i = m_body_list.size() - 1; i >= 0; i--) { - RigidBody &body = m_body_list[i]; - // calculate mass, center of mass and inertia of "composite rigid body", - // ie, sub-tree starting at current body - body.m_subtree_mass = body.m_mass; - body.m_body_subtree_mass_com = body.m_body_mass_com; - body.m_body_subtree_I_body = body.m_body_I_body; - - for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { - RigidBody &child = m_body_list[m_child_indices[i][c]]; - mat33 body_T_child = child.m_body_T_parent.transpose(); - - body.m_subtree_mass += child.m_subtree_mass; - body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + - child.m_parent_pos_parent_body * child.m_subtree_mass; - body.m_body_subtree_I_body += - body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; - - if (child.m_subtree_mass > 0) { - // Shift the reference point for the child subtree inertia using the - // Huygens-Steiner ("parallel axis") theorem. - // (First shift from child origin to child com, then from there to this body's - // origin) - vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; - mat33 tilde_r_child_com = tildeOperator(r_com); - mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); - body.m_body_subtree_I_body += - child.m_subtree_mass * - (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); - } - } - } - - for (int i = m_body_list.size() - 1; i >= 0; i--) { - const RigidBody &body = m_body_list[i]; - - // determine DoF-range for body - const int q_index_min = body.m_q_index; - const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; - // loop over the DoFs used by this body - // local joint jacobians (ok as is for 1-DoF joints) - vec3 Jac_JR = body.m_Jac_JR; - vec3 Jac_JT = body.m_Jac_JT; - for (int col = q_index_max; col >= q_index_min; col--) { - // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) { - setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); - } - - vec3 body_eom_rot = - body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); - vec3 body_eom_trans = - body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); - setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix); - - // rest of the mass matrix column upwards - { - // 1. for multi-dof joints, rest of the dofs of this body - for (int row = col - 1; row >= q_index_min; row--) { - if (FLOATING != body.m_joint_type) { - error_message("??\n"); - return -1; - } - setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMatxxElem(col, row, Mrc, mass_matrix); - } - // 2. ancestor dofs - int child_idx = i; - int parent_idx = m_parent_index[i]; - while (parent_idx >= 0) { - const RigidBody &child_body = m_body_list[child_idx]; - const RigidBody &parent_body = m_body_list[parent_idx]; - - const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); - body_eom_rot = parent_T_child * body_eom_rot; - body_eom_trans = parent_T_child * body_eom_trans; - body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); - - const int parent_body_q_index_min = parent_body.m_q_index; - const int parent_body_q_index_max = - parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; - vec3 Jac_JR = parent_body.m_Jac_JR; - vec3 Jac_JT = parent_body.m_Jac_JT; - for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { - // set jacobians for 6-DoF joints - if (FLOATING == parent_body.m_joint_type) { - setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); - } - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMatxxElem(col, row, Mrc, mass_matrix); - } - - child_idx = parent_idx; - parent_idx = m_parent_index[child_idx]; - } - } - } - } - - if (set_lower_triangular_matrix) { - for (int col = 0; col < m_num_dofs; col++) { - for (int row = 0; row < col; row++) { - setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); - } - } - } - return 0; -} - -// utility macro -#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ - do { \ - if (index < 0 || index >= m_num_bodies) { \ - error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ - return -1; \ - } \ - } while (0) - -int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *p = m_parent_index[body_index]; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_int = m_user_int[body_index]; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_ptr = m_user_ptr[body_index]; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_int[body_index] = user_int; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_ptr[body_index] = user_ptr; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - if (body.m_mass > 0) { - *world_com = body.m_body_T_world.transpose() * - (body.m_body_pos + body.m_body_mass_com / body.m_mass); - } else { - *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); - } - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_T_body = body.m_body_T_world.transpose(); - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, - vec3 *world_velocity) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - vec3 com; - if (body.m_mass > 0) { - com = body.m_body_mass_com / body.m_mass; - } else { - com(0) = 0; - com(1) = 0; - com(2) = 0; - } - - *world_velocity = - body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, - vec3 *world_acceleration) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *joint_type = m_body_list[body_index].m_joint_type; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, - const char **joint_type) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *r=m_body_list[body_index].m_parent_pos_parent_body_ref; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *T=m_body_list[body_index].m_body_T_parent_ref; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - if(m_body_list[body_index].m_joint_type == REVOLUTE) { - *axis = m_body_list[body_index].m_Jac_JR; - return 0; - } - if(m_body_list[body_index].m_joint_type == PRISMATIC) { - *axis = m_body_list[body_index].m_Jac_JT; - return 0; - } - setZero(*axis); - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *q_index = m_body_list[body_index].m_q_index; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_mass = mass; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3& first_mass_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_mass_com = first_mass_moment; - return 0; -} -int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, - const mat33& second_mass_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_I_body = second_mass_moment; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *mass = m_body_list[body_index].m_mass; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, - vec3 *first_mass_moment) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *first_mass_moment = m_body_list[body_index].m_body_mass_com; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, - mat33 *second_mass_moment) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *second_mass_moment = m_body_list[body_index].m_body_I_body; - return 0; -} - -void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() { - for (int index = 0; index < m_num_bodies; index++) { - RigidBody &body = m_body_list[index]; - setZero(body.m_body_force_user); - setZero(body.m_body_moment_user); - } -} - -int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_force_user += body_force; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_moment_user += body_moment; - return 0; -} - -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) -int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans); - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot); - return 0; -} - -#endif -} |