AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

Renamed error_message and warning_message macros.

Browse Source 
error_message and warning_message are common strings that are likely to
collide. Renamed to bt_id_{error,warning}_message to more strongly
uniquify.

grep -r error_message -l src/BulletInverseDynamics | \
  xargs sed -i -e "s/error_message/bt_id_error_message/g"

grep -r warning_message -l src/BulletInverseDynamics | \
  xargs sed -i -e "s/warning_message/bt_id_warning_message/g"
This commit is contained in:
Michael Beardsworth 2018-07-24 11:19:42 -07:00
parent 0e1dce41ea
commit 3f03b41091
17 changed files with 124 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Extras/InverseDynamics/CloneTreeCreator.cpp
View File

@ -6,7 +6,7 @@ namespace btInverseDynamics {
#define CHECK_NULLPTR() \
do { \
if (m_reference == 0x0) { \
error_message("m_reference == 0x0\n"); \
bt_id_error_message("m_reference == 0x0\n"); \
return -1; \
} \
} while (0)
@ -14,7 +14,7 @@ namespace btInverseDynamics {
#define TRY(x) \
do { \
if (x == -1) { \
error_message("error calling " #x "\n"); \
bt_id_error_message("error calling " #x "\n"); \
return -1; \
} \
} while (0)

2
Extras/InverseDynamics/CoilCreator.cpp
View File

@ -48,7 +48,7 @@ int CoilCreator::getBody(int body_index, int* parent_index, JointType* joint_typ
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
if (body_index < 0 || body_index >= m_num_bodies) {
error_message("invalid body index %d\n", body_index);
bt_id_error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];

8
Extras/InverseDynamics/DillCreator.cpp
View File

@ -32,7 +32,7 @@ DillCreator::DillCreator(int level)
const idScalar alpha_DH = 0.0;
if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) {
error_message("recurseDill failed\n");
bt_id_error_message("recurseDill failed\n");
abort();
}
}
@ -49,7 +49,7 @@ int DillCreator::getBody(const int body_index, int* parent_index, JointType* joi
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
if (body_index < 0 || body_index >= m_num_bodies) {
error_message("invalid body index %d\n", body_index);
bt_id_error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
@ -69,12 +69,12 @@ int DillCreator::getBody(const int body_index, int* parent_index, JointType* joi
int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in) {
if (level < 0) {
error_message("invalid level parameter (%d)\n", level);
bt_id_error_message("invalid level parameter (%d)\n", level);
return -1;
}
if (m_current_body >= m_num_bodies || m_current_body < 0) {
error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
bt_id_error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
m_num_bodies);
return -1;
}

16
Extras/InverseDynamics/MultiBodyNameMap.cpp
View File

@ -6,11 +6,11 @@ MultiBodyNameMap::MultiBodyNameMap() {}
int MultiBodyNameMap::addBody(const int index, const std::string& name) {
if (m_index_to_body_name.count(index) > 0) {
error_message("trying to add index %d again\n", index);
bt_id_error_message("trying to add index %d again\n", index);
return -1;
}
if (m_body_name_to_index.count(name) > 0) {
error_message("trying to add name %s again\n", name.c_str());
bt_id_error_message("trying to add name %s again\n", name.c_str());
return -1;
}
@ -22,11 +22,11 @@ int MultiBodyNameMap::addBody(const int index, const std::string& name) {
int MultiBodyNameMap::addJoint(const int index, const std::string& name) {
if (m_index_to_joint_name.count(index) > 0) {
error_message("trying to add index %d again\n", index);
bt_id_error_message("trying to add index %d again\n", index);
return -1;
}
if (m_joint_name_to_index.count(name) > 0) {
error_message("trying to add name %s again\n", name.c_str());
bt_id_error_message("trying to add name %s again\n", name.c_str());
return -1;
}
@ -39,7 +39,7 @@ int MultiBodyNameMap::addJoint(const int index, const std::string& name) {
int MultiBodyNameMap::getBodyName(const int index, std::string* name) const {
std::map<int, std::string>::const_iterator it = m_index_to_body_name.find(index);
if (it == m_index_to_body_name.end()) {
error_message("index %d not known\n", index);
bt_id_error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
@ -49,7 +49,7 @@ int MultiBodyNameMap::getBodyName(const int index, std::string* name) const {
int MultiBodyNameMap::getJointName(const int index, std::string* name) const {
std::map<int, std::string>::const_iterator it = m_index_to_joint_name.find(index);
if (it == m_index_to_joint_name.end()) {
error_message("index %d not known\n", index);
bt_id_error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
@ -59,7 +59,7 @@ int MultiBodyNameMap::getJointName(const int index, std::string* name) const {
int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const {
std::map<std::string, int>::const_iterator it = m_body_name_to_index.find(name);
if (it == m_body_name_to_index.end()) {
error_message("name %s not known\n", name.c_str());
bt_id_error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
@ -69,7 +69,7 @@ int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const {
int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const {
std::map<std::string, int>::const_iterator it = m_joint_name_to_index.find(name);
if (it == m_joint_name_to_index.end()) {
error_message("name %s not known\n", name.c_str());
bt_id_error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;

10
Extras/InverseDynamics/MultiBodyTreeCreator.cpp
View File

@ -17,7 +17,7 @@ MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) {
MultiBodyTree* tree = new MultiBodyTree();
if (0x0 == tree) {
error_message("cannot allocate tree\n");
bt_id_error_message("cannot allocate tree\n");
return 0x0;
}
@ -26,7 +26,7 @@ MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) {
// get number of bodies in the system
if (-1 == creator.getNumBodies(&num_bodies)) {
error_message("getting body indices\n");
bt_id_error_message("getting body indices\n");
delete tree;
return 0x0;
}
@ -38,7 +38,7 @@ MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) {
creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
&body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
&body_I_body, &user_int, &user_ptr)) {
error_message("getting data for body %d\n", index);
bt_id_error_message("getting data for body %d\n", index);
delete tree;
return 0x0;
}
@ -47,14 +47,14 @@ MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) {
tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
body_I_body, user_int, user_ptr)) {
error_message("adding body %d\n", index);
bt_id_error_message("adding body %d\n", index);
delete tree;
return 0x0;
}
}
// finalize initialization
if (-1 == tree->finalize()) {
error_message("building system\n");
bt_id_error_message("building system\n");
delete tree;
return 0x0;
}

14
Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp
View File

@ -7,17 +7,17 @@ namespace btInverseDynamics {
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename) {
if (0x0 == tree) {
error_message("tree pointer is null\n");
bt_id_error_message("tree pointer is null\n");
return -1;
}
if (0x0 == filename) {
error_message("filename is null\n");
bt_id_error_message("filename is null\n");
return -1;
}
FILE* fp = fopen(filename, "w");
if (NULL == fp) {
error_message("cannot open file %s for writing\n", filename);
bt_id_error_message("cannot open file %s for writing\n", filename);
return -1;
}
fprintf(fp, "// to generate postscript file, run dot -Tps %s -o %s.ps\n"
@ -29,7 +29,7 @@ int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
std::string name;
if (0x0 != map) {
if (-1 == map->getBodyName(body, &name)) {
error_message("can't get name of body %d\n", body);
bt_id_error_message("can't get name of body %d\n", body);
return -1;
}
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str());
@ -40,15 +40,15 @@ int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* joint_type;
int qi;
if (-1 == tree->getParentIndex(body, &parent)) {
error_message("indexing error\n");
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getJointTypeStr(body, &joint_type)) {
error_message("indexing error\n");
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getDoFOffset(body, &qi)) {
error_message("indexing error\n");
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 != parent) {

2
Extras/InverseDynamics/RandomTreeCreator.cpp
View File

@ -47,7 +47,7 @@ int RandomTreeCreator::getBody(const int body_index, int* parent_index, JointTyp
*joint_type = FLOATING;
break;
default:
error_message("randomInt() result out of range\n");
bt_id_error_message("randomInt() result out of range\n");
return -1;
}

6
Extras/InverseDynamics/User2InternalIndex.cpp
View File

@ -35,7 +35,7 @@ int User2InternalIndex::buildMapping() {
user_root_index = current_root_index;
} else {
if (user_root_index != current_root_index) {
error_message("multiple roots (at least) %d and %d\n", user_root_index,
bt_id_error_message("multiple roots (at least) %d and %d\n", user_root_index,
current_root_index);
return -1;
}
@ -75,7 +75,7 @@ int User2InternalIndex::user2internal(const int user, int *internal) const {
*internal = it->second;
return 0;
} else {
error_message("no user index %d\n", user);
bt_id_error_message("no user index %d\n", user);
return -1;
}
}
@ -92,7 +92,7 @@ int User2InternalIndex::internal2user(const int internal, int *user) const {
*user = it->second;
return 0;
} else {
error_message("no internal index %d\n", internal);
bt_id_error_message("no internal index %d\n", internal);
return -1;
}
}

14
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
View File

@ -6,7 +6,7 @@ btMultiBodyTreeCreator::btMultiBodyTreeCreator() : m_initialized(false) {}
int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) {
if (0x0 == btmb) {
error_message("cannot create MultiBodyTree from null pointer\n");
bt_id_error_message("cannot create MultiBodyTree from null pointer\n");
return -1;
}
@ -184,10 +184,10 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::eSpherical:
error_message("spherical joints not implemented\n");
bt_id_error_message("spherical joints not implemented\n");
return -1;
case btMultibodyLink::ePlanar:
error_message("planar joints not implemented\n");
bt_id_error_message("planar joints not implemented\n");
return -1;
case btMultibodyLink::eFixed:
link.joint_type = FIXED;
@ -203,7 +203,7 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
default:
error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_id_error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_link.m_jointType);
return -1;
}
@ -231,7 +231,7 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const {
if (false == m_initialized) {
error_message("btMultiBody not converted yet\n");
bt_id_error_message("btMultiBody not converted yet\n");
return -1;
}
@ -245,12 +245,12 @@ int btMultiBodyTreeCreator::getBody(const int body_index, int *parent_index, Joi
vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
void **user_ptr) const {
if (false == m_initialized) {
error_message("MultiBodyTree not created yet\n");
bt_id_error_message("MultiBodyTree not created yet\n");
return -1;
}
if (body_index < 0 || body_index >= static_cast<int>(m_data.size())) {
error_message("index out of range (got %d but only %zu bodies)\n", body_index,
bt_id_error_message("index out of range (got %d but only %zu bodies)\n", body_index,
m_data.size());
return -1;
}

18
Extras/InverseDynamics/invdyn_bullet_comparison.cpp
View File

@ -12,11 +12,11 @@ namespace btInverseDynamics {
int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error) {
// call function and return -1 if it does, printing an error_message
// call function and return -1 if it does, printing an bt_id_error_message
#define RETURN_ON_FAILURE(x) \
do { \
if (-1 == x) { \
error_message("calling " #x "\n"); \
bt_id_error_message("calling " #x "\n"); \
return -1; \
} \
} while (0)
@ -75,13 +75,13 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
}
// sanity check
if (q_index != q.size()) {
error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// run inverse dynamics to determine joint_forces for given q, u, dot_u
if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces)) {
error_message("calculating inverse dynamics\n");
bt_id_error_message("calculating inverse dynamics\n");
return -1;
}
@ -143,7 +143,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
// sanity check
if (q_index != q.size()) {
error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
@ -223,12 +223,12 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
{
mat33 world_T_body;
if (-1 == id_tree->getBodyTransform(0, &world_T_body)) {
error_message("getting transform for body %d\n", 0);
bt_id_error_message("getting transform for body %d\n", 0);
return -1;
}
vec3 world_com;
if (-1 == id_tree->getBodyCoM(0, &world_com)) {
error_message("getting com for body %d\n", 0);
bt_id_error_message("getting com for body %d\n", 0);
return -1;
}
if (verbose) {
@ -261,11 +261,11 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
vec3 id_world_com;
if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body)) {
error_message("getting transform for body %d\n", l);
bt_id_error_message("getting transform for body %d\n", l);
return -1;
}
if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com)) {
error_message("getting com for body %d\n", l);
bt_id_error_message("getting com for body %d\n", l);
return -1;
}
if (verbose) {

8
src/BulletInverseDynamics/IDErrorMessages.hpp
View File

@ -7,19 +7,19 @@
#if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2)
#include "Bullet3Common/b3Logging.h"
#define error_message(...) b3Error(__VA_ARGS__)
#define warning_message(...) b3Warning(__VA_ARGS__)
#define bt_id_error_message(...) b3Error(__VA_ARGS__)
#define bt_id_warning_message(...) b3Warning(__VA_ARGS__)
#define id_printf(...) b3Printf(__VA_ARGS__)
#else // BT_ID_WO_BULLET
#include <cstdio>
/// print error message with file/line information
#define error_message(...) \
#define bt_id_error_message(...) \
do { \
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
} while (0)
/// print warning message with file/line information
#define warning_message(...) \
#define bt_id_warning_message(...) \
do { \
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \

42
src/BulletInverseDynamics/IDMath.cpp
View File

@ -81,7 +81,7 @@ idScalar maxAbsMat3x(const mat3x &m) {
void mul(const mat33 &a, const mat3x &b, mat3x *result) {
if (b.cols() != result->cols()) {
error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}
@ -97,7 +97,7 @@ void mul(const mat33 &a, const mat3x &b, mat3x *result) {
}
void add(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
@ -109,7 +109,7 @@ void add(const mat3x &a, const mat3x &b, mat3x *result) {
}
void sub(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
@ -305,10 +305,10 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// the determinant of the inertia tensor about the joint axis is almost
// zero and can have a very small negative value.
if (!isPositiveSemiDefiniteFuzzy(I)) {
error_message("invalid inertia matrix for body %d, not positive definite "
bt_id_error_message("invalid inertia matrix for body %d, not positive definite "
"(fixed joint)\n",
index);
error_message("matrix is:\n"
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -321,8 +321,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
if (!has_fixed_joint) {
if (I(0, 0) + I(1, 1) < I(2, 2)) {
error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
error_message("matrix is:\n"
bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -331,8 +331,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
return false;
}
if (I(0, 0) + I(1, 1) < I(2, 2)) {
error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
error_message("matrix is:\n"
bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -341,8 +341,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
return false;
}
if (I(1, 1) + I(2, 2) < I(0, 0)) {
error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
error_message("matrix is:\n"
bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -354,25 +354,25 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// check positive/zero diagonal elements
for (int i = 0; i < 3; i++) {
if (I(i, i) < 0) { // accept zero
error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
return false;
}
}
// check symmetry
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) {
error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
bt_id_error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
"%e\n",
index, I(1, 0) - I(0, 1));
return false;
}
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) {
error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
bt_id_error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
"%e\n",
index, I(2, 0) - I(0, 2));
return false;
}
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) {
error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
I(1, 2) - I(2, 1));
return false;
}
@ -381,7 +381,7 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
bool isValidTransformMatrix(const mat33 &m) {
#define print_mat(x) \
error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
// check for unit length column vectors
@ -389,7 +389,7 @@ bool isValidTransformMatrix(const mat33 &m) {
const idScalar length_minus_1 =
BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
if (length_minus_1 > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (column %d not unit length)\n"
bt_id_error_message("Not a valid rotation matrix (column %d not unit length)\n"
"column = [%.18e %.18e %.18e]\n"
"length-1.0= %.18e\n",
i, m(0, i), m(1, i), m(2, i), length_minus_1);
@ -399,23 +399,23 @@ bool isValidTransformMatrix(const mat33 &m) {
}
// check for orthogonal column vectors
if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
print_mat(m);
return false;
}
if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
// check determinant (rotation not reflection)
if (determinant(m) <= 0) {
error_message("Not a valid rotation matrix (determinant <=0)\n");
bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
print_mat(m);
return false;
}

44
src/BulletInverseDynamics/MultiBodyTree.cpp
View File

@ -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<idScalar>::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;
}
@ -386,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;
}

8
src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
View File

@ -78,7 +78,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -92,7 +92,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -121,7 +121,7 @@ public:
}
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<rows();i++) {
@ -139,7 +139,7 @@ public:
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;

10
src/BulletInverseDynamics/details/IDMatVec.hpp
View File

@ -123,7 +123,7 @@ public:
};
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<3*m_cols;i++) {
@ -336,7 +336,7 @@ inline vec3 operator/(const vec3& a, const idScalar& s) {
inline const vecx& vecx::operator=(const vecx& rhs) {
if (size() != rhs.size()) {
error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
abort();
}
if (&rhs != this) {
@ -356,7 +356,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -369,7 +369,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -389,7 +389,7 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;

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

@ -80,7 +80,7 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
case FLOATING:
return 6;
}
error_message("unknown joint type %d\n", type);
bt_id_error_message("unknown joint type %d\n", type);
return 0;
}
@ -136,13 +136,13 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
q_index += 6;
break;
default:
error_message("unsupported joint type %d\n", body.m_joint_type);
bt_id_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);
bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
return -1;
}
@ -155,10 +155,10 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
} 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);
bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
} else {
// should never happen
error_message(
bt_id_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()));
}
@ -234,7 +234,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
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"
bt_id_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()));
@ -242,7 +242,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
}
// 1. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
error_message("error in calculateKinematics\n");
bt_id_error_message("error in calculateKinematics\n");
return -1;
}
// 2. update contributions to equations of motion for every body.
@ -322,14 +322,14 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
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"
bt_id_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);
bt_id_error_message("invalid type %d\n", type);
return -1;
}
@ -516,13 +516,13 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body)
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"
bt_id_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);
bt_id_error_message("invalid type %d\n", type);
return -1;
}
@ -606,7 +606,7 @@ static inline int jointNumDoFs(const JointType &type) {
return 6;
}
// this should never happen
error_message("invalid joint type\n");
bt_id_error_message("invalid joint type\n");
// TODO add configurable abort/crash function
abort();
return 0;
@ -626,7 +626,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
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"
bt_id_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()));
@ -734,7 +734,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// 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");
bt_id_error_message("??\n");
return -1;
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
@ -788,7 +788,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
#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); \
bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
return -1; \
} \
} while (0)

12
src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
View File

@ -29,13 +29,13 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
m_num_dofs += 6;
break;
default:
error_message("unknown joint type %d\n", joint_type);
bt_id_error_message("unknown joint type %d\n", joint_type);
return -1;
}
if(-1 == parent_index) {
if(m_root_index>=0) {
error_message("trying to add body %d as root, but already added %d as root body\n",
bt_id_error_message("trying to add body %d as root, but already added %d as root body\n",
body_index, m_root_index);
return -1;
}
@ -63,7 +63,7 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
}
int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
if (index < 0 || index > static_cast<int>(m_inertias.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
@ -73,7 +73,7 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
if (index < 0 || index > static_cast<int>(m_user_int.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
*user_int = m_user_int[index];
@ -82,7 +82,7 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
*user_ptr = m_user_ptr[index];
@ -91,7 +91,7 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
if (index < 0 || index > static_cast<int>(m_joints.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
*joint = m_joints[index];