From 6142448417f4e15bf0bc0c94df7d1862a790e3c7 Mon Sep 17 00:00:00 2001 From: Andrea Catania Date: Fri, 7 Sep 2018 16:11:04 +0200 Subject: Update bullet to Master 12409f1118a7c7a266f9071350c70789dfe73bb9 --- .../bullet/BulletInverseDynamics/MultiBodyTree.cpp | 60 ++++++++++++++-------- 1 file changed, 38 insertions(+), 22 deletions(-) (limited to 'thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp') diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp index c67588d49f..becfe0f4a2 100644 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp @@ -83,11 +83,11 @@ int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; } int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u, vecx *joint_forces) { if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { - error_message("error in inverse dynamics calculation\n"); + bt_id_error_message("error in inverse dynamics calculation\n"); return -1; } return 0; @@ -97,13 +97,13 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati const bool initialize_matrix, const bool set_lower_triangular_matrix, matxx *mass_matrix) { if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, set_lower_triangular_matrix, mass_matrix)) { - error_message("error in mass matrix calculation\n"); + bt_id_error_message("error in mass matrix calculation\n"); return -1; } return 0; @@ -121,12 +121,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& setZero(m_impl->m_world_gravity); if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateKinematics(q, u, dot_u, MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) { - error_message("error in kinematics calculation\n"); + bt_id_error_message("error in kinematics calculation\n"); return -1; } @@ -137,12 +137,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& int MultiBodyTree::calculatePositionKinematics(const vecx& q) { if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateKinematics(q, q, q, MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { - error_message("error in kinematics calculation\n"); + bt_id_error_message("error in kinematics calculation\n"); return -1; } return 0; @@ -150,12 +150,12 @@ int MultiBodyTree::calculatePositionKinematics(const vecx& q) { int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) { if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateKinematics(q, u, u, MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { - error_message("error in kinematics calculation\n"); + bt_id_error_message("error in kinematics calculation\n"); return -1; } return 0; @@ -165,12 +165,12 @@ int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const v #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) { if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateJacobians(q, u, MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { - error_message("error in jacobian calculation\n"); + bt_id_error_message("error in jacobian calculation\n"); return -1; } return 0; @@ -178,12 +178,12 @@ int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) { int MultiBodyTree::calculateJacobians(const vecx& q){ if (false == m_is_finalized) { - error_message("system has not been initialized\n"); + bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateJacobians(q, q, MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) { - error_message("error in jacobian calculation\n"); + bt_id_error_message("error in jacobian calculation\n"); return -1; } return 0; @@ -214,7 +214,7 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ const vec3 &body_r_body_com, const mat33 &body_I_body, const int user_int, void *user_ptr) { if (body_index < 0) { - error_message("body index must be positive (got %d)\n", body_index); + bt_id_error_message("body index must be positive (got %d)\n", body_index); return -1; } vec3 body_axis_of_motion(body_axis_of_motion_); @@ -223,14 +223,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ case PRISMATIC: // check if axis is unit vector if (!isUnitVector(body_axis_of_motion)) { - warning_message( + bt_id_warning_message( "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) + BT_ID_POW(body_axis_of_motion(1), 2) + BT_ID_POW(body_axis_of_motion(2), 2)); if (length < BT_ID_SQRT(std::numeric_limits::min())) { - error_message("axis of motion vector too short (%e)\n", length); + bt_id_error_message("axis of motion vector too short (%e)\n", length); return -1; } body_axis_of_motion = (1.0 / length) * body_axis_of_motion; @@ -241,14 +241,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ case FLOATING: break; default: - error_message("unknown joint type %d\n", joint_type); + bt_id_error_message("unknown joint type %d\n", joint_type); return -1; } // sanity check for mass properties. Zero mass is OK. if (mass < 0) { m_mass_parameters_are_valid = false; - error_message("Body %d has invalid mass %e\n", body_index, mass); + bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass); if (!m_accept_invalid_mass_parameters) { return -1; } @@ -296,7 +296,7 @@ int MultiBodyTree::finalize() { const int &num_dofs = m_init_cache->numDoFs(); if(num_dofs<=0) { - error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); + bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); //return -1; } @@ -331,6 +331,22 @@ int MultiBodyTree::finalize() { rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; rigid_body.m_joint_type = joint.m_type; + int user_int; + if (-1 == m_init_cache->getUserInt(index, &user_int)) { + return -1; + } + if (-1 == m_impl->setUserInt(index, user_int)) { + return -1; + } + + void* user_ptr; + if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) { + return -1; + } + if (-1 == m_impl->setUserPtr(index, user_ptr)) { + return -1; + } + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized // matrices. switch (rigid_body.m_joint_type) { @@ -370,14 +386,14 @@ int MultiBodyTree::finalize() { rigid_body.m_Jac_JT(2) = 0.0; break; default: - error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type); return -1; } } // 4 assign degree of freedom indices & build per-joint-type index arrays if (-1 == m_impl->generateIndexSets()) { - error_message("generating index sets\n"); + bt_id_error_message("generating index sets\n"); return -1; } -- cgit v1.2.3