diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics')
| -rw-r--r-- | thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp | 2 | ||||
| -rw-r--r-- | thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp | 33 |
2 files changed, 31 insertions, 4 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp index f150b5ae4c..9326b0d098 100644 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp @@ -349,7 +349,7 @@ int MultiBodyTree::finalize() const int &num_bodies = m_init_cache->numBodies(); const int &num_dofs = m_init_cache->numDoFs(); - if (num_dofs <= 0) + if (num_dofs < 0) { bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); //return -1; diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index befbc2e2a4..ec9a562295 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -479,9 +479,17 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx //todo: review RigidBody &body = m_body_list[m_body_spherical_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)); + mat33 T; + + T = transformX(q(body.m_q_index)) * + transformY(q(body.m_q_index + 1)) * + transformZ(q(body.m_q_index + 2)); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; if (type >= POSITION_VELOCITY) @@ -832,6 +840,25 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; } + + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_list[i]]; + + mat33 T; + + T = transformX(q(body.m_q_index)) * + transformY(q(body.m_q_index + 1)) * + transformZ(q(body.m_q_index + 2)); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + + 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--) { |