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

Add and use BT_ID_* math functions for idScalar.

Browse Source
This commit is contained in:
Kibeom Kim
2017-01-26 10:59:14 -08:00
parent a230c941f9
commit 761275df28
6 changed files with 53 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/InverseDynamics/IDRandomUtil.cpp
View File

@@ -63,7 +63,7 @@ vec3 randomAxis() {
axis(1) = randomFloat(-1.0, 1.0); axis(1) = randomFloat(-1.0, 1.0);
axis(2) = randomFloat(-1.0, 1.0); axis(2) = randomFloat(-1.0, 1.0);
length = std::sqrt(std::pow(axis(0), 2) + std::pow(axis(1), 2) + std::pow(axis(2), 2)); length = BT_ID_SQRT(BT_ID_POW(axis(0), 2) + BT_ID_POW(axis(1), 2) + BT_ID_POW(axis(2), 2));
} while (length < 0.01); } while (length < 0.01);
return axis / length; return axis / length;

20
Extras/InverseDynamics/invdyn_bullet_comparison.cpp
View File

@@ -172,7 +172,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i)); dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i));
} }
*acc_error += std::pow(joint_accel[i] - dot_u(i + dot_u_offset), 2); *acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + dot_u_offset), 2);
} }
} else { } else {
vec3 base_dot_omega; vec3 base_dot_omega;
@@ -196,14 +196,14 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i), printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i),
i, dot_u[i], base_dot_omega(i) - dot_u[i]); i, dot_u[i], base_dot_omega(i) - dot_u[i]);
} }
*acc_error += std::pow(base_dot_omega(i) - dot_u(i), 2); *acc_error += BT_ID_POW(base_dot_omega(i) - dot_u(i), 2);
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (verbose) { if (verbose) {
printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i), printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i),
i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]); i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]);
} }
*acc_error += std::pow(base_ddot_com(i) - dot_u(i + 3), 2); *acc_error += BT_ID_POW(base_ddot_com(i) - dot_u(i + 3), 2);
} }
for (int i = 0; i < btmb->getNumDofs(); i++) { for (int i = 0; i < btmb->getNumDofs(); i++) {
@@ -211,7 +211,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + 6), joint_accel[i] - dot_u(i + 6)); dot_u(i + 6), joint_accel[i] - dot_u(i + 6));
} }
*acc_error += std::pow(joint_accel[i] - dot_u(i + 6), 2); *acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + 6), 2);
} }
} }
*acc_error = std::sqrt(*acc_error); *acc_error = std::sqrt(*acc_error);
@@ -288,12 +288,12 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2)); diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2));
} }
double total_pos_err = double total_pos_err =
std::sqrt(std::pow(diff_com(0), 2) + std::pow(diff_com(1), 2) + BT_ID_SQRT(BT_ID_POW(diff_com(0), 2) + BT_ID_POW(diff_com(1), 2) +
std::pow(diff_com(2), 2) + std::pow(diff_basis(0, 0), 2) + BT_ID_POW(diff_com(2), 2) + BT_ID_POW(diff_basis(0, 0), 2) +
std::pow(diff_basis(0, 1), 2) + std::pow(diff_basis(0, 2), 2) + BT_ID_POW(diff_basis(0, 1), 2) + BT_ID_POW(diff_basis(0, 2), 2) +
std::pow(diff_basis(1, 0), 2) + std::pow(diff_basis(1, 1), 2) + BT_ID_POW(diff_basis(1, 0), 2) + BT_ID_POW(diff_basis(1, 1), 2) +
std::pow(diff_basis(1, 2), 2) + std::pow(diff_basis(2, 0), 2) + BT_ID_POW(diff_basis(1, 2), 2) + BT_ID_POW(diff_basis(2, 0), 2) +
std::pow(diff_basis(2, 1), 2) + std::pow(diff_basis(2, 2), 2)); BT_ID_POW(diff_basis(2, 1), 2) + BT_ID_POW(diff_basis(2, 2), 2));
if (verbose) { if (verbose) {
printf("======kin-pos-err: %e\n", total_pos_err); printf("======kin-pos-err: %e\n", total_pos_err);
} }

14
src/BulletInverseDynamics/IDConfig.hpp
View File

@@ -14,12 +14,22 @@
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
#include <cmath> #include <cmath>
#define BT_ID_WO_BULLET #define BT_ID_WO_BULLET
#define BT_ID_POW(a,b) std::pow(a,b) #define BT_ID_SQRT(x) std::sqrt(x)
#define BT_ID_FABS(x) std::fabs(x)
#define BT_ID_COS(x) std::cos(x)
#define BT_ID_SIN(x) std::sin(x)
#define BT_ID_ATAN2(x, y) std::atan2(x, y)
#define BT_ID_POW(x, y) std::pow(x, y)
#define BT_ID_SNPRINTF snprintf #define BT_ID_SNPRINTF snprintf
#define BT_ID_PI M_PI #define BT_ID_PI M_PI
#define BT_ID_USE_DOUBLE_PRECISION #define BT_ID_USE_DOUBLE_PRECISION
#else #else
#define BT_ID_POW(a,b) btPow(a,b) #define BT_ID_SQRT(x) btSqrt(x)
#define BT_ID_FABS(x) btFabs(x)
#define BT_ID_COS(x) btCos(x)
#define BT_ID_SIN(x) btSin(x)
#define BT_ID_ATAN2(x, y) btAtan2(x, y)
#define BT_ID_POW(x, y) btPow(x, y)
#define BT_ID_PI SIMD_PI #define BT_ID_PI SIMD_PI
#ifdef _WIN32 #ifdef _WIN32
#define BT_ID_SNPRINTF _snprintf #define BT_ID_SNPRINTF _snprintf

50
src/BulletInverseDynamics/IDMath.cpp
View File

@@ -36,7 +36,7 @@ void setZero(mat33 &m) {
idScalar maxAbs(const vecx &v) { idScalar maxAbs(const vecx &v) {
idScalar result = 0.0; idScalar result = 0.0;
for (int i = 0; i < v.size(); i++) { for (int i = 0; i < v.size(); i++) {
const idScalar tmp = std::fabs(v(i)); const idScalar tmp = BT_ID_FABS(v(i));
if (tmp > result) { if (tmp > result) {
result = tmp; result = tmp;
} }
@@ -47,7 +47,7 @@ idScalar maxAbs(const vecx &v) {
idScalar maxAbs(const vec3 &v) { idScalar maxAbs(const vec3 &v) {
idScalar result = 0.0; idScalar result = 0.0;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
const idScalar tmp = std::fabs(v(i)); const idScalar tmp = BT_ID_FABS(v(i));
if (tmp > result) { if (tmp > result) {
result = tmp; result = tmp;
} }
@@ -111,8 +111,8 @@ void sub(const mat3x &a, const mat3x &b, mat3x *result) {
mat33 transformX(const idScalar &alpha) { mat33 transformX(const idScalar &alpha) {
mat33 T; mat33 T;
const idScalar cos_alpha = std::cos(alpha); const idScalar cos_alpha = BT_ID_COS(alpha);
const idScalar sin_alpha = std::sin(alpha); const idScalar sin_alpha = BT_ID_SIN(alpha);
// [1 0 0] // [1 0 0]
// [0 c s] // [0 c s]
// [0 -s c] // [0 -s c]
@@ -133,8 +133,8 @@ mat33 transformX(const idScalar &alpha) {
mat33 transformY(const idScalar &beta) { mat33 transformY(const idScalar &beta) {
mat33 T; mat33 T;
const idScalar cos_beta = std::cos(beta); const idScalar cos_beta = BT_ID_COS(beta);
const idScalar sin_beta = std::sin(beta); const idScalar sin_beta = BT_ID_SIN(beta);
// [c 0 -s] // [c 0 -s]
// [0 1 0] // [0 1 0]
// [s 0 c] // [s 0 c]
@@ -155,8 +155,8 @@ mat33 transformY(const idScalar &beta) {
mat33 transformZ(const idScalar &gamma) { mat33 transformZ(const idScalar &gamma) {
mat33 T; mat33 T;
const idScalar cos_gamma = std::cos(gamma); const idScalar cos_gamma = BT_ID_COS(gamma);
const idScalar sin_gamma = std::sin(gamma); const idScalar sin_gamma = BT_ID_SIN(gamma);
// [ c s 0] // [ c s 0]
// [-s c 0] // [-s c 0]
// [ 0 0 1] // [ 0 0 1]
@@ -190,10 +190,10 @@ mat33 tildeOperator(const vec3 &v) {
} }
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) {
const idScalar sa = std::sin(alpha); const idScalar sa = BT_ID_SIN(alpha);
const idScalar ca = std::cos(alpha); const idScalar ca = BT_ID_COS(alpha);
const idScalar st = std::sin(theta); const idScalar st = BT_ID_SIN(theta);
const idScalar ct = std::cos(theta); const idScalar ct = BT_ID_COS(theta);
(*r)(0) = a; (*r)(0) = a;
(*r)(1) = -sa * d; (*r)(1) = -sa * d;
@@ -213,8 +213,8 @@ void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec
} }
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
const idScalar c = cos(angle); const idScalar c = BT_ID_COS(angle);
const idScalar s = -sin(angle); const idScalar s = -BT_ID_SIN(angle);
const idScalar one_m_c = 1.0 - c; const idScalar one_m_c = 1.0 - c;
const idScalar &x = axis(0); const idScalar &x = axis(0);
@@ -347,19 +347,19 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
} }
} }
// check symmetry // check symmetry
if (std::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)= " 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 (std::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)= " 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 (std::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, 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;
@@ -375,7 +375,7 @@ bool isValidTransformMatrix(const mat33 &m) {
// check for unit length column vectors // check for unit length column vectors
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
const idScalar length_minus_1 = const idScalar length_minus_1 =
std::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" error_message("Not a valid rotation matrix (column %d not unit length)\n"
"column = [%.18e %.18e %.18e]\n" "column = [%.18e %.18e %.18e]\n"
@@ -386,17 +386,17 @@ bool isValidTransformMatrix(const mat33 &m) {
} }
} }
// check for orthogonal column vectors // check for orthogonal column vectors
if (std::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"); 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 (std::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"); 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 (std::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"); error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m); print_mat(m);
return false; return false;
@@ -411,15 +411,15 @@ bool isValidTransformMatrix(const mat33 &m) {
} }
bool isUnitVector(const vec3 &vector) { bool isUnitVector(const vec3 &vector) {
return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
kIsZero; kIsZero;
} }
vec3 rpyFromMatrix(const mat33 &rot) { vec3 rpyFromMatrix(const mat33 &rot) {
vec3 rpy; vec3 rpy;
rpy(2) = std::atan2(-rot(1, 0), rot(0, 0)); rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0)); rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
rpy(0) = std::atan2(-rot(2, 0), rot(2, 2)); rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
return rpy; return rpy;
} }
} }

8
src/BulletInverseDynamics/MultiBodyTree.cpp
View File

@@ -226,10 +226,10 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
warning_message( 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 = std::sqrt(std::pow(body_axis_of_motion(0), 2) + idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
std::pow(body_axis_of_motion(1), 2) + BT_ID_POW(body_axis_of_motion(1), 2) +
std::pow(body_axis_of_motion(2), 2)); BT_ID_POW(body_axis_of_motion(2), 2));
if (length < std::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); error_message("axis of motion vector too short (%e)\n", length);
return -1; return -1;
} }

2
test/InverseDynamics/test_invdyn_kinematics.cpp
View File

@@ -24,7 +24,7 @@ idScalar calculateNorm(T&);
// only implemented for vec3 // only implemented for vec3
template <> template <>
idScalar calculateNorm(vec3& v) { idScalar calculateNorm(vec3& v) {
return std::sqrt(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2)); return BT_ID_SQRT(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
} }
// template function to convert a DiffType (finite differences) // template function to convert a DiffType (finite differences)