Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix portability of BulletInverseDynamics (usual issues: std::pow, M_PI, std::vector->idArray<T>::type, snprintf, pass vec3 and mat33 by const reference, not by value)

Browse Source
This commit is contained in:
erwincoumans
2015-11-17 21:51:02 -08:00
parent aa4d119f98
commit de763a26e7
11 changed files with 49 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Extras/InverseDynamics/CoilCreator.cpp
View File

@@ -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;

22
Extras/InverseDynamics/DillCreator.cpp
View File

@@ -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);
}

14
Extras/InverseDynamics/DillCreator.hpp
View File

@@ -34,13 +34,13 @@ private:
const idScalar a_DH_in, const idScalar alpha_DH_in);
int m_level;
int m_num_bodies;
std::vector<int> m_parent;
std::vector<vec3> m_parent_r_parent_body_ref;
std::vector<mat33> m_body_T_parent_ref;
std::vector<vec3> m_body_axis_of_motion;
std::vector<idScalar> m_mass;
std::vector<vec3> m_body_r_body_com;
std::vector<mat33> m_body_I_body;
idArray<int>::type m_parent;
idArray<vec3>::type m_parent_r_parent_body_ref;
idArray<mat33>::type m_body_T_parent_ref;
idArray<vec3>::type m_body_axis_of_motion;
idArray<idScalar>::type m_mass;
idArray<vec3>::type m_body_r_body_com;
idArray<mat33>::type m_body_I_body;
int m_current_body;
};
}

4
Extras/InverseDynamics/IDRandomUtil.cpp
View File

@@ -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;

2
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
View File

@@ -42,7 +42,7 @@ private:
vec3 body_r_body_com;
mat33 body_I_body;
};
std::vector<LinkData> m_data;
idArray<LinkData>::type m_data;
bool m_initialized;
};
}

11
src/BulletInverseDynamics/IDConfig.hpp
View File

@@ -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"

4
src/BulletInverseDynamics/MultiBodyTree.cpp
View File

@@ -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);
}

4
src/BulletInverseDynamics/MultiBodyTree.hpp
View File

@@ -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

4
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
View File

@@ -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;

4
src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
View File

@@ -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

12
test/InverseDynamics/test_invdyn_kinematics.cpp
View File

@@ -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 <typename T>
@@ -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)) {