diff --git a/Extras/InverseDynamics/CloneTreeCreator.cpp b/Extras/InverseDynamics/CloneTreeCreator.cpp index 899b5d996..d267282a4 100644 --- a/Extras/InverseDynamics/CloneTreeCreator.cpp +++ b/Extras/InverseDynamics/CloneTreeCreator.cpp @@ -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) diff --git a/Extras/InverseDynamics/CoilCreator.cpp b/Extras/InverseDynamics/CoilCreator.cpp index 6ccfbcf28..ce37a9922 100644 --- a/Extras/InverseDynamics/CoilCreator.cpp +++ b/Extras/InverseDynamics/CoilCreator.cpp @@ -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]; diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp index 46f68e698..17f30dcd5 100644 --- a/Extras/InverseDynamics/DillCreator.cpp +++ b/Extras/InverseDynamics/DillCreator.cpp @@ -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; } diff --git a/Extras/InverseDynamics/MultiBodyNameMap.cpp b/Extras/InverseDynamics/MultiBodyNameMap.cpp index 4e82641c1..81d33f285 100644 --- a/Extras/InverseDynamics/MultiBodyNameMap.cpp +++ b/Extras/InverseDynamics/MultiBodyNameMap.cpp @@ -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::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::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::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::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; diff --git a/Extras/InverseDynamics/MultiBodyTreeCreator.cpp b/Extras/InverseDynamics/MultiBodyTreeCreator.cpp index 300910d7a..0972f4396 100644 --- a/Extras/InverseDynamics/MultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/MultiBodyTreeCreator.cpp @@ -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; } diff --git a/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp index 587a02be1..aac58e4b3 100644 --- a/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp +++ b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp @@ -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) { diff --git a/Extras/InverseDynamics/RandomTreeCreator.cpp b/Extras/InverseDynamics/RandomTreeCreator.cpp index b2731c590..a531037a2 100644 --- a/Extras/InverseDynamics/RandomTreeCreator.cpp +++ b/Extras/InverseDynamics/RandomTreeCreator.cpp @@ -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; } diff --git a/Extras/InverseDynamics/User2InternalIndex.cpp b/Extras/InverseDynamics/User2InternalIndex.cpp index 3e39cb227..76874bf70 100644 --- a/Extras/InverseDynamics/User2InternalIndex.cpp +++ b/Extras/InverseDynamics/User2InternalIndex.cpp @@ -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; } } diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index e432cf438..1b32df617 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -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(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; } diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp index a32cf828d..4187e134b 100644 --- a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp @@ -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) { diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp index 1dc22f860..1b3fd268a 100644 --- a/src/BulletInverseDynamics/IDErrorMessages.hpp +++ b/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -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 /// 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__); \ diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index 99fe20e49..d279d3435 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -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(b.cols()), static_cast(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(a.cols()), static_cast(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(a.cols()), static_cast(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; } diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index 0f6668fdb..becfe0f4a 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/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; } @@ -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; } diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index a44f779be..c179daeec 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -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(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(q.size()), static_cast(u.size()), static_cast(dot_u.size()), static_cast(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(q.size()), static_cast(u.size()), static_cast(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(q.size()), static_cast(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(q.size()), static_cast(mass_matrix->rows()), static_cast(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) diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index 47b4ab389..e9511b707 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -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(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(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(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(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];