From de763a26e7ef742102252df8da9e8216e5056178 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 17 Nov 2015 21:51:02 -0800 Subject: [PATCH] fix portability of BulletInverseDynamics (usual issues: std::pow, M_PI, std::vector->idArray::type, snprintf, pass vec3 and mat33 by const reference, not by value) --- Extras/InverseDynamics/CoilCreator.cpp | 4 ++-- Extras/InverseDynamics/DillCreator.cpp | 22 ++++++++++--------- Extras/InverseDynamics/DillCreator.hpp | 14 ++++++------ Extras/InverseDynamics/IDRandomUtil.cpp | 4 ++-- .../btMultiBodyTreeCreator.hpp | 2 +- src/BulletInverseDynamics/IDConfig.hpp | 11 ++++++++++ src/BulletInverseDynamics/MultiBodyTree.cpp | 4 ++-- src/BulletInverseDynamics/MultiBodyTree.hpp | 4 ++-- .../details/MultiBodyTreeImpl.cpp | 4 ++-- .../details/MultiBodyTreeImpl.hpp | 4 ++-- .../test_invdyn_kinematics.cpp | 12 +++++----- 11 files changed, 49 insertions(+), 36 deletions(-) diff --git a/Extras/InverseDynamics/CoilCreator.cpp b/Extras/InverseDynamics/CoilCreator.cpp index 905ba7a57..6ccfbcf28 100644 --- a/Extras/InverseDynamics/CoilCreator.cpp +++ b/Extras/InverseDynamics/CoilCreator.cpp @@ -12,7 +12,7 @@ CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) { const idScalar theta_DH = 0; const idScalar d_DH = 0.0; const idScalar a_DH = 1.0 / m_num_bodies; - const idScalar alpha_DH = 5.0 * M_PI / m_num_bodies; + const idScalar alpha_DH = 5.0 * BT_ID_PI / m_num_bodies; getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref, &m_body_T_parent_ref); // always z-axis @@ -29,7 +29,7 @@ CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) { m_body_I_body(0, 1) = 0.0; m_body_I_body(0, 2) = 0.0; m_body_I_body(1, 0) = 0.0; - m_body_I_body(1, 1) = (3e-4 + 4.0 / pow(m_num_bodies, 2)) / (12.0 * m_num_bodies); + m_body_I_body(1, 1) = (3e-4 + 4.0 / BT_ID_POW(m_num_bodies, 2)) / (12.0 * m_num_bodies); m_body_I_body(1, 2) = 0.0; m_body_I_body(2, 0) = 0.0; m_body_I_body(2, 1) = 0.0; diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp index cdb3e06a6..8463ab887 100644 --- a/Extras/InverseDynamics/DillCreator.cpp +++ b/Extras/InverseDynamics/DillCreator.cpp @@ -4,14 +4,16 @@ namespace btInverseDynamics { DillCreator::DillCreator(int level) : m_level(level), - m_num_bodies(std::pow(2, level)), - m_parent(m_num_bodies), - m_parent_r_parent_body_ref(m_num_bodies), - m_body_T_parent_ref(m_num_bodies), - m_body_axis_of_motion(m_num_bodies), - m_mass(m_num_bodies), - m_body_r_body_com(m_num_bodies), - m_body_I_body(m_num_bodies) { + m_num_bodies(BT_ID_POW(2, level)) + { + m_parent.resize(m_num_bodies); + m_parent_r_parent_body_ref.resize(m_num_bodies); + m_body_T_parent_ref.resize(m_num_bodies); + m_body_axis_of_motion.resize(m_num_bodies); + m_mass.resize(m_num_bodies); + m_body_r_body_com.resize(m_num_bodies); + m_body_I_body.resize(m_num_bodies); + // generate names (for debugging) for (int i = 0; i < m_num_bodies; i++) { m_parent[i] = i - 1; @@ -85,7 +87,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d /// these parameters are from the paper ... /// TODO: add proper citation m_parent[body] = parent; - m_mass[body] = 0.1 * std::pow(size, 3); + m_mass[body] = 0.1 * BT_ID_POW(size, 3); m_body_r_body_com[body](0) = 0.05 * size; m_body_r_body_com[body](1) = 0; m_body_r_body_com[body](2) = 0; @@ -112,7 +114,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d d_DH = 0.0; } const idScalar a_DH = i * 0.1; - const idScalar alpha_DH = i * M_PI / 3.0; + const idScalar alpha_DH = i * BT_ID_PI / 3.0; m_current_body++; recurseDill(i - 1, body, d_DH, a_DH, alpha_DH); } diff --git a/Extras/InverseDynamics/DillCreator.hpp b/Extras/InverseDynamics/DillCreator.hpp index 42f1b1ba0..d52e0d06d 100644 --- a/Extras/InverseDynamics/DillCreator.hpp +++ b/Extras/InverseDynamics/DillCreator.hpp @@ -34,13 +34,13 @@ private: const idScalar a_DH_in, const idScalar alpha_DH_in); int m_level; int m_num_bodies; - std::vector m_parent; - std::vector m_parent_r_parent_body_ref; - std::vector m_body_T_parent_ref; - std::vector m_body_axis_of_motion; - std::vector m_mass; - std::vector m_body_r_body_com; - std::vector m_body_I_body; + idArray::type m_parent; + idArray::type m_parent_r_parent_body_ref; + idArray::type m_body_T_parent_ref; + idArray::type m_body_axis_of_motion; + idArray::type m_mass; + idArray::type m_body_r_body_com; + idArray::type m_body_I_body; int m_current_body; }; } diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp index 8896eaf55..c3110e5c7 100644 --- a/Extras/InverseDynamics/IDRandomUtil.cpp +++ b/Extras/InverseDynamics/IDRandomUtil.cpp @@ -39,8 +39,8 @@ mat33 randomInertiaMatrix() { // generate random valid inertia matrix by first getting valid components // along major axes and then rotating by random amount vec3 principal = randomInertiaPrincipal(); - mat33 rot(transformX(randomFloat(-M_PI, M_PI)) * transformY(randomFloat(-M_PI, M_PI)) * - transformZ(randomFloat(-M_PI, M_PI))); + mat33 rot(transformX(randomFloat(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) * + transformZ(randomFloat(-BT_ID_PI, BT_ID_PI))); mat33 inertia; inertia(0, 0) = principal(0); inertia(0, 1) = 0; diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp index 1ae00ff79..0e2d614f2 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -42,7 +42,7 @@ private: vec3 body_r_body_com; mat33 body_I_body; }; - std::vector m_data; + idArray::type m_data; bool m_initialized; }; } diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index 6df61071f..4beb75a75 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -5,6 +5,17 @@ // If we have a custom configuration, compile without using other parts of bullet. #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H #define BT_ID_WO_BULLET +#define BT_ID_POW(a,b) std::pow(a,b) +#define BT_ID_SNPRINTF snprintf +#define BT_ID_PI M_PI +#else +#define BT_ID_POW(a,b) btPow(a,b) +#define BT_ID_PI SIMD_PI +#ifdef _WIN32 + #define BT_ID_SNPRINTF _snprintf +#else + #define BT_ID_SNPRINTF snprintf +#endif // #endif // error messages #include "IDErrorMessages.hpp" diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index 3de0d8e4f..5c0af64e9 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -298,11 +298,11 @@ int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { return m_impl->setBodyMass(body_index, mass); } -int MultiBodyTree::setBodyFirstMassMoment(const int body_index, vec3 first_mass_moment) { +int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) { return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); } -int MultiBodyTree::setBodySecondMassMoment(const int body_index, mat33 second_mass_moment) { +int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) { return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); } diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp index 833baf1d3..887005c3a 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -250,13 +250,13 @@ public: /// @param body_index index of the body /// @param first_mass_moment the vector to set /// @return 0 on success, -1 on failure - int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); /// set second moment of mass for a body /// (moment of inertia, in body fixed frame, relative to joint) /// @param body_index index of the body /// @param second_mass_moment the inertia matrix /// @return 0 on success, -1 on failure - int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); /// get mass for a body /// @param body_index index of the body /// @param mass the mass diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 53ef4503d..211139459 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -792,13 +792,13 @@ int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScal } int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3 first_mass_moment) { + 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) { + 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; diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 63d8945eb..286b19d26 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -181,9 +181,9 @@ public: ///\copydoc MultiBodytTree::setBodyMass int setBodyMass(const int body_index, const idScalar mass); ///\copydoc MultiBodytTree::setBodyFirstMassMoment - int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); ///\copydoc MultiBodytTree::setBodySecondMassMoment - int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); ///\copydoc MultiBodytTree::getBodyMass int getBodyMass(const int body_index, idScalar* mass) const; ///\copydoc MultiBodytTree::getBodyFirstMassMoment diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp index dd5d443df..4ee728a0d 100644 --- a/test/InverseDynamics/test_invdyn_kinematics.cpp +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -16,7 +16,7 @@ using namespace btInverseDynamics; const int kLevel = 5; -const int kNumBodies = std::pow(2, kLevel); +const int kNumBodies = BT_ID_POW(2, kLevel); // template function for calculating the norm template @@ -24,7 +24,7 @@ idScalar calculateNorm(T&); // only implemented for vec3 template <> idScalar calculateNorm(vec3& v) { - return std::sqrt(std::pow(v(0), 2) + std::pow(v(1), 2) + std::pow(v(2), 2)); + return std::sqrt(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2)); } // template function to convert a DiffType (finite differences) @@ -136,7 +136,7 @@ public: VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) { for (int i = 0; i < m_fd.size(); i++) { char buf[256]; - snprintf(buf, 256, "%s-%.2d", name.c_str(), i); + BT_ID_SNPRINTF(buf, 256, "%s-%.2d", name.c_str(), i); m_fd[i].init(buf, dt); } } @@ -202,10 +202,10 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar for (idScalar t = 0.0; t < endTime; t += deltaT) { for (int body = 0; body < tree->numBodies(); body++) { - q(body) = kAmplitude * sin(t * 2.0 * M_PI * kFrequency); - dot_q(body) = kAmplitude * 2.0 * M_PI * kFrequency * cos(t * 2.0 * M_PI * kFrequency); + q(body) = kAmplitude * sin(t * 2.0 * BT_ID_PI * kFrequency); + dot_q(body) = kAmplitude * 2.0 * BT_ID_PI * kFrequency * cos(t * 2.0 * BT_ID_PI * kFrequency); ddot_q(body) = - -kAmplitude * pow(2.0 * M_PI * kFrequency, 2) * sin(t * 2.0 * M_PI * kFrequency); + -kAmplitude * pow(2.0 * BT_ID_PI * kFrequency, 2) * sin(t * 2.0 * BT_ID_PI * kFrequency); } if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) {