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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/BulletInverseDynamics/IDConfig.hpp
View File

@@ -32,10 +32,10 @@
#define BT_ID_POW(x, y) btPow(x, y)
#define BT_ID_PI SIMD_PI
#ifdef _WIN32
#define BT_ID_SNPRINTF _snprintf
#define BT_ID_SNPRINTF _snprintf
#else
#define BT_ID_SNPRINTF snprintf
#endif //
#define BT_ID_SNPRINTF snprintf
#endif //
#endif
// error messages
#include "IDErrorMessages.hpp"
@@ -52,8 +52,8 @@
#error "custom inverse dynamics config, but no custom namespace defined"
#endif
#define BT_ID_MAX(a,b) std::max(a,b)
#define BT_ID_MIN(a,b) std::min(a,b)
#define BT_ID_MAX(a, b) std::max(a, b)
#define BT_ID_MIN(a, b) std::min(a, b)
#else
#define btInverseDynamics btInverseDynamicsBullet3
@@ -62,8 +62,8 @@
#include "LinearMath/btScalar.h"
typedef btScalar idScalar;
#include "LinearMath/btMinMax.h"
#define BT_ID_MAX(a,b) btMax(a,b)
#define BT_ID_MIN(a,b) btMin(a,b)
#define BT_ID_MAX(a, b) btMax(a, b)
#define BT_ID_MIN(a, b) btMin(a, b)
#ifdef BT_USE_DOUBLE_PRECISION
#define BT_ID_USE_DOUBLE_PRECISION
@@ -71,31 +71,31 @@ typedef btScalar idScalar;
#ifndef BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
// use bullet types for arrays and array indices
#include "Bullet3Common/b3AlignedObjectArray.h"
// this is to make it work with C++2003, otherwise we could do this:
// template <typename T>
// using idArray = b3AlignedObjectArray<T>;
template <typename T>
struct idArray {
struct idArray
{
typedef b3AlignedObjectArray<T> type;
};
typedef int idArrayIdx;
#define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR()
#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
#include "LinearMath/btAlignedObjectArray.h"
template <typename T>
struct idArray {
struct idArray
{
typedef btAlignedObjectArray<T> type;
};
typedef int idArrayIdx;
#define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR()
#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
// use bullet's allocator functions
#define idMalloc btAllocFunc

19
src/BulletInverseDynamics/IDConfigBuiltin.hpp
View File

@@ -14,7 +14,8 @@ typedef float idScalar;
// template <typename T>
// using idArray = std::vector<T>;
template <typename T>
struct idArray {
struct idArray
{
typedef std::vector<T> type;
};
typedef std::vector<int>::size_type idArrayIdx;
@@ -23,14 +24,14 @@ typedef std::vector<int>::size_type idArrayIdx;
#define idMalloc ::malloc
#define idFree ::free
// currently not aligned at all...
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
inline void operator delete(void* ptr) { idFree(ptr); } \
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
inline void operator delete(void*, void*) {} \
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
inline void operator delete[](void* ptr) { idFree(ptr); } \
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
inline void operator delete(void* ptr) { idFree(ptr); } \
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
inline void operator delete(void*, void*) {} \
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
inline void operator delete[](void* ptr) { idFree(ptr); } \
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
inline void operator delete[](void*, void*) {}
#include "details/IDMatVec.hpp"

3
src/BulletInverseDynamics/IDConfigEigen.hpp
View File

@@ -15,7 +15,8 @@ typedef float idScalar;
// template <typename T>
// using idArray = std::vector<T>;
template <typename T>
struct idArray {
struct idArray
{
typedef std::vector<T> type;
};
typedef std::vector<int>::size_type idArrayIdx;

18
src/BulletInverseDynamics/IDErrorMessages.hpp
View File

@@ -13,16 +13,18 @@
#else // BT_ID_WO_BULLET
#include <cstdio>
/// print error message with file/line information
#define bt_id_error_message(...) \
do { \
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
#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 bt_id_warning_message(...) \
do { \
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
#define bt_id_warning_message(...) \
do \
{ \
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
} while (0)
#define id_printf(...) printf(__VA_ARGS__)
#endif // BT_ID_WO_BULLET

349
src/BulletInverseDynamics/IDMath.cpp
View File

@@ -3,25 +3,30 @@
#include <cmath>
#include <limits>
namespace btInverseDynamics {
namespace btInverseDynamics
{
static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
// requirements for axis length deviation from 1.0
// experimentally set from random euler angle rotation matrices
static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
void setZero(vec3 &v) {
void setZero(vec3 &v)
{
v(0) = 0;
v(1) = 0;
v(2) = 0;
}
void setZero(vecx &v) {
for (int i = 0; i < v.size(); i++) {
void setZero(vecx &v)
{
for (int i = 0; i < v.size(); i++)
{
v(i) = 0;
}
}
void setZero(mat33 &m) {
void setZero(mat33 &m)
{
m(0, 0) = 0;
m(0, 1) = 0;
m(0, 2) = 0;
@@ -33,7 +38,8 @@ void setZero(mat33 &m) {
m(2, 2) = 0;
}
void skew(vec3& v, mat33* result) {
void skew(vec3 &v, mat33 *result)
{
(*result)(0, 0) = 0.0;
(*result)(0, 1) = -v(2);
(*result)(0, 2) = v(1);
@@ -45,22 +51,28 @@ void skew(vec3& v, mat33* result) {
(*result)(2, 2) = 0.0;
}
idScalar maxAbs(const vecx &v) {
idScalar maxAbs(const vecx &v)
{
idScalar result = 0.0;
for (int i = 0; i < v.size(); i++) {
for (int i = 0; i < v.size(); i++)
{
const idScalar tmp = BT_ID_FABS(v(i));
if (tmp > result) {
if (tmp > result)
{
result = tmp;
}
}
return result;
}
idScalar maxAbs(const vec3 &v) {
idScalar maxAbs(const vec3 &v)
{
idScalar result = 0.0;
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
const idScalar tmp = BT_ID_FABS(v(i));
if (tmp > result) {
if (tmp > result)
{
result = tmp;
}
}
@@ -68,60 +80,75 @@ idScalar maxAbs(const vec3 &v) {
}
#if (defined BT_ID_HAVE_MAT3X)
idScalar maxAbsMat3x(const mat3x &m) {
// only used for tests -- so just loop here for portability
idScalar result = 0.0;
for (idArrayIdx col = 0; col < m.cols(); col++) {
for (idArrayIdx row = 0; row < 3; row++) {
result = BT_ID_MAX(result, std::fabs(m(row, col)));
}
}
return result;
idScalar maxAbsMat3x(const mat3x &m)
{
// only used for tests -- so just loop here for portability
idScalar result = 0.0;
for (idArrayIdx col = 0; col < m.cols(); col++)
{
for (idArrayIdx row = 0; row < 3; row++)
{
result = BT_ID_MAX(result, std::fabs(m(row, col)));
}
}
return result;
}
void mul(const mat33 &a, const mat3x &b, mat3x *result) {
if (b.cols() != result->cols()) {
bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}
void mul(const mat33 &a, const mat3x &b, mat3x *result)
{
if (b.cols() != result->cols())
{
bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++) {
const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col);
const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col);
const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col);
setMat3xElem(0, col, x, result);
setMat3xElem(1, col, y, result);
setMat3xElem(2, col, z, result);
}
for (idArrayIdx col = 0; col < b.cols(); col++)
{
const idScalar x = a(0, 0) * b(0, col) + a(0, 1) * b(1, col) + a(0, 2) * b(2, col);
const idScalar y = a(1, 0) * b(0, col) + a(1, 1) * b(1, col) + a(1, 2) * b(2, col);
const idScalar z = a(2, 0) * b(0, col) + a(2, 1) * b(1, col) + a(2, 2) * b(2, col);
setMat3xElem(0, col, x, result);
setMat3xElem(1, col, y, result);
setMat3xElem(2, col, z, result);
}
}
void add(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++) {
for (idArrayIdx row = 0; row < 3; row++) {
setMat3xElem(row, col, a(row, col) + b(row, col), result);
}
}
void add(const mat3x &a, const mat3x &b, mat3x *result)
{
if (a.cols() != b.cols())
{
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++)
{
for (idArrayIdx row = 0; row < 3; row++)
{
setMat3xElem(row, col, a(row, col) + b(row, col), result);
}
}
}
void sub(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++) {
for (idArrayIdx row = 0; row < 3; row++) {
setMat3xElem(row, col, a(row, col) - b(row, col), result);
}
}
void sub(const mat3x &a, const mat3x &b, mat3x *result)
{
if (a.cols() != b.cols())
{
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++)
{
for (idArrayIdx row = 0; row < 3; row++)
{
setMat3xElem(row, col, a(row, col) - b(row, col), result);
}
}
}
#endif
mat33 transformX(const idScalar &alpha) {
mat33 transformX(const idScalar &alpha)
{
mat33 T;
const idScalar cos_alpha = BT_ID_COS(alpha);
const idScalar sin_alpha = BT_ID_SIN(alpha);
@@ -143,7 +170,8 @@ mat33 transformX(const idScalar &alpha) {
return T;
}
mat33 transformY(const idScalar &beta) {
mat33 transformY(const idScalar &beta)
{
mat33 T;
const idScalar cos_beta = BT_ID_COS(beta);
const idScalar sin_beta = BT_ID_SIN(beta);
@@ -165,7 +193,8 @@ mat33 transformY(const idScalar &beta) {
return T;
}
mat33 transformZ(const idScalar &gamma) {
mat33 transformZ(const idScalar &gamma)
{
mat33 T;
const idScalar cos_gamma = BT_ID_COS(gamma);
const idScalar sin_gamma = BT_ID_SIN(gamma);
@@ -187,7 +216,8 @@ mat33 transformZ(const idScalar &gamma) {
return T;
}
mat33 tildeOperator(const vec3 &v) {
mat33 tildeOperator(const vec3 &v)
{
mat33 m;
m(0, 0) = 0.0;
m(0, 1) = -v(2);
@@ -201,7 +231,8 @@ mat33 tildeOperator(const vec3 &v) {
return m;
}
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 = BT_ID_SIN(alpha);
const idScalar ca = BT_ID_COS(alpha);
const idScalar st = BT_ID_SIN(theta);
@@ -224,7 +255,8 @@ void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec
(*T)(2, 2) = ca;
}
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
{
const idScalar c = BT_ID_COS(angle);
const idScalar s = -BT_ID_SIN(angle);
const idScalar one_m_c = 1.0 - c;
@@ -246,175 +278,214 @@ void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
(*T)(2, 2) = z * z * one_m_c + c;
}
bool isPositiveDefinite(const mat33 &m) {
bool isPositiveDefinite(const mat33 &m)
{
// test if all upper left determinants are positive
if (m(0, 0) <= 0) { // upper 1x1
if (m(0, 0) <= 0)
{ // upper 1x1
return false;
}
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0)
{ // upper 2x2
return false;
}
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
{
return false;
}
return true;
}
bool isPositiveSemiDefinite(const mat33 &m) {
bool isPositiveSemiDefinite(const mat33 &m)
{
// test if all upper left determinants are positive
if (m(0, 0) < 0) { // upper 1x1
if (m(0, 0) < 0)
{ // upper 1x1
return false;
}
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0)
{ // upper 2x2
return false;
}
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
{
return false;
}
return true;
}
bool isPositiveSemiDefiniteFuzzy(const mat33 &m) {
bool isPositiveSemiDefiniteFuzzy(const mat33 &m)
{
// test if all upper left determinants are positive
if (m(0, 0) < -kIsZero) { // upper 1x1
if (m(0, 0) < -kIsZero)
{ // upper 1x1
return false;
}
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero)
{ // upper 2x2
return false;
}
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) {
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero)
{
return false;
}
return true;
}
idScalar determinant(const mat33 &m) {
idScalar determinant(const mat33 &m)
{
return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) -
m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2);
}
bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) {
bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
{
// TODO(Thomas) do we really want this?
// in cases where the inertia tensor about the center of mass is zero,
// the determinant of the inertia tensor about the joint axis is almost
// zero and can have a very small negative value.
if (!isPositiveSemiDefiniteFuzzy(I)) {
bt_id_error_message("invalid inertia matrix for body %d, not positive definite "
"(fixed joint)\n",
index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
if (!isPositiveSemiDefiniteFuzzy(I))
{
bt_id_error_message(
"invalid inertia matrix for body %d, not positive definite "
"(fixed joint)\n",
index);
bt_id_error_message(
"matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
return false;
}
// 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)) {
if (!has_fixed_joint)
{
if (I(0, 0) + I(1, 1) < I(2, 2))
{
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",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
bt_id_error_message(
"matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
return false;
}
if (I(0, 0) + I(1, 1) < I(2, 2)) {
if (I(0, 0) + I(1, 1) < I(2, 2))
{
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",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
bt_id_error_message(
"matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
return false;
}
if (I(1, 1) + I(2, 2) < I(0, 0)) {
if (I(1, 1) + I(2, 2) < I(0, 0))
{
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",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
bt_id_error_message(
"matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
I(2, 2));
return false;
}
}
// check positive/zero diagonal elements
for (int i = 0; i < 3; i++) {
if (I(i, i) < 0) { // accept zero
for (int i = 0; i < 3; i++)
{
if (I(i, i) < 0)
{ // accept zero
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) {
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));
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero)
{
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) {
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));
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero)
{
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) {
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero)
{
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 true;
}
bool isValidTransformMatrix(const mat33 &m) {
#define print_mat(x) \
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))
bool isValidTransformMatrix(const mat33 &m)
{
#define print_mat(x) \
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
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
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) {
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);
if (length_minus_1 > kAxisLengthEpsilon)
{
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);
print_mat(m);
return false;
}
}
// 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)
{
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) {
if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon)
{
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) {
if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon)
{
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) {
if (determinant(m) <= 0)
{
bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
print_mat(m);
return false;
@@ -422,16 +493,18 @@ bool isValidTransformMatrix(const mat33 &m) {
return true;
}
bool isUnitVector(const vec3 &vector) {
bool isUnitVector(const vec3 &vector)
{
return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
kIsZero;
}
vec3 rpyFromMatrix(const mat33 &rot) {
vec3 rpyFromMatrix(const mat33 &rot)
{
vec3 rpy;
rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 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) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
return rpy;
}
}
} // namespace btInverseDynamics

15
src/BulletInverseDynamics/IDMath.hpp
View File

@@ -5,7 +5,8 @@
#define IDMATH_HPP_
#include "IDConfig.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// set all elements to zero
void setZero(vec3& v);
/// set all elements to zero
@@ -23,11 +24,11 @@ idScalar maxAbs(const vec3& v);
#if (defined BT_ID_HAVE_MAT3X)
idScalar maxAbsMat3x(const mat3x& m);
void setZero(mat3x&m);
void setZero(mat3x& m);
// define math functions on mat3x here to avoid allocations in operators.
void mul(const mat33&a, const mat3x&b, mat3x* result);
void add(const mat3x&a, const mat3x&b, mat3x* result);
void sub(const mat3x&a, const mat3x&b, mat3x* result);
void mul(const mat33& a, const mat3x& b, mat3x* result);
void add(const mat3x& a, const mat3x& b, mat3x* result);
void sub(const mat3x& a, const mat3x& b, mat3x* result);
#endif
/// get offset vector & transform matrix from DH parameters
@@ -94,6 +95,6 @@ mat33 transformZ(const idScalar& gamma);
///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix
/// @param rot rotation matrix
/// @returns x-y-z Euler angles
vec3 rpyFromMatrix(const mat33&rot);
}
vec3 rpyFromMatrix(const mat33& rot);
} // namespace btInverseDynamics
#endif // IDMATH_HPP_

333
src/BulletInverseDynamics/MultiBodyTree.cpp
View File

@@ -8,69 +8,84 @@
#include "details/MultiBodyTreeImpl.hpp"
#include "details/MultiBodyTreeInitCache.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
MultiBodyTree::MultiBodyTree()
: m_is_finalized(false),
m_mass_parameters_are_valid(true),
m_accept_invalid_mass_parameters(false),
m_impl(0x0),
m_init_cache(0x0) {
m_init_cache(0x0)
{
m_init_cache = new InitCache();
}
MultiBodyTree::~MultiBodyTree() {
MultiBodyTree::~MultiBodyTree()
{
delete m_impl;
delete m_init_cache;
}
void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
{
m_accept_invalid_mass_parameters = flag;
}
bool MultiBodyTree::getAcceptInvalidMassProperties() const {
bool MultiBodyTree::getAcceptInvalidMassProperties() const
{
return m_accept_invalid_mass_parameters;
}
int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const {
int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
{
return m_impl->getBodyOrigin(body_index, world_origin);
}
int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const {
int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
{
return m_impl->getBodyCoM(body_index, world_com);
}
int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const {
int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
{
return m_impl->getBodyTransform(body_index, world_T_body);
}
int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const {
int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
{
return m_impl->getBodyAngularVelocity(body_index, world_omega);
}
int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const {
int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
{
return m_impl->getBodyLinearVelocity(body_index, world_velocity);
}
int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const {
int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
{
return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
}
int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const {
int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
{
return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
}
int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const {
int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
{
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
}
int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const {
return m_impl->getParentRParentBodyRef(body_index, r);
int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
{
return m_impl->getParentRParentBodyRef(body_index, r);
}
int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const {
return m_impl->getBodyTParentRef(body_index, T);
int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
{
return m_impl->getBodyTParentRef(body_index, T);
}
int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const {
return m_impl->getBodyAxisOfMotion(body_index, axis);
int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
{
return m_impl->getBodyAxisOfMotion(body_index, axis);
}
void MultiBodyTree::printTree() { m_impl->printTree(); }
@@ -81,12 +96,15 @@ int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
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) {
vecx *joint_forces)
{
if (false == m_is_finalized)
{
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces))
{
bt_id_error_message("error in inverse dynamics calculation\n");
return -1;
}
@@ -95,141 +113,164 @@ int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const
int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
const bool initialize_matrix,
const bool set_lower_triangular_matrix, matxx *mass_matrix) {
if (false == m_is_finalized) {
const bool set_lower_triangular_matrix, matxx *mass_matrix)
{
if (false == m_is_finalized)
{
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)) {
set_lower_triangular_matrix, mass_matrix))
{
bt_id_error_message("error in mass matrix calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
{
return calculateMassMatrix(q, true, true, true, mass_matrix);
}
int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u)
{
vec3 world_gravity(m_impl->m_world_gravity);
// temporarily set gravity to zero, to ensure we get the actual accelerations
setZero(m_impl->m_world_gravity);
if (false == m_is_finalized)
{
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))
{
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) {
vec3 world_gravity(m_impl->m_world_gravity);
// temporarily set gravity to zero, to ensure we get the actual accelerations
setZero(m_impl->m_world_gravity);
if (false == m_is_finalized) {
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)) {
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
m_impl->m_world_gravity=world_gravity;
return 0;
m_impl->m_world_gravity = world_gravity;
return 0;
}
int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
if (false == m_is_finalized) {
int MultiBodyTree::calculatePositionKinematics(const vecx &q)
{
if (false == m_is_finalized)
{
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, q, q,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
{
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
if (false == m_is_finalized) {
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u)
{
if (false == m_is_finalized)
{
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, u, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
{
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
return 0;
}
#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) {
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
bt_id_error_message("error in jacobian calculation\n");
return -1;
}
return 0;
int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u)
{
if (false == m_is_finalized)
{
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
{
bt_id_error_message("error in jacobian calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::calculateJacobians(const vecx& q){
if (false == m_is_finalized) {
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, q,
MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
bt_id_error_message("error in jacobian calculation\n");
return -1;
}
return 0;
int MultiBodyTree::calculateJacobians(const vecx &q)
{
if (false == m_is_finalized)
{
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, q,
MultiBodyTree::MultiBodyImpl::POSITION_ONLY))
{
bt_id_error_message("error in jacobian calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u);
int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
{
return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u);
}
int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const {
return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u);
int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
{
return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u);
}
int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const {
return m_impl->getBodyJacobianTrans(body_index,world_jac_trans);
int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
{
return m_impl->getBodyJacobianTrans(body_index, world_jac_trans);
}
int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const {
return m_impl->getBodyJacobianRot(body_index,world_jac_rot);
int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
{
return m_impl->getBodyJacobianRot(body_index, world_jac_rot);
}
#endif
int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
const vec3 &body_axis_of_motion_, idScalar mass,
const vec3 &body_r_body_com, const mat33 &body_I_body,
const int user_int, void *user_ptr) {
if (body_index < 0) {
const int user_int, void *user_ptr)
{
if (body_index < 0)
{
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_);
switch (joint_type) {
switch (joint_type)
{
case REVOLUTE:
case PRISMATIC:
// check if axis is unit vector
if (!isUnitVector(body_axis_of_motion)) {
if (!isUnitVector(body_axis_of_motion))
{
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())) {
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()))
{
bt_id_error_message("axis of motion vector too short (%e)\n", length);
return -1;
}
@@ -246,23 +287,28 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
}
// sanity check for mass properties. Zero mass is OK.
if (mass < 0) {
if (mass < 0)
{
m_mass_parameters_are_valid = false;
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;
}
}
if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) {
if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type))
{
m_mass_parameters_are_valid = false;
// error message printed in function call
if (!m_accept_invalid_mass_parameters) {
if (!m_accept_invalid_mass_parameters)
{
return -1;
}
}
if (!isValidTransformMatrix(body_T_parent_ref)) {
if (!isValidTransformMatrix(body_T_parent_ref))
{
return -1;
}
@@ -271,52 +317,63 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
body_I_body, user_int, user_ptr);
}
int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const {
int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
{
return m_impl->getParentIndex(body_index, parent_index);
}
int MultiBodyTree::getUserInt(const int body_index, int *user_int) const {
int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
{
return m_impl->getUserInt(body_index, user_int);
}
int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const {
int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
{
return m_impl->getUserPtr(body_index, user_ptr);
}
int MultiBodyTree::setUserInt(const int body_index, const int user_int) {
int MultiBodyTree::setUserInt(const int body_index, const int user_int)
{
return m_impl->setUserInt(body_index, user_int);
}
int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) {
int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
{
return m_impl->setUserPtr(body_index, user_ptr);
}
int MultiBodyTree::finalize() {
int MultiBodyTree::finalize()
{
const int &num_bodies = m_init_cache->numBodies();
const int &num_dofs = m_init_cache->numDoFs();
if(num_dofs<=0) {
bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
//return -1;
}
if (num_dofs <= 0)
{
bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
//return -1;
}
// 1 allocate internal MultiBody structure
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
// 2 build new index set assuring index(parent) < index(child)
if (-1 == m_init_cache->buildIndexSets()) {
if (-1 == m_init_cache->buildIndexSets())
{
return -1;
}
m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
// 3 setup internal kinematic and dynamic data
for (int index = 0; index < num_bodies; index++) {
for (int index = 0; index < num_bodies; index++)
{
InertiaData inertia;
JointData joint;
if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
if (-1 == m_init_cache->getInertiaData(index, &inertia))
{
return -1;
}
if (-1 == m_init_cache->getJointData(index, &joint)) {
if (-1 == m_init_cache->getJointData(index, &joint))
{
return -1;
}
@@ -332,24 +389,29 @@ int MultiBodyTree::finalize() {
rigid_body.m_joint_type = joint.m_type;
int user_int;
if (-1 == m_init_cache->getUserInt(index, &user_int)) {
if (-1 == m_init_cache->getUserInt(index, &user_int))
{
return -1;
}
if (-1 == m_impl->setUserInt(index, user_int)) {
if (-1 == m_impl->setUserInt(index, user_int))
{
return -1;
}
void* user_ptr;
if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) {
void *user_ptr;
if (-1 == m_init_cache->getUserPtr(index, &user_ptr))
{
return -1;
}
if (-1 == m_impl->setUserPtr(index, user_ptr)) {
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) {
switch (rigid_body.m_joint_type)
{
case REVOLUTE:
rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
@@ -392,7 +454,8 @@ int MultiBodyTree::finalize() {
}
// 4 assign degree of freedom indices & build per-joint-type index arrays
if (-1 == m_impl->generateIndexSets()) {
if (-1 == m_impl->generateIndexSets())
{
bt_id_error_message("generating index sets\n");
return -1;
}
@@ -408,54 +471,66 @@ int MultiBodyTree::finalize() {
return 0;
}
int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
{
return m_impl->setGravityInWorldFrame(gravity);
}
int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const {
int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
{
return m_impl->getJointType(body_index, joint_type);
}
int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const {
int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
{
return m_impl->getJointTypeStr(body_index, joint_type);
}
int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const {
int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
{
return m_impl->getDoFOffset(body_index, q_offset);
}
int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
{
return m_impl->setBodyMass(body_index, mass);
}
int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) {
int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment)
{
return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
}
int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) {
int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment)
{
return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
}
int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const {
int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
{
return m_impl->getBodyMass(body_index, mass);
}
int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const {
int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const
{
return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
}
int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const {
int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const
{
return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
}
void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) {
int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
{
return m_impl->addUserForce(body_index, body_force);
}
int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) {
int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment)
{
return m_impl->addUserMoment(body_index, body_moment);
}
}
} // namespace btInverseDynamics

86
src/BulletInverseDynamics/MultiBodyTree.hpp
View File

@@ -4,10 +4,11 @@
#include "IDConfig.hpp"
#include "IDMath.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Enumeration of supported joint types
enum JointType {
enum JointType
{
/// no degree of freedom, moves with parent
FIXED = 0,
/// one rotational degree of freedom relative to parent
@@ -49,9 +50,10 @@ enum JointType {
///
/// TODO - force element interface (friction, springs, dampers, etc)
/// - gears and motor inertia
class MultiBodyTree {
class MultiBodyTree
{
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
ID_DECLARE_ALIGNED_ALLOCATOR();
/// The contructor.
/// Initialization & allocation is via addBody and buildSystem calls.
MultiBodyTree();
@@ -119,9 +121,9 @@ public:
/// print tree data to stdout
void printTreeData();
/// Calculate joint forces for given generalized state & derivatives.
/// This also updates kinematic terms computed in calculateKinematics.
/// If gravity is not set to zero, acceleration terms will contain
/// gravitational acceleration.
/// This also updates kinematic terms computed in calculateKinematics.
/// If gravity is not set to zero, acceleration terms will contain
/// gravitational acceleration.
/// @param q generalized coordinates
/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
/// @param dot_u time derivative of u
@@ -152,30 +154,28 @@ public:
/// @return -1 on error, 0 on success
int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
/// Calculates kinematics also calculated in calculateInverseDynamics,
/// but not dynamics.
/// This function ensures that correct accelerations are computed that do not
/// contain gravitational acceleration terms.
/// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
/// Calculate position kinematics
int calculatePositionKinematics(const vecx& q);
/// Calculate position and velocity kinematics
int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
/// Calculates kinematics also calculated in calculateInverseDynamics,
/// but not dynamics.
/// This function ensures that correct accelerations are computed that do not
/// contain gravitational acceleration terms.
/// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
/// Calculate position kinematics
int calculatePositionKinematics(const vecx& q);
/// Calculate position and velocity kinematics
int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
/// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
/// d(Jacobian)/dt*u
/// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
/// or calculatePositionAndVelocityKinematics
int calculateJacobians(const vecx& q, const vecx& u);
/// Calculate Jacobians (dvel/du)
/// This function assumes that calculateInverseDynamics was called, or
/// one of the calculateKineamtics functions
int calculateJacobians(const vecx& q);
#endif // BT_ID_HAVE_MAT3X
/// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
/// d(Jacobian)/dt*u
/// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
/// or calculatePositionAndVelocityKinematics
int calculateJacobians(const vecx& q, const vecx& u);
/// Calculate Jacobians (dvel/du)
/// This function assumes that calculateInverseDynamics was called, or
/// one of the calculateKineamtics functions
int calculateJacobians(const vecx& q);
#endif // BT_ID_HAVE_MAT3X
/// set gravitational acceleration
/// the default is [0;0;-9.8] in the world frame
@@ -231,15 +231,15 @@ public:
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
// get translational jacobian, in world frame (dworld_velocity/du)
int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
// get rotational jacobian, in world frame (dworld_omega/du)
int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
// get product of translational jacobian derivative * generatlized velocities
int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
// get product of rotational jacobian derivative * generatlized velocities
int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
#endif // BT_ID_HAVE_MAT3X
// get translational jacobian, in world frame (dworld_velocity/du)
int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
// get rotational jacobian, in world frame (dworld_omega/du)
int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
// get product of translational jacobian derivative * generatlized velocities
int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
// get product of rotational jacobian derivative * generatlized velocities
int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
#endif // BT_ID_HAVE_MAT3X
/// returns the (internal) index of body
/// @param body_index is the index of a body
@@ -256,21 +256,21 @@ public:
/// @param joint_type string naming the corresponding joint type
/// @return 0 on success, -1 on failure
int getJointTypeStr(const int body_index, const char** joint_type) const;
/// get offset translation to parent body (see addBody)
/// get offset translation to parent body (see addBody)
/// @param body_index index of the body
/// @param r the offset translation (see above)
/// @return 0 on success, -1 on failure
int getParentRParentBodyRef(const int body_index, vec3* r) const;
int getParentRParentBodyRef(const int body_index, vec3* r) const;
/// get offset rotation to parent body (see addBody)
/// @param body_index index of the body
/// @param T the transform (see above)
/// @return 0 on success, -1 on failure
int getBodyTParentRef(const int body_index, mat33* T) const;
int getBodyTParentRef(const int body_index, mat33* T) const;
/// get axis of motion (see addBody)
/// @param body_index index of the body
/// @param axis the axis (see above)
/// @return 0 on success, -1 on failure
int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// get offset for degrees of freedom of this body into the q-vector
/// @param body_index index of the body
/// @param q_offset offset the q vector

23
src/BulletInverseDynamics/details/IDEigenInterface.hpp
View File

@@ -1,8 +1,8 @@
#ifndef INVDYNEIGENINTERFACE_HPP_
#define INVDYNEIGENINTERFACE_HPP_
#include "../IDConfig.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
#define BT_ID_HAVE_MAT3X
#ifdef BT_USE_DOUBLE_PRECISION
@@ -19,18 +19,21 @@ typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> m
typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
#endif
inline void resize(mat3x &m, Eigen::Index size) {
m.resize(3, size);
m.setZero();
inline void resize(mat3x &m, Eigen::Index size)
{
m.resize(3, size);
m.setZero();
}
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
(*m)(row, col) = val;
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx *m)
{
(*m)(row, col) = val;
}
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
(*m)(row, col) = val;
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x *m)
{
(*m)(row, col) = val;
}
}
} // namespace btInverseDynamics
#endif // INVDYNEIGENINTERFACE_HPP_

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

@@ -10,32 +10,37 @@
#include "../../LinearMath/btMatrixX.h"
#define BT_ID_HAVE_MAT3X
namespace btInverseDynamics {
namespace btInverseDynamics
{
class vec3;
class vecx;
class mat33;
typedef btMatrixX<idScalar> matxx;
class vec3 : public btVector3 {
class vec3 : public btVector3
{
public:
vec3() : btVector3() {}
vec3(const btVector3& btv) { *this = btv; }
idScalar& operator()(int i) { return (*this)[i]; }
const idScalar& operator()(int i) const { return (*this)[i]; }
int size() const { return 3; }
const vec3& operator=(const btVector3& rhs) {
const vec3& operator=(const btVector3& rhs)
{
*static_cast<btVector3*>(this) = rhs;
return *this;
}
};
class mat33 : public btMatrix3x3 {
class mat33 : public btMatrix3x3
{
public:
mat33() : btMatrix3x3() {}
mat33(const btMatrix3x3& btm) { *this = btm; }
idScalar& operator()(int i, int j) { return (*this)[i][j]; }
const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
const mat33& operator=(const btMatrix3x3& rhs) {
const mat33& operator=(const btMatrix3x3& rhs)
{
*static_cast<btMatrix3x3*>(this) = rhs;
return *this;
}
@@ -47,10 +52,12 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s)
inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
class vecx : public btVectorX<idScalar> {
class vecx : public btVectorX<idScalar>
{
public:
vecx(int size) : btVectorX<idScalar>(size) {}
const vecx& operator=(const btVectorX<idScalar>& rhs) {
const vecx& operator=(const btVectorX<idScalar>& rhs)
{
*static_cast<btVectorX<idScalar>*>(this) = rhs;
return *this;
}
@@ -66,43 +73,53 @@ public:
friend vecx operator/(const vecx& a, const idScalar& s);
};
inline vecx operator*(const vecx& a, const idScalar& s) {
inline vecx operator*(const vecx& a, const idScalar& s)
{
vecx result(a.size());
for (int i = 0; i < result.size(); i++) {
for (int i = 0; i < result.size(); i++)
{
result(i) = a(i) * s;
}
return result;
}
inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
inline vecx operator+(const vecx& a, const vecx& b) {
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()) {
if (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++) {
for (int i = 0; i < a.size(); i++)
{
result(i) = a(i) + b(i);
}
return result;
}
inline vecx operator-(const vecx& a, const vecx& b) {
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()) {
if (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++) {
for (int i = 0; i < a.size(); i++)
{
result(i) = a(i) - b(i);
}
return result;
}
inline vecx operator/(const vecx& a, const idScalar& s) {
inline vecx operator/(const vecx& a, const idScalar& s)
{
vecx result(a.size());
for (int i = 0; i < result.size(); i++) {
for (int i = 0; i < result.size(); i++)
{
result(i) = a(i) / s;
}
@@ -110,63 +127,76 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
}
// use btMatrixX to implement 3xX matrix
class mat3x : public matxx {
class mat3x : public matxx
{
public:
mat3x(){}
mat3x(const mat3x&rhs) {
matxx::resize(rhs.rows(), rhs.cols());
*this = rhs;
}
mat3x(int rows, int cols): matxx(3,cols) {
}
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
mat3x() {}
mat3x(const mat3x& rhs)
{
matxx::resize(rhs.rows(), rhs.cols());
*this = rhs;
}
mat3x(int rows, int cols) : matxx(3, cols)
{
}
void operator=(const mat3x& rhs)
{
if (m_cols != rhs.m_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++)
{
for (int k = 0; k < cols(); k++)
{
setElem(i, k, rhs(i, k));
}
}
}
void setZero()
{
matxx::setZero();
}
for(int i=0;i<rows();i++) {
for(int k=0;k<cols();k++) {
setElem(i,k,rhs(i,k));
}
}
}
void setZero() {
matxx::setZero();
}
};
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (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;
result(1)=0.0;
result(2)=0.0;
for(int i=0;i<b.size();i++) {
for(int k=0;k<3;k++) {
result(k)+=a(k,i)*b(i);
}
}
return result;
inline vec3 operator*(const mat3x& a, const vecx& b)
{
vec3 result;
if (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;
result(1) = 0.0;
result(2) = 0.0;
for (int i = 0; i < b.size(); i++)
{
for (int k = 0; k < 3; k++)
{
result(k) += a(k, i) * b(i);
}
}
return result;
}
inline void resize(mat3x &m, idArrayIdx size) {
m.resize(3, size);
m.setZero();
inline void resize(mat3x& m, idArrayIdx size)
{
m.resize(3, size);
m.setZero();
}
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
m->setElem(row, col, val);
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
{
m->setElem(row, col, val);
}
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
m->setElem(row, col, val);
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
{
m->setElem(row, col, val);
}
}
} // namespace btInverseDynamics
#endif // IDLINEARMATHINTERFACE_HPP_

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

@@ -7,7 +7,8 @@
#include "../IDConfig.hpp"
#define BT_ID_HAVE_MAT3X
namespace btInverseDynamics {
namespace btInverseDynamics
{
class vec3;
class vecx;
class mat33;
@@ -17,7 +18,8 @@ class mat3x;
/// This is a very basic implementation to enable stand-alone use of the library.
/// The implementation is not really optimized and misses many features that you would
/// want from a "fully featured" linear math library.
class vec3 {
class vec3
{
public:
idScalar& operator()(int i) { return m_data[i]; }
const idScalar& operator()(int i) const { return m_data[i]; }
@@ -40,7 +42,8 @@ private:
idScalar m_data[3];
};
class mat33 {
class mat33
{
public:
idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
@@ -62,9 +65,11 @@ private:
idScalar m_data[9];
};
class vecx {
class vecx
{
public:
vecx(int size) : m_size(size) {
vecx(int size) : m_size(size)
{
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
}
~vecx() { idFree(m_data); }
@@ -85,14 +90,17 @@ private:
idScalar* m_data;
};
class matxx {
class matxx
{
public:
matxx() {
m_data = 0x0;
m_cols=0;
m_rows=0;
}
matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
matxx()
{
m_data = 0x0;
m_cols = 0;
m_rows = 0;
}
matxx(int rows, int cols) : m_rows(rows), m_cols(cols)
{
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
}
~matxx() { idFree(m_data); }
@@ -107,69 +115,86 @@ private:
idScalar* m_data;
};
class mat3x {
class mat3x
{
public:
mat3x() {
m_data = 0x0;
m_cols=0;
}
mat3x(const mat3x&rhs) {
m_cols=rhs.m_cols;
allocate();
*this = rhs;
}
mat3x(int rows, int cols): m_cols(cols) {
allocate();
};
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
mat3x()
{
m_data = 0x0;
m_cols = 0;
}
mat3x(const mat3x& rhs)
{
m_cols = rhs.m_cols;
allocate();
*this = rhs;
}
mat3x(int rows, int cols) : m_cols(cols)
{
allocate();
};
void operator=(const mat3x& rhs)
{
if (m_cols != rhs.m_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++)
{
m_data[i] = rhs.m_data[i];
}
}
for(int i=0;i<3*m_cols;i++) {
m_data[i] = rhs.m_data[i];
}
}
~mat3x() {
free();
}
idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
int rows() const { return m_rows; }
const int& cols() const { return m_cols; }
void resize(int rows, int cols) {
m_cols=cols;
free();
allocate();
}
void setZero() {
memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols);
}
// avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
~mat3x()
{
free();
}
idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
int rows() const { return m_rows; }
const int& cols() const { return m_cols; }
void resize(int rows, int cols)
{
m_cols = cols;
free();
allocate();
}
void setZero()
{
memset(m_data, 0x0, sizeof(idScalar) * m_rows * m_cols);
}
// avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
private:
void allocate(){m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));}
void free() { idFree(m_data);}
enum {m_rows=3};
int m_cols;
idScalar* m_data;
void allocate() { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols)); }
void free() { idFree(m_data); }
enum
{
m_rows = 3
};
int m_cols;
idScalar* m_data;
};
inline void resize(mat3x &m, idArrayIdx size) {
m.resize(3, size);
m.setZero();
inline void resize(mat3x& m, idArrayIdx size)
{
m.resize(3, size);
m.setZero();
}
//////////////////////////////////////////////////
// Implementations
inline const vec3& vec3::operator=(const vec3& rhs) {
if (&rhs != this) {
inline const vec3& vec3::operator=(const vec3& rhs)
{
if (&rhs != this)
{
memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
}
return *this;
}
inline vec3 vec3::cross(const vec3& b) const {
inline vec3 vec3::cross(const vec3& b) const
{
vec3 result;
result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2];
@@ -178,17 +203,21 @@ inline vec3 vec3::cross(const vec3& b) const {
return result;
}
inline idScalar vec3::dot(const vec3& b) const {
inline idScalar vec3::dot(const vec3& b) const
{
return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
}
inline const mat33& mat33::operator=(const mat33& rhs) {
if (&rhs != this) {
inline const mat33& mat33::operator=(const mat33& rhs)
{
if (&rhs != this)
{
memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
}
return *this;
}
inline mat33 mat33::transpose() const {
inline mat33 mat33::transpose() const
{
mat33 result;
result.m_data[0] = m_data[0];
result.m_data[1] = m_data[3];
@@ -203,7 +232,8 @@ inline mat33 mat33::transpose() const {
return result;
}
inline mat33 operator*(const mat33& a, const mat33& b) {
inline mat33 operator*(const mat33& a, const mat33& b)
{
mat33 result;
result.m_data[0] =
a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6];
@@ -227,22 +257,27 @@ inline mat33 operator*(const mat33& a, const mat33& b) {
return result;
}
inline const mat33& mat33::operator+=(const mat33& b) {
for (int i = 0; i < 9; i++) {
inline const mat33& mat33::operator+=(const mat33& b)
{
for (int i = 0; i < 9; i++)
{
m_data[i] += b.m_data[i];
}
return *this;
}
inline const mat33& mat33::operator-=(const mat33& b) {
for (int i = 0; i < 9; i++) {
inline const mat33& mat33::operator-=(const mat33& b)
{
for (int i = 0; i < 9; i++)
{
m_data[i] -= b.m_data[i];
}
return *this;
}
inline vec3 operator*(const mat33& a, const vec3& b) {
inline vec3 operator*(const mat33& a, const vec3& b)
{
vec3 result;
result.m_data[0] =
@@ -255,23 +290,29 @@ inline vec3 operator*(const mat33& a, const vec3& b) {
return result;
}
inline const vec3& vec3::operator+=(const vec3& b) {
for (int i = 0; i < 3; i++) {
inline const vec3& vec3::operator+=(const vec3& b)
{
for (int i = 0; i < 3; i++)
{
m_data[i] += b.m_data[i];
}
return *this;
}
inline const vec3& vec3::operator-=(const vec3& b) {
for (int i = 0; i < 3; i++) {
inline const vec3& vec3::operator-=(const vec3& b)
{
for (int i = 0; i < 3; i++)
{
m_data[i] -= b.m_data[i];
}
return *this;
}
inline mat33 operator*(const mat33& a, const idScalar& s) {
inline mat33 operator*(const mat33& a, const idScalar& s)
{
mat33 result;
for (int i = 0; i < 9; i++) {
for (int i = 0; i < 9; i++)
{
result.m_data[i] = a.m_data[i] * s;
}
return result;
@@ -279,137 +320,170 @@ inline mat33 operator*(const mat33& a, const idScalar& s) {
inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
inline vec3 operator*(const vec3& a, const idScalar& s) {
inline vec3 operator*(const vec3& a, const idScalar& s)
{
vec3 result;
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
result.m_data[i] = a.m_data[i] * s;
}
return result;
}
inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
inline mat33 operator+(const mat33& a, const mat33& b) {
inline mat33 operator+(const mat33& a, const mat33& b)
{
mat33 result;
for (int i = 0; i < 9; i++) {
for (int i = 0; i < 9; i++)
{
result.m_data[i] = a.m_data[i] + b.m_data[i];
}
return result;
}
inline vec3 operator+(const vec3& a, const vec3& b) {
inline vec3 operator+(const vec3& a, const vec3& b)
{
vec3 result;
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
result.m_data[i] = a.m_data[i] + b.m_data[i];
}
return result;
}
inline mat33 operator-(const mat33& a, const mat33& b) {
inline mat33 operator-(const mat33& a, const mat33& b)
{
mat33 result;
for (int i = 0; i < 9; i++) {
for (int i = 0; i < 9; i++)
{
result.m_data[i] = a.m_data[i] - b.m_data[i];
}
return result;
}
inline vec3 operator-(const vec3& a, const vec3& b) {
inline vec3 operator-(const vec3& a, const vec3& b)
{
vec3 result;
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
result.m_data[i] = a.m_data[i] - b.m_data[i];
}
return result;
}
inline mat33 operator/(const mat33& a, const idScalar& s) {
inline mat33 operator/(const mat33& a, const idScalar& s)
{
mat33 result;
for (int i = 0; i < 9; i++) {
for (int i = 0; i < 9; i++)
{
result.m_data[i] = a.m_data[i] / s;
}
return result;
}
inline vec3 operator/(const vec3& a, const idScalar& s) {
inline vec3 operator/(const vec3& a, const idScalar& s)
{
vec3 result;
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
result.m_data[i] = a.m_data[i] / s;
}
return result;
}
inline const vecx& vecx::operator=(const vecx& rhs) {
if (size() != rhs.size()) {
inline const vecx& vecx::operator=(const vecx& rhs)
{
if (size() != rhs.size())
{
bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
abort();
}
if (&rhs != this) {
if (&rhs != this)
{
memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
}
return *this;
}
inline vecx operator*(const vecx& a, const idScalar& s) {
inline vecx operator*(const vecx& a, const idScalar& s)
{
vecx result(a.size());
for (int i = 0; i < result.size(); i++) {
for (int i = 0; i < result.size(); i++)
{
result.m_data[i] = a.m_data[i] * s;
}
return result;
}
inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
inline vecx operator+(const vecx& a, const vecx& b) {
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()) {
if (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++) {
for (int i = 0; i < a.size(); i++)
{
result.m_data[i] = a.m_data[i] + b.m_data[i];
}
return result;
}
inline vecx operator-(const vecx& a, const vecx& b) {
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()) {
if (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++) {
for (int i = 0; i < a.size(); i++)
{
result.m_data[i] = a.m_data[i] - b.m_data[i];
}
return result;
}
inline vecx operator/(const vecx& a, const idScalar& s) {
inline vecx operator/(const vecx& a, const idScalar& s)
{
vecx result(a.size());
for (int i = 0; i < result.size(); i++) {
for (int i = 0; i < result.size(); i++)
{
result.m_data[i] = a.m_data[i] / s;
}
return result;
}
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (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;
result(1)=0.0;
result(2)=0.0;
for(int i=0;i<b.size();i++) {
for(int k=0;k<3;k++) {
result(k)+=a(k,i)*b(i);
}
}
return result;
inline vec3 operator*(const mat3x& a, const vecx& b)
{
vec3 result;
if (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;
result(1) = 0.0;
result(2) = 0.0;
for (int i = 0; i < b.size(); i++)
{
for (int k = 0; k < 3; k++)
{
result(k) += a(k, i) * b(i);
}
}
return result;
}
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
(*m)(row, col) = val;
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
{
(*m)(row, col) = val;
}
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
(*m)(row, col) = val;
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
{
(*m)(row, col) = val;
}
} // namespace btInverseDynamcis
} // namespace btInverseDynamics
#endif

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

File diff suppressed because it is too large Load Diff

91
src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
View File

@@ -8,12 +8,13 @@
#include "../IDConfig.hpp"
#include "../MultiBodyTree.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Structure for for rigid body mass properties, connectivity and kinematic state
/// all vectors and matrices are in body-fixed frame, if not indicated otherwise.
/// The body-fixed frame is located in the joint connecting the body to its parent.
struct RigidBody {
struct RigidBody
{
ID_DECLARE_ALIGNED_ALLOCATOR();
// 1 Inertial properties
/// Mass
@@ -112,31 +113,33 @@ struct RigidBody {
mat33 m_body_subtree_I_body;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
/// translational jacobian in body-fixed frame d(m_body_vel)/du
mat3x m_body_Jac_T;
/// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
mat3x m_body_Jac_R;
/// components of linear acceleration depending on u
/// (same as is d(m_Jac_T)/dt*u)
vec3 m_body_dot_Jac_T_u;
/// components of angular acceleration depending on u
/// (same as is d(m_Jac_T)/dt*u)
vec3 m_body_dot_Jac_R_u;
/// translational jacobian in body-fixed frame d(m_body_vel)/du
mat3x m_body_Jac_T;
/// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
mat3x m_body_Jac_R;
/// components of linear acceleration depending on u
/// (same as is d(m_Jac_T)/dt*u)
vec3 m_body_dot_Jac_T_u;
/// components of angular acceleration depending on u
/// (same as is d(m_Jac_T)/dt*u)
vec3 m_body_dot_Jac_R_u;
#endif
};
/// The MBS implements a tree structured multibody system
class MultiBodyTree::MultiBodyImpl {
class MultiBodyTree::MultiBodyImpl
{
friend class MultiBodyTree;
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
enum KinUpdateType {
POSITION_ONLY,
POSITION_VELOCITY,
POSITION_VELOCITY_ACCELERATION
};
enum KinUpdateType
{
POSITION_ONLY,
POSITION_VELOCITY,
POSITION_VELOCITY_ACCELERATION
};
/// constructor
/// @param num_bodies the number of bodies in the system
@@ -150,24 +153,24 @@ public:
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
const bool initialize_matrix, const bool set_lower_triangular_matrix,
matxx* mass_matrix);
/// calculate kinematics (vector quantities)
/// Depending on type, update positions only, positions & velocities, or positions, velocities
/// and accelerations.
int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
/// calculate kinematics (vector quantities)
/// Depending on type, update positions only, positions & velocities, or positions, velocities
/// and accelerations.
int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
/// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
/// \copydoc MultiBodyTree::getBodyDotJacobianTransU
int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ;
/// \copydoc MultiBodyTree::getBodyDotJacobianRotU
int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
/// \copydoc MultiBodyTree::getBodyJacobianTrans
int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ;
/// \copydoc MultiBodyTree::getBodyJacobianRot
int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
/// Add relative Jacobian component from motion relative to parent body
/// @param body the body to add the Jacobian component for
void addRelativeJacobianComponent(RigidBody&body);
/// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
/// \copydoc MultiBodyTree::getBodyDotJacobianTransU
int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
/// \copydoc MultiBodyTree::getBodyDotJacobianRotU
int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
/// \copydoc MultiBodyTree::getBodyJacobianTrans
int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
/// \copydoc MultiBodyTree::getBodyJacobianRot
int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
/// Add relative Jacobian component from motion relative to parent body
/// @param body the body to add the Jacobian component for
void addRelativeJacobianComponent(RigidBody& body);
#endif
/// generate additional index sets from the parent_index array
/// @return -1 on error, 0 on success
@@ -190,12 +193,12 @@ public:
int getJointType(const int body_index, JointType* joint_type) const;
/// \copydoc MultiBodyTree::getJointTypeStr
int getJointTypeStr(const int body_index, const char** joint_type) const;
/// \copydoc MultiBodyTree::getParentRParentBodyRef
int getParentRParentBodyRef(const int body_index, vec3* r) const;
/// \copydoc MultiBodyTree::getBodyTParentRef
int getBodyTParentRef(const int body_index, mat33* T) const;
/// \copydoc MultiBodyTree::getBodyAxisOfMotion
int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// \copydoc MultiBodyTree::getParentRParentBodyRef
int getParentRParentBodyRef(const int body_index, vec3* r) const;
/// \copydoc MultiBodyTree::getBodyTParentRef
int getBodyTParentRef(const int body_index, mat33* T) const;
/// \copydoc MultiBodyTree::getBodyAxisOfMotion
int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// \copydoc MultiBodyTree:getDoFOffset
int getDoFOffset(const int body_index, int* q_index) const;
/// \copydoc MultiBodyTree::getBodyOrigin
@@ -276,8 +279,8 @@ private:
// a user-provided pointer
idArray<void*>::type m_user_ptr;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
mat3x m_m3x;
mat3x m_m3x;
#endif
};
}
} // namespace btInverseDynamics
#endif

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

@@ -1,12 +1,13 @@
#include "MultiBodyTreeInitCache.hpp"
namespace btInverseDynamics {
MultiBodyTree::InitCache::InitCache() {
namespace btInverseDynamics
{
MultiBodyTree::InitCache::InitCache()
{
m_inertias.resize(0);
m_joints.resize(0);
m_num_dofs = 0;
m_root_index=-1;
m_root_index = -1;
}
int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
@@ -15,8 +16,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
const mat33& body_T_parent_ref,
const vec3& body_axis_of_motion, const idScalar mass,
const vec3& body_r_body_com, const mat33& body_I_body,
const int user_int, void* user_ptr) {
switch (joint_type) {
const int user_int, void* user_ptr)
{
switch (joint_type)
{
case REVOLUTE:
case PRISMATIC:
m_num_dofs += 1;
@@ -33,13 +36,15 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
return -1;
}
if(-1 == parent_index) {
if(m_root_index>=0) {
if (-1 == parent_index)
{
if (m_root_index >= 0)
{
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;
}
m_root_index=body_index;
m_root_index = body_index;
}
JointData joint;
@@ -61,8 +66,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
m_user_ptr.push_back(user_ptr);
return 0;
}
int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
if (index < 0 || index > static_cast<int>(m_inertias.size())) {
int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const
{
if (index < 0 || index > static_cast<int>(m_inertias.size()))
{
bt_id_error_message("index out of range\n");
return -1;
}
@@ -71,8 +78,10 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert
return 0;
}
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
if (index < 0 || index > static_cast<int>(m_user_int.size())) {
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const
{
if (index < 0 || index > static_cast<int>(m_user_int.size()))
{
bt_id_error_message("index out of range\n");
return -1;
}
@@ -80,8 +89,10 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
return 0;
}
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
{
if (index < 0 || index > static_cast<int>(m_user_ptr.size()))
{
bt_id_error_message("index out of range\n");
return -1;
}
@@ -89,8 +100,10 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
return 0;
}
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
if (index < 0 || index > static_cast<int>(m_joints.size())) {
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const
{
if (index < 0 || index > static_cast<int>(m_joints.size()))
{
bt_id_error_message("index out of range\n");
return -1;
}
@@ -98,16 +111,18 @@ int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) co
return 0;
}
int MultiBodyTree::InitCache::buildIndexSets() {
int MultiBodyTree::InitCache::buildIndexSets()
{
// NOTE: This function assumes that proper indices were provided
// User2InternalIndex from utils can be used to facilitate this.
m_parent_index.resize(numBodies());
for (idArrayIdx j = 0; j < m_joints.size(); j++) {
for (idArrayIdx j = 0; j < m_joints.size(); j++)
{
const JointData& joint = m_joints[j];
m_parent_index[joint.m_child] = joint.m_parent;
}
return 0;
}
}
} // namespace btInverseDynamics

14
src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
View File

@@ -5,9 +5,11 @@
#include "../IDMath.hpp"
#include "../MultiBodyTree.hpp"
namespace btInverseDynamics {
namespace btInverseDynamics
{
/// Mass properties of a rigid body
struct InertiaData {
struct InertiaData
{
ID_DECLARE_ALIGNED_ALLOCATOR();
/// mass
@@ -21,7 +23,8 @@ struct InertiaData {
};
/// Joint properties
struct JointData {
struct JointData
{
ID_DECLARE_ALIGNED_ALLOCATOR();
/// type of joint
@@ -48,7 +51,8 @@ struct JointData {
/// Data structure to store data passed by the user.
/// This is used in MultiBodyTree::finalize to build internal data structures.
class MultiBodyTree::InitCache {
class MultiBodyTree::InitCache
{
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
/// constructor
@@ -105,5 +109,5 @@ private:
// index of root body (or -1 if not set)
int m_root_index;
};
}
} // namespace btInverseDynamics
#endif // MULTIBODYTREEINITCACHE_HPP_