summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletInverseDynamics
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics')
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp8
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/IDMath.cpp42
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp60
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp12
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp10
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp30
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp12
7 files changed, 95 insertions, 79 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp b/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp
index 1dc22f860a..1b3fd268a0 100644
--- a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp
+++ b/thirdparty/bullet/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 <cstdio>
/// print error message with file/line information
-#define error_message(...) \
+#define bt_id_error_message(...) \
do { \
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
} while (0)
/// print warning message with file/line information
-#define warning_message(...) \
+#define bt_id_warning_message(...) \
do { \
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp
index 99fe20e492..d279d3435c 100644
--- a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp
+++ b/thirdparty/bullet/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<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}
@@ -97,7 +97,7 @@ void mul(const mat33 &a, const mat3x &b, mat3x *result) {
}
void add(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
- error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+ bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
@@ -109,7 +109,7 @@ void add(const mat3x &a, const mat3x &b, mat3x *result) {
}
void sub(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
- error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+ bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
@@ -305,10 +305,10 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// the determinant of the inertia tensor about the joint axis is almost
// zero and can have a very small negative value.
if (!isPositiveSemiDefiniteFuzzy(I)) {
- error_message("invalid inertia matrix for body %d, not positive definite "
+ bt_id_error_message("invalid inertia matrix for body %d, not positive definite "
"(fixed joint)\n",
index);
- error_message("matrix is:\n"
+ bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@@ -321,8 +321,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
if (!has_fixed_joint) {
if (I(0, 0) + I(1, 1) < I(2, 2)) {
- error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
- error_message("matrix is:\n"
+ bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+ bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@@ -331,8 +331,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
return false;
}
if (I(0, 0) + I(1, 1) < I(2, 2)) {
- error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
- error_message("matrix is:\n"
+ bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+ bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@@ -341,8 +341,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
return false;
}
if (I(1, 1) + I(2, 2) < I(0, 0)) {
- error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
- error_message("matrix is:\n"
+ bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
+ bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@@ -354,25 +354,25 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// check positive/zero diagonal elements
for (int i = 0; i < 3; i++) {
if (I(i, i) < 0) { // accept zero
- error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
+ bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
return false;
}
}
// check symmetry
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) {
- error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
+ bt_id_error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
"%e\n",
index, I(1, 0) - I(0, 1));
return false;
}
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) {
- error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
+ bt_id_error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
"%e\n",
index, I(2, 0) - I(0, 2));
return false;
}
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) {
- error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
+ bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
I(1, 2) - I(2, 1));
return false;
}
@@ -381,7 +381,7 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
bool isValidTransformMatrix(const mat33 &m) {
#define print_mat(x) \
- error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
+ bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
// check for unit length column vectors
@@ -389,7 +389,7 @@ bool isValidTransformMatrix(const mat33 &m) {
const idScalar length_minus_1 =
BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
if (length_minus_1 > kAxisLengthEpsilon) {
- error_message("Not a valid rotation matrix (column %d not unit length)\n"
+ bt_id_error_message("Not a valid rotation matrix (column %d not unit length)\n"
"column = [%.18e %.18e %.18e]\n"
"length-1.0= %.18e\n",
i, m(0, i), m(1, i), m(2, i), length_minus_1);
@@ -399,23 +399,23 @@ bool isValidTransformMatrix(const mat33 &m) {
}
// check for orthogonal column vectors
if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
- error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
+ bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
print_mat(m);
return false;
}
if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
- error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+ bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
- error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+ bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
// check determinant (rotation not reflection)
if (determinant(m) <= 0) {
- error_message("Not a valid rotation matrix (determinant <=0)\n");
+ bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
print_mat(m);
return false;
}
diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
index c67588d49f..becfe0f4a2 100644
--- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
+++ b/thirdparty/bullet/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<idScalar>::min())) {
- error_message("axis of motion vector too short (%e)\n", length);
+ bt_id_error_message("axis of motion vector too short (%e)\n", length);
return -1;
}
body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
@@ -241,14 +241,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
case FLOATING:
break;
default:
- error_message("unknown joint type %d\n", joint_type);
+ bt_id_error_message("unknown joint type %d\n", joint_type);
return -1;
}
// sanity check for mass properties. Zero mass is OK.
if (mass < 0) {
m_mass_parameters_are_valid = false;
- error_message("Body %d has invalid mass %e\n", body_index, mass);
+ bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
if (!m_accept_invalid_mass_parameters) {
return -1;
}
@@ -296,7 +296,7 @@ int MultiBodyTree::finalize() {
const int &num_dofs = m_init_cache->numDoFs();
if(num_dofs<=0) {
- error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
+ bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
//return -1;
}
@@ -331,6 +331,22 @@ int MultiBodyTree::finalize() {
rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
rigid_body.m_joint_type = joint.m_type;
+ int user_int;
+ if (-1 == m_init_cache->getUserInt(index, &user_int)) {
+ return -1;
+ }
+ if (-1 == m_impl->setUserInt(index, user_int)) {
+ return -1;
+ }
+
+ void* user_ptr;
+ if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) {
+ return -1;
+ }
+ if (-1 == m_impl->setUserPtr(index, user_ptr)) {
+ return -1;
+ }
+
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
// matrices.
switch (rigid_body.m_joint_type) {
@@ -370,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/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
index 5bb4a33bdd..c179daeec6 100644
--- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
+++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
@@ -49,9 +49,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
class vecx : public btVectorX<idScalar> {
public:
- vecx(int size) : btVectorX(size) {}
+ vecx(int size) : btVectorX<idScalar>(size) {}
const vecx& operator=(const btVectorX<idScalar>& rhs) {
- *static_cast<btVectorX*>(this) = rhs;
+ *static_cast<btVectorX<idScalar>*>(this) = rhs;
return *this;
}
@@ -78,7 +78,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
- error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+ bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@@ -92,7 +92,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
- error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+ bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@@ -121,7 +121,7 @@ public:
}
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
- error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+ bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<rows();i++) {
@@ -139,7 +139,7 @@ public:
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
- error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+ bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
index 4d3f6c87e9..c89db5e123 100644
--- a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
+++ b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
@@ -123,7 +123,7 @@ public:
};
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
- error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+ bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<3*m_cols;i++) {
@@ -336,7 +336,7 @@ inline vec3 operator/(const vec3& a, const idScalar& s) {
inline const vecx& vecx::operator=(const vecx& rhs) {
if (size() != rhs.size()) {
- error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
+ bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
abort();
}
if (&rhs != this) {
@@ -356,7 +356,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
- error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+ bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@@ -369,7 +369,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
- error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+ bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@@ -389,7 +389,7 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
- error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+ bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
index b35c55df61..e8563238c3 100644
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
@@ -80,7 +80,7 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
case FLOATING:
return 6;
}
- error_message("unknown joint type %d\n", type);
+ bt_id_error_message("unknown joint type %d\n", type);
return 0;
}
@@ -136,13 +136,13 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
q_index += 6;
break;
default:
- error_message("unsupported joint type %d\n", body.m_joint_type);
+ bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
return -1;
}
}
// sanity check
if (q_index != m_num_dofs) {
- error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
+ bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
return -1;
}
@@ -155,10 +155,10 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
} else {
if (-1 == parent) {
// multiple bodies are directly linked to the environment, ie, not a single root
- error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
+ bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
} else {
// should never happen
- error_message(
+ bt_id_error_message(
"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
child, parent, static_cast<int>(m_parent_index.size()));
}
@@ -234,7 +234,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
const vecx &dot_u, vecx *joint_forces) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
joint_forces->size() != m_num_dofs) {
- error_message("wrong vector dimension. system has %d DOFs,\n"
+ bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
@@ -242,7 +242,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
}
// 1. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
- error_message("error in calculateKinematics\n");
+ bt_id_error_message("error in calculateKinematics\n");
return -1;
}
// 2. update contributions to equations of motion for every body.
@@ -322,14 +322,14 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
- error_message("wrong vector dimension. system has %d DOFs,\n"
+ bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
static_cast<int>(dot_u.size()));
return -1;
}
if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
- error_message("invalid type %d\n", type);
+ bt_id_error_message("invalid type %d\n", type);
return -1;
}
@@ -516,13 +516,13 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body)
int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
- error_message("wrong vector dimension. system has %d DOFs,\n"
+ bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
return -1;
}
if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
- error_message("invalid type %d\n", type);
+ bt_id_error_message("invalid type %d\n", type);
return -1;
}
@@ -606,7 +606,7 @@ static inline int jointNumDoFs(const JointType &type) {
return 6;
}
// this should never happen
- error_message("invalid joint type\n");
+ bt_id_error_message("invalid joint type\n");
// TODO add configurable abort/crash function
abort();
return 0;
@@ -626,7 +626,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != m_num_dofs) {
- error_message("Dimension error. System has %d DOFs,\n"
+ bt_id_error_message("Dimension error. System has %d DOFs,\n"
"but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
static_cast<int>(mass_matrix->cols()));
@@ -734,7 +734,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// 1. for multi-dof joints, rest of the dofs of this body
for (int row = col - 1; row >= q_index_min; row--) {
if (FLOATING != body.m_joint_type) {
- error_message("??\n");
+ bt_id_error_message("??\n");
return -1;
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
@@ -788,7 +788,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
#define CHECK_IF_BODY_INDEX_IS_VALID(index) \
do { \
if (index < 0 || index >= m_num_bodies) { \
- error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
+ bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
return -1; \
} \
} while (0)
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
index 47b4ab3890..e9511b7076 100644
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
+++ b/thirdparty/bullet/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<int>(m_inertias.size())) {
- error_message("index out of range\n");
+ bt_id_error_message("index out of range\n");
return -1;
}
@@ -73,7 +73,7 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
if (index < 0 || index > static_cast<int>(m_user_int.size())) {
- error_message("index out of range\n");
+ bt_id_error_message("index out of range\n");
return -1;
}
*user_int = m_user_int[index];
@@ -82,7 +82,7 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
- error_message("index out of range\n");
+ bt_id_error_message("index out of range\n");
return -1;
}
*user_ptr = m_user_ptr[index];
@@ -91,7 +91,7 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
if (index < 0 || index > static_cast<int>(m_joints.size())) {
- error_message("index out of range\n");
+ bt_id_error_message("index out of range\n");
return -1;
}
*joint = m_joints[index];
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-28 15:34:40 +0000