AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +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() \ #define CHECK_NULLPTR() \
do { \ do { \
if (m_reference == 0x0) { \ if (m_reference == 0x0) { \
error_message("m_reference == 0x0\n"); \ bt_id_error_message("m_reference == 0x0\n"); \
return -1; \ return -1; \
} \ } \
} while (0) } while (0)
@ -14,7 +14,7 @@ namespace btInverseDynamics {
#define TRY(x) \ #define TRY(x) \
do { \ do { \
if (x == -1) { \ if (x == -1) { \
error_message("error calling " #x "\n"); \ bt_id_error_message("error calling " #x "\n"); \
return -1; \ return -1; \
} \ } \
} while (0) } 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, vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const { mat33* body_I_body, int* user_int, void** user_ptr) const {
if (body_index < 0 || body_index >= m_num_bodies) { 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; return -1;
} }
*parent_index = m_parent[body_index]; *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; const idScalar alpha_DH = 0.0;
if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) { 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(); 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, vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const { mat33* body_I_body, int* user_int, void** user_ptr) const {
if (body_index < 0 || body_index >= m_num_bodies) { 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; return -1;
} }
*parent_index = m_parent[body_index]; *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, int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in) { const idScalar a_DH_in, const idScalar alpha_DH_in) {
if (level < 0) { if (level < 0) {
error_message("invalid level parameter (%d)\n", level); bt_id_error_message("invalid level parameter (%d)\n", level);
return -1; return -1;
} }
if (m_current_body >= m_num_bodies || m_current_body < 0) { 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); m_num_bodies);
return -1; 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) { int MultiBodyNameMap::addBody(const int index, const std::string& name) {
if (m_index_to_body_name.count(index) > 0) { 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; return -1;
} }
if (m_body_name_to_index.count(name) > 0) { 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; 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) { int MultiBodyNameMap::addJoint(const int index, const std::string& name) {
if (m_index_to_joint_name.count(index) > 0) { 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; return -1;
} }
if (m_joint_name_to_index.count(name) > 0) { 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; 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 { 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); std::map<int, std::string>::const_iterator it = m_index_to_body_name.find(index);
if (it == m_index_to_body_name.end()) { 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; return -1;
} }
*name = it->second; *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 { 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); std::map<int, std::string>::const_iterator it = m_index_to_joint_name.find(index);
if (it == m_index_to_joint_name.end()) { 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; return -1;
} }
*name = it->second; *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 { 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); std::map<std::string, int>::const_iterator it = m_body_name_to_index.find(name);
if (it == m_body_name_to_index.end()) { 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; return -1;
} }
*index = it->second; *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 { 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); std::map<std::string, int>::const_iterator it = m_joint_name_to_index.find(name);
if (it == m_joint_name_to_index.end()) { 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; return -1;
} }
*index = it->second; *index = it->second;

10
Extras/InverseDynamics/MultiBodyTreeCreator.cpp
View File

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

14
Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp
View File

@ -7,17 +7,17 @@ namespace btInverseDynamics {
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map, int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename) { const char* filename) {
if (0x0 == tree) { if (0x0 == tree) {
error_message("tree pointer is null\n"); bt_id_error_message("tree pointer is null\n");
return -1; return -1;
} }
if (0x0 == filename) { if (0x0 == filename) {
error_message("filename is null\n"); bt_id_error_message("filename is null\n");
return -1; return -1;
} }
FILE* fp = fopen(filename, "w"); FILE* fp = fopen(filename, "w");
if (NULL == fp) { 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; return -1;
} }
fprintf(fp, "// to generate postscript file, run dot -Tps %s -o %s.ps\n" 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; std::string name;
if (0x0 != map) { if (0x0 != map) {
if (-1 == map->getBodyName(body, &name)) { 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; return -1;
} }
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str()); 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; const char* joint_type;
int qi; int qi;
if (-1 == tree->getParentIndex(body, &parent)) { if (-1 == tree->getParentIndex(body, &parent)) {
error_message("indexing error\n"); bt_id_error_message("indexing error\n");
return -1; return -1;
} }
if (-1 == tree->getJointTypeStr(body, &joint_type)) { if (-1 == tree->getJointTypeStr(body, &joint_type)) {
error_message("indexing error\n"); bt_id_error_message("indexing error\n");
return -1; return -1;
} }
if (-1 == tree->getDoFOffset(body, &qi)) { if (-1 == tree->getDoFOffset(body, &qi)) {
error_message("indexing error\n"); bt_id_error_message("indexing error\n");
return -1; return -1;
} }
if (-1 != parent) { 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; *joint_type = FLOATING;
break; break;
default: default:
error_message("randomInt() result out of range\n"); bt_id_error_message("randomInt() result out of range\n");
return -1; return -1;
} }

6
Extras/InverseDynamics/User2InternalIndex.cpp
View File

@ -35,7 +35,7 @@ int User2InternalIndex::buildMapping() {
user_root_index = current_root_index; user_root_index = current_root_index;
} else { } else {
if (user_root_index != current_root_index) { 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); current_root_index);
return -1; return -1;
} }
@ -75,7 +75,7 @@ int User2InternalIndex::user2internal(const int user, int *internal) const {
*internal = it->second; *internal = it->second;
return 0; return 0;
} else { } else {
error_message("no user index %d\n", user); bt_id_error_message("no user index %d\n", user);
return -1; return -1;
} }
} }
@ -92,7 +92,7 @@ int User2InternalIndex::internal2user(const int internal, int *user) const {
*user = it->second; *user = it->second;
return 0; return 0;
} else { } else {
error_message("no internal index %d\n", internal); bt_id_error_message("no internal index %d\n", internal);
return -1; 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) { int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) {
if (0x0 == btmb) { 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; 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]; link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break; break;
case btMultibodyLink::eSpherical: case btMultibodyLink::eSpherical:
error_message("spherical joints not implemented\n"); bt_id_error_message("spherical joints not implemented\n");
return -1; return -1;
case btMultibodyLink::ePlanar: case btMultibodyLink::ePlanar:
error_message("planar joints not implemented\n"); bt_id_error_message("planar joints not implemented\n");
return -1; return -1;
case btMultibodyLink::eFixed: case btMultibodyLink::eFixed:
link.joint_type = FIXED; 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]; link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break; break;
default: default:
error_message("unknown btMultiBody::eFeatherstoneJointType %d\n", bt_id_error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_link.m_jointType); bt_link.m_jointType);
return -1; return -1;
} }
@ -231,7 +231,7 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const { int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const {
if (false == m_initialized) { if (false == m_initialized) {
error_message("btMultiBody not converted yet\n"); bt_id_error_message("btMultiBody not converted yet\n");
return -1; 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, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
void **user_ptr) const { void **user_ptr) const {
if (false == m_initialized) { if (false == m_initialized) {
error_message("MultiBodyTree not created yet\n"); bt_id_error_message("MultiBodyTree not created yet\n");
return -1; return -1;
} }
if (body_index < 0 || body_index >= static_cast<int>(m_data.size())) { 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()); m_data.size());
return -1; 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, int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error, btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_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) \ #define RETURN_ON_FAILURE(x) \
do { \ do { \
if (-1 == x) { \ if (-1 == x) { \
error_message("calling " #x "\n"); \ bt_id_error_message("calling " #x "\n"); \
return -1; \ return -1; \
} \ } \
} while (0) } while (0)
@ -75,13 +75,13 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
} }
// sanity check // sanity check
if (q_index != q.size()) { 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; return -1;
} }
// run inverse dynamics to determine joint_forces for given q, u, dot_u // run inverse dynamics to determine joint_forces for given q, u, dot_u
if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces)) { 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; return -1;
} }
@ -143,7 +143,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
// sanity check // sanity check
if (q_index != q.size()) { 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; return -1;
} }
@ -223,12 +223,12 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
{ {
mat33 world_T_body; mat33 world_T_body;
if (-1 == id_tree->getBodyTransform(0, &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; return -1;
} }
vec3 world_com; vec3 world_com;
if (-1 == id_tree->getBodyCoM(0, &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; return -1;
} }
if (verbose) { if (verbose) {
@ -261,11 +261,11 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
vec3 id_world_com; vec3 id_world_com;
if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body)) { 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; return -1;
} }
if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com)) { 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; return -1;
} }
if (verbose) { 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) #if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2)
#include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Logging.h"
#define error_message(...) b3Error(__VA_ARGS__) #define bt_id_error_message(...) b3Error(__VA_ARGS__)
#define warning_message(...) b3Warning(__VA_ARGS__) #define bt_id_warning_message(...) b3Warning(__VA_ARGS__)
#define id_printf(...) b3Printf(__VA_ARGS__) #define id_printf(...) b3Printf(__VA_ARGS__)
#else // BT_ID_WO_BULLET #else // BT_ID_WO_BULLET
#include <cstdio> #include <cstdio>
/// print error message with file/line information /// print error message with file/line information
#define error_message(...) \ #define bt_id_error_message(...) \
do { \ do { \
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \ fprintf(stderr, __VA_ARGS__); \
} while (0) } while (0)
/// print warning message with file/line information /// print warning message with file/line information
#define warning_message(...) \ #define bt_id_warning_message(...) \
do { \ do { \
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \ 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) { void mul(const mat33 &a, const mat3x &b, mat3x *result) {
if (b.cols() != result->cols()) { 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())); static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort(); 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) { void add(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) { 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())); static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort(); 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) { void sub(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) { 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())); static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort(); 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 // the determinant of the inertia tensor about the joint axis is almost
// zero and can have a very small negative value. // zero and can have a very small negative value.
if (!isPositiveSemiDefiniteFuzzy(I)) { 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", "(fixed joint)\n",
index); 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" "%.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) // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
if (!has_fixed_joint) { if (!has_fixed_joint) {
if (I(0, 0) + I(1, 1) < I(2, 2)) { 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); bt_id_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("matrix is:\n"
"[%.20e %.20e %.20e;\n" "[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\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; return false;
} }
if (I(0, 0) + I(1, 1) < I(2, 2)) { 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); bt_id_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("matrix is:\n"
"[%.20e %.20e %.20e;\n" "[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\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; return false;
} }
if (I(1, 1) + I(2, 2) < I(0, 0)) { 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); bt_id_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("matrix is:\n"
"[%.20e %.20e %.20e;\n" "[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\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 // check positive/zero diagonal elements
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (I(i, i) < 0) { // accept zero 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; return false;
} }
} }
// check symmetry // check symmetry
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) { 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", "%e\n",
index, I(1, 0) - I(0, 1)); index, I(1, 0) - I(0, 1));
return false; return false;
} }
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) { 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", "%e\n",
index, I(2, 0) - I(0, 2)); index, I(2, 0) - I(0, 2));
return false; return false;
} }
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) { 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)); I(1, 2) - I(2, 1));
return false; return false;
} }
@ -381,7 +381,7 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
bool isValidTransformMatrix(const mat33 &m) { bool isValidTransformMatrix(const mat33 &m) {
#define print_mat(x) \ #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)) x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
// check for unit length column vectors // check for unit length column vectors
@ -389,7 +389,7 @@ bool isValidTransformMatrix(const mat33 &m) {
const idScalar length_minus_1 = 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); 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) { 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" "column = [%.18e %.18e %.18e]\n"
"length-1.0= %.18e\n", "length-1.0= %.18e\n",
i, m(0, i), m(1, i), m(2, i), length_minus_1); 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 // 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) { 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); print_mat(m);
return false; 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) { 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); print_mat(m);
return false; 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) { 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); print_mat(m);
return false; return false;
} }
// check determinant (rotation not reflection) // check determinant (rotation not reflection)
if (determinant(m) <= 0) { 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); print_mat(m);
return false; 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, int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
vecx *joint_forces) { vecx *joint_forces) {
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { 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 -1;
} }
return 0; return 0;
@ -97,13 +97,13 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati
const bool initialize_matrix, const bool initialize_matrix,
const bool set_lower_triangular_matrix, matxx *mass_matrix) { const bool set_lower_triangular_matrix, matxx *mass_matrix) {
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == if (-1 ==
m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
set_lower_triangular_matrix, mass_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 -1;
} }
return 0; return 0;
@ -121,12 +121,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx&
setZero(m_impl->m_world_gravity); setZero(m_impl->m_world_gravity);
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == m_impl->calculateKinematics(q, u, dot_u, if (-1 == m_impl->calculateKinematics(q, u, dot_u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) { MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
error_message("error in kinematics calculation\n"); bt_id_error_message("error in kinematics calculation\n");
return -1; return -1;
} }
@ -137,12 +137,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx&
int MultiBodyTree::calculatePositionKinematics(const vecx& q) { int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == m_impl->calculateKinematics(q, q, q, if (-1 == m_impl->calculateKinematics(q, q, q,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in kinematics calculation\n"); bt_id_error_message("error in kinematics calculation\n");
return -1; return -1;
} }
return 0; return 0;
@ -150,12 +150,12 @@ int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) { int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == m_impl->calculateKinematics(q, u, u, if (-1 == m_impl->calculateKinematics(q, u, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in kinematics calculation\n"); bt_id_error_message("error in kinematics calculation\n");
return -1; return -1;
} }
return 0; 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) #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) { int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == m_impl->calculateJacobians(q, u, if (-1 == m_impl->calculateJacobians(q, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in jacobian calculation\n"); bt_id_error_message("error in jacobian calculation\n");
return -1; return -1;
} }
return 0; return 0;
@ -178,12 +178,12 @@ int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
int MultiBodyTree::calculateJacobians(const vecx& q){ int MultiBodyTree::calculateJacobians(const vecx& q){
if (false == m_is_finalized) { 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; return -1;
} }
if (-1 == m_impl->calculateJacobians(q, q, if (-1 == m_impl->calculateJacobians(q, q,
MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) { MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
error_message("error in jacobian calculation\n"); bt_id_error_message("error in jacobian calculation\n");
return -1; return -1;
} }
return 0; 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 vec3 &body_r_body_com, const mat33 &body_I_body,
const int user_int, void *user_ptr) { const int user_int, void *user_ptr) {
if (body_index < 0) { 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; return -1;
} }
vec3 body_axis_of_motion(body_axis_of_motion_); 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: case PRISMATIC:
// check if axis is unit vector // check if axis is unit vector
if (!isUnitVector(body_axis_of_motion)) { 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", "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)); 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) + 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(1), 2) +
BT_ID_POW(body_axis_of_motion(2), 2)); BT_ID_POW(body_axis_of_motion(2), 2));
if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) { 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; return -1;
} }
body_axis_of_motion = (1.0 / length) * body_axis_of_motion; 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: case FLOATING:
break; break;
default: default:
error_message("unknown joint type %d\n", joint_type); bt_id_error_message("unknown joint type %d\n", joint_type);
return -1; return -1;
} }
// sanity check for mass properties. Zero mass is OK. // sanity check for mass properties. Zero mass is OK.
if (mass < 0) { if (mass < 0) {
m_mass_parameters_are_valid = false; 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) { if (!m_accept_invalid_mass_parameters) {
return -1; return -1;
} }
@ -296,7 +296,7 @@ int MultiBodyTree::finalize() {
const int &num_dofs = m_init_cache->numDoFs(); const int &num_dofs = m_init_cache->numDoFs();
if(num_dofs<=0) { 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; //return -1;
} }
@ -386,14 +386,14 @@ int MultiBodyTree::finalize() {
rigid_body.m_Jac_JT(2) = 0.0; rigid_body.m_Jac_JT(2) = 0.0;
break; break;
default: 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; return -1;
} }
} }
// 4 assign degree of freedom indices & build per-joint-type index arrays // 4 assign degree of freedom indices & build per-joint-type index arrays
if (-1 == m_impl->generateIndexSets()) { if (-1 == m_impl->generateIndexSets()) {
error_message("generating index sets\n"); bt_id_error_message("generating index sets\n");
return -1; 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()); vecx result(a.size());
// TODO: error handling for a.size() != b.size()?? // TODO: error handling for a.size() != b.size()??
if (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(); abort();
} }
for (int i = 0; i < a.size(); i++) { 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()); vecx result(a.size());
// TODO: error handling for a.size() != b.size()?? // TODO: error handling for a.size() != b.size()??
if (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(); abort();
} }
for (int i = 0; i < a.size(); i++) { for (int i = 0; i < a.size(); i++) {
@ -121,7 +121,7 @@ public:
} }
void operator=(const mat3x& rhs) { void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) { 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(); abort();
} }
for(int i=0;i<rows();i++) { for(int i=0;i<rows();i++) {
@ -139,7 +139,7 @@ public:
inline vec3 operator*(const mat3x& a, const vecx& b) { inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result; vec3 result;
if (a.cols() != b.size()) { 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(); abort();
} }
result(0)=0.0; result(0)=0.0;

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

@ -123,7 +123,7 @@ public:
}; };
void operator=(const mat3x& rhs) { void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) { 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(); abort();
} }
for(int i=0;i<3*m_cols;i++) { 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) { inline const vecx& vecx::operator=(const vecx& rhs) {
if (size() != rhs.size()) { 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(); abort();
} }
if (&rhs != this) { if (&rhs != this) {
@ -356,7 +356,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size()); vecx result(a.size());
// TODO: error handling for a.size() != b.size()?? // TODO: error handling for a.size() != b.size()??
if (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(); abort();
} }
for (int i = 0; i < a.size(); i++) { 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()); vecx result(a.size());
// TODO: error handling for a.size() != b.size()?? // TODO: error handling for a.size() != b.size()??
if (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(); abort();
} }
for (int i = 0; i < a.size(); i++) { 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) { inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result; vec3 result;
if (a.cols() != b.size()) { 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(); abort();
} }
result(0)=0.0; 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: case FLOATING:
return 6; return 6;
} }
error_message("unknown joint type %d\n", type); bt_id_error_message("unknown joint type %d\n", type);
return 0; return 0;
} }
@ -136,13 +136,13 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
q_index += 6; q_index += 6;
break; break;
default: 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; return -1;
} }
} }
// sanity check // sanity check
if (q_index != m_num_dofs) { 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; return -1;
} }
@ -155,10 +155,10 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
} else { } else {
if (-1 == parent) { if (-1 == parent) {
// multiple bodies are directly linked to the environment, ie, not a single root // 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 { } else {
// should never happen // should never happen
error_message( bt_id_error_message(
"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
child, parent, static_cast<int>(m_parent_index.size())); 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) { const vecx &dot_u, vecx *joint_forces) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
joint_forces->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", "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()), 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())); 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 // 1. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { 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; return -1;
} }
// 2. update contributions to equations of motion for every body. // 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, int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
const KinUpdateType type) { const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) { 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", "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()), m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
static_cast<int>(dot_u.size())); static_cast<int>(dot_u.size()));
return -1; return -1;
} }
if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { 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; 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) { int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs) { 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", "but dim(q)= %d, dim(u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
return -1; return -1;
} }
if(type != POSITION_ONLY && type != POSITION_VELOCITY) { 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; return -1;
} }
@ -606,7 +606,7 @@ static inline int jointNumDoFs(const JointType &type) {
return 6; return 6;
} }
// this should never happen // this should never happen
error_message("invalid joint type\n"); bt_id_error_message("invalid joint type\n");
// TODO add configurable abort/crash function // TODO add configurable abort/crash function
abort(); abort();
return 0; 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 || if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != 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", "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()), m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
static_cast<int>(mass_matrix->cols())); 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 // 1. for multi-dof joints, rest of the dofs of this body
for (int row = col - 1; row >= q_index_min; row--) { for (int row = col - 1; row >= q_index_min; row--) {
if (FLOATING != body.m_joint_type) { if (FLOATING != body.m_joint_type) {
error_message("??\n"); bt_id_error_message("??\n");
return -1; return -1;
} }
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); 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) \ #define CHECK_IF_BODY_INDEX_IS_VALID(index) \
do { \ do { \
if (index < 0 || index >= m_num_bodies) { \ 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; \ return -1; \
} \ } \
} while (0) } 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; m_num_dofs += 6;
break; break;
default: default:
error_message("unknown joint type %d\n", joint_type); bt_id_error_message("unknown joint type %d\n", joint_type);
return -1; return -1;
} }
if(-1 == parent_index) { if(-1 == parent_index) {
if(m_root_index>=0) { 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); body_index, m_root_index);
return -1; 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 { int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
if (index < 0 || index > static_cast<int>(m_inertias.size())) { 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; 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 { int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
if (index < 0 || index > static_cast<int>(m_user_int.size())) { 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; return -1;
} }
*user_int = m_user_int[index]; *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 { int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) { 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; return -1;
} }
*user_ptr = m_user_ptr[index]; *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 { int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
if (index < 0 || index > static_cast<int>(m_joints.size())) { 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; return -1;
} }
*joint = m_joints[index]; *joint = m_joints[index];