From 75d657ec85e754d4330e13604ee8e860f2b933d3 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 19 Nov 2015 12:08:04 -0800 Subject: [PATCH] change 4 spaces to tab for src/BulletInverseDynamics fix unit test in single precision compilation (use delta t of 0.01 if BT_ID_USE_DOUBLE_PRECISION is not defined) --- src/BulletInverseDynamics/IDConfig.hpp | 8 +- src/BulletInverseDynamics/IDConfigBuiltin.hpp | 20 +- src/BulletInverseDynamics/IDConfigEigen.hpp | 20 +- src/BulletInverseDynamics/IDErrorMessages.hpp | 30 +- src/BulletInverseDynamics/IDMath.cpp | 566 +++---- src/BulletInverseDynamics/IDMath.hpp | 4 +- src/BulletInverseDynamics/MultiBodyTree.cpp | 396 ++--- src/BulletInverseDynamics/MultiBodyTree.hpp | 542 +++---- .../details/IDLinearMathInterface.hpp | 122 +- .../details/IDMatVec.hpp | 408 ++--- .../details/MultiBodyTreeImpl.cpp | 1332 ++++++++--------- .../details/MultiBodyTreeImpl.hpp | 406 ++--- .../details/MultiBodyTreeInitCache.cpp | 164 +- .../details/MultiBodyTreeInitCache.hpp | 164 +- .../test_invdyn_kinematics.cpp | 4 + 15 files changed, 2097 insertions(+), 2089 deletions(-) diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index e7d6c122f..97646dc43 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -1,5 +1,5 @@ ///@file Configuration for Inverse Dynamics Library, -/// such as choice of linear algebra library and underlying scalar type +/// such as choice of linear algebra library and underlying scalar type #ifndef IDCONFIG_HPP_ #define IDCONFIG_HPP_ // If we have a custom configuration, compile without using other parts of bullet. @@ -8,6 +8,7 @@ #define BT_ID_POW(a,b) std::pow(a,b) #define BT_ID_SNPRINTF snprintf #define BT_ID_PI M_PI +#define BT_ID_USE_DOUBLE_PRECISION #else #define BT_ID_POW(a,b) btPow(a,b) #define BT_ID_PI SIMD_PI @@ -33,6 +34,9 @@ // Use the same scalar type as rest of bullet library #include "LinearMath/btScalar.h" typedef btScalar idScalar; +#ifdef BT_USE_DOUBLE_PRECISION +#define BT_ID_USE_DOUBLE_PRECISION +#endif // 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: @@ -40,7 +44,7 @@ typedef btScalar idScalar; // using idArray = b3AlignedObjectArray; template struct idArray { - typedef b3AlignedObjectArray type; + typedef b3AlignedObjectArray type; }; typedef int idArrayIdx; #define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR diff --git a/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/src/BulletInverseDynamics/IDConfigBuiltin.hpp index 3965bc5eb..130c19c6d 100644 --- a/src/BulletInverseDynamics/IDConfigBuiltin.hpp +++ b/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -15,7 +15,7 @@ typedef float idScalar; // using idArray = std::vector; template struct idArray { - typedef std::vector type; + typedef std::vector type; }; typedef std::vector::size_type idArrayIdx; // default to standard malloc/free @@ -23,15 +23,15 @@ typedef std::vector::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; } \ - inline void operator delete[](void*, void*) {} +#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" #endif diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp index f0b72b425..a83ee8e3f 100644 --- a/src/BulletInverseDynamics/IDConfigEigen.hpp +++ b/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -16,7 +16,7 @@ typedef float idScalar; // using idArray = std::vector; template struct idArray { - typedef std::vector type; + typedef std::vector type; }; typedef std::vector::size_type idArrayIdx; // default to standard malloc/free @@ -25,15 +25,15 @@ typedef std::vector::size_type idArrayIdx; #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; } \ - inline void operator delete[](void*, void*) {} +#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*) {} // Note on interfaces: // Eigen::Matrix has data(), to get c-array storage diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp index a66ea8323..a3866edc5 100644 --- a/src/BulletInverseDynamics/IDErrorMessages.hpp +++ b/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -13,22 +13,22 @@ #else // BT_ID_WO_BULLET #include /// print error message with file/line information -#define error_message(...) \ - do { \ - fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) +#define error_message(...) \ + do { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) /// print warning message with file/line information -#define warning_message(...) \ - do { \ - fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) -#define warning_message(...) \ - do { \ - fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define 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 #endif diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index a3daaf1f5..7ce055fdf 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -10,362 +10,362 @@ static const idScalar kIsZero = 5 * std::numeric_limits::epsilon(); static const idScalar kAxisLengthEpsilon = 10 * kIsZero; void setZero(vec3 &v) { - v(0) = 0; - v(1) = 0; - v(2) = 0; + v(0) = 0; + v(1) = 0; + v(2) = 0; } void setZero(vecx &v) { - for (int i = 0; i < v.size(); i++) { - v(i) = 0; - } + for (int i = 0; i < v.size(); i++) { + v(i) = 0; + } } void setZero(mat33 &m) { - m(0, 0) = 0; - m(0, 1) = 0; - m(0, 2) = 0; - m(1, 0) = 0; - m(1, 1) = 0; - m(1, 2) = 0; - m(2, 0) = 0; - m(2, 1) = 0; - m(2, 2) = 0; + m(0, 0) = 0; + m(0, 1) = 0; + m(0, 2) = 0; + m(1, 0) = 0; + m(1, 1) = 0; + m(1, 2) = 0; + m(2, 0) = 0; + m(2, 1) = 0; + m(2, 2) = 0; } idScalar maxAbs(const vecx &v) { - idScalar result = 0.0; - for (int i = 0; i < v.size(); i++) { - const idScalar tmp = std::fabs(v(i)); - if (tmp > result) { - result = tmp; - } - } - return result; + idScalar result = 0.0; + for (int i = 0; i < v.size(); i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; } idScalar maxAbs(const vec3 &v) { - idScalar result = 0.0; - for (int i = 0; i < 3; i++) { - const idScalar tmp = std::fabs(v(i)); - if (tmp > result) { - result = tmp; - } - } - return result; + idScalar result = 0.0; + for (int i = 0; i < 3; i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; } mat33 transformX(const idScalar &alpha) { - mat33 T; - const idScalar cos_alpha = std::cos(alpha); - const idScalar sin_alpha = std::sin(alpha); - // [1 0 0] - // [0 c s] - // [0 -s c] - T(0, 0) = 1.0; - T(0, 1) = 0.0; - T(0, 2) = 0.0; + mat33 T; + const idScalar cos_alpha = std::cos(alpha); + const idScalar sin_alpha = std::sin(alpha); + // [1 0 0] + // [0 c s] + // [0 -s c] + T(0, 0) = 1.0; + T(0, 1) = 0.0; + T(0, 2) = 0.0; - T(1, 0) = 0.0; - T(1, 1) = cos_alpha; - T(1, 2) = sin_alpha; + T(1, 0) = 0.0; + T(1, 1) = cos_alpha; + T(1, 2) = sin_alpha; - T(2, 0) = 0.0; - T(2, 1) = -sin_alpha; - T(2, 2) = cos_alpha; + T(2, 0) = 0.0; + T(2, 1) = -sin_alpha; + T(2, 2) = cos_alpha; - return T; + return T; } mat33 transformY(const idScalar &beta) { - mat33 T; - const idScalar cos_beta = std::cos(beta); - const idScalar sin_beta = std::sin(beta); - // [c 0 -s] - // [0 1 0] - // [s 0 c] - T(0, 0) = cos_beta; - T(0, 1) = 0.0; - T(0, 2) = -sin_beta; + mat33 T; + const idScalar cos_beta = std::cos(beta); + const idScalar sin_beta = std::sin(beta); + // [c 0 -s] + // [0 1 0] + // [s 0 c] + T(0, 0) = cos_beta; + T(0, 1) = 0.0; + T(0, 2) = -sin_beta; - T(1, 0) = 0.0; - T(1, 1) = 1.0; - T(1, 2) = 0.0; + T(1, 0) = 0.0; + T(1, 1) = 1.0; + T(1, 2) = 0.0; - T(2, 0) = sin_beta; - T(2, 1) = 0.0; - T(2, 2) = cos_beta; + T(2, 0) = sin_beta; + T(2, 1) = 0.0; + T(2, 2) = cos_beta; - return T; + return T; } mat33 transformZ(const idScalar &gamma) { - mat33 T; - const idScalar cos_gamma = std::cos(gamma); - const idScalar sin_gamma = std::sin(gamma); - // [ c s 0] - // [-s c 0] - // [ 0 0 1] - T(0, 0) = cos_gamma; - T(0, 1) = sin_gamma; - T(0, 2) = 0.0; + mat33 T; + const idScalar cos_gamma = std::cos(gamma); + const idScalar sin_gamma = std::sin(gamma); + // [ c s 0] + // [-s c 0] + // [ 0 0 1] + T(0, 0) = cos_gamma; + T(0, 1) = sin_gamma; + T(0, 2) = 0.0; - T(1, 0) = -sin_gamma; - T(1, 1) = cos_gamma; - T(1, 2) = 0.0; + T(1, 0) = -sin_gamma; + T(1, 1) = cos_gamma; + T(1, 2) = 0.0; - T(2, 0) = 0.0; - T(2, 1) = 0.0; - T(2, 2) = 1.0; + T(2, 0) = 0.0; + T(2, 1) = 0.0; + T(2, 2) = 1.0; - return T; + return T; } mat33 tildeOperator(const vec3 &v) { - mat33 m; - m(0, 0) = 0.0; - m(0, 1) = -v(2); - m(0, 2) = v(1); - m(1, 0) = v(2); - m(1, 1) = 0.0; - m(1, 2) = -v(0); - m(2, 0) = -v(1); - m(2, 1) = v(0); - m(2, 2) = 0.0; - return m; + mat33 m; + m(0, 0) = 0.0; + m(0, 1) = -v(2); + m(0, 2) = v(1); + m(1, 0) = v(2); + m(1, 1) = 0.0; + m(1, 2) = -v(0); + m(2, 0) = -v(1); + m(2, 1) = v(0); + m(2, 2) = 0.0; + return m; } void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { - const idScalar sa = std::sin(alpha); - const idScalar ca = std::cos(alpha); - const idScalar st = std::sin(theta); - const idScalar ct = std::cos(theta); + const idScalar sa = std::sin(alpha); + const idScalar ca = std::cos(alpha); + const idScalar st = std::sin(theta); + const idScalar ct = std::cos(theta); - (*r)(0) = a; - (*r)(1) = -sa * d; - (*r)(2) = ca * d; + (*r)(0) = a; + (*r)(1) = -sa * d; + (*r)(2) = ca * d; - (*T)(0, 0) = ct; - (*T)(0, 1) = -st; - (*T)(0, 2) = 0.0; + (*T)(0, 0) = ct; + (*T)(0, 1) = -st; + (*T)(0, 2) = 0.0; - (*T)(1, 0) = st * ca; - (*T)(1, 1) = ct * ca; - (*T)(1, 2) = -sa; + (*T)(1, 0) = st * ca; + (*T)(1, 1) = ct * ca; + (*T)(1, 2) = -sa; - (*T)(2, 0) = st * sa; - (*T)(2, 1) = ct * sa; - (*T)(2, 2) = ca; + (*T)(2, 0) = st * sa; + (*T)(2, 1) = ct * sa; + (*T)(2, 2) = ca; } void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { - const idScalar c = cos(angle); - const idScalar s = -sin(angle); - const idScalar one_m_c = 1.0 - c; + const idScalar c = cos(angle); + const idScalar s = -sin(angle); + const idScalar one_m_c = 1.0 - c; - const idScalar &x = axis(0); - const idScalar &y = axis(1); - const idScalar &z = axis(2); + const idScalar &x = axis(0); + const idScalar &y = axis(1); + const idScalar &z = axis(2); - (*T)(0, 0) = x * x * one_m_c + c; - (*T)(0, 1) = x * y * one_m_c - z * s; - (*T)(0, 2) = x * z * one_m_c + y * s; + (*T)(0, 0) = x * x * one_m_c + c; + (*T)(0, 1) = x * y * one_m_c - z * s; + (*T)(0, 2) = x * z * one_m_c + y * s; - (*T)(1, 0) = x * y * one_m_c + z * s; - (*T)(1, 1) = y * y * one_m_c + c; - (*T)(1, 2) = y * z * one_m_c - x * s; + (*T)(1, 0) = x * y * one_m_c + z * s; + (*T)(1, 1) = y * y * one_m_c + c; + (*T)(1, 2) = y * z * one_m_c - x * s; - (*T)(2, 0) = x * z * one_m_c - y * s; - (*T)(2, 1) = y * z * one_m_c + x * s; - (*T)(2, 2) = z * z * one_m_c + c; + (*T)(2, 0) = x * z * one_m_c - y * s; + (*T)(2, 1) = y * z * one_m_c + x * s; + (*T)(2, 2) = z * z * one_m_c + c; } bool isPositiveDefinite(const mat33 &m) { - // test if all upper left determinants are positive - 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 - 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) { - return false; - } - return true; + // test if all upper left determinants are positive + 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 + 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) { + return false; + } + return true; } bool isPositiveSemiDefinite(const mat33 &m) { - // test if all upper left determinants are positive - 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 - 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) { - return false; - } - return true; + // test if all upper left determinants are positive + 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 + 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) { + return false; + } + return true; } bool isPositiveSemiDefiniteFuzzy(const mat33 &m) { - // test if all upper left determinants are positive - 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 - 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) { - return false; - } - return true; + // test if all upper left determinants are positive + 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 + 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) { + return false; + } + return true; } 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); + 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) { - // 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)) { - error_message("invalid inertia matrix for body %d, not positive definite " - "(fixed joint)\n", - index); - 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)); + // 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)) { + error_message("invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + 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; - } + 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)) { - error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); - 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)) { - error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); - 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)) { - error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); - 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 - error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); - return false; - } - } - // check symmetry - if (std::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)= " - "%e\n", - index, I(1, 0) - I(0, 1)); - return false; - } - if (std::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)= " - "%e\n", - index, I(2, 0) - I(0, 2)); - return false; - } - if (std::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, - I(1, 2) - I(2, 1)); - return false; - } - return true; + // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) + if (!has_fixed_joint) { + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.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)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + 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)) { + error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + 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 + error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); + return false; + } + } + // check symmetry + if (std::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)= " + "%e\n", + index, I(1, 0) - I(0, 1)); + return false; + } + if (std::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)= " + "%e\n", + index, I(2, 0) - I(0, 2)); + return false; + } + if (std::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, + I(1, 2) - I(2, 1)); + return false; + } + return true; } bool isValidTransformMatrix(const mat33 &m) { -#define print_mat(x) \ - error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ - x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) +#define print_mat(x) \ + error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + 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++) { - 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); - if (length_minus_1 > kAxisLengthEpsilon) { - 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 (std::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"); - print_mat(m); - return false; - } - if (std::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"); - print_mat(m); - return false; - } - if (std::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"); - print_mat(m); - return false; - } - // check determinant (rotation not reflection) - if (determinant(m) <= 0) { - error_message("Not a valid rotation matrix (determinant <=0)\n"); - print_mat(m); - return false; - } - return true; + // check for unit length column vectors + for (int i = 0; i < 3; i++) { + 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); + if (length_minus_1 > kAxisLengthEpsilon) { + 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 (std::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"); + print_mat(m); + return false; + } + if (std::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"); + print_mat(m); + return false; + } + if (std::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"); + print_mat(m); + return false; + } + // check determinant (rotation not reflection) + if (determinant(m) <= 0) { + error_message("Not a valid rotation matrix (determinant <=0)\n"); + print_mat(m); + return false; + } + return true; } bool isUnitVector(const vec3 &vector) { - return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < - kIsZero; + return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < + kIsZero; } vec3 rpyFromMatrix(const mat33 &rot) { - vec3 rpy; - rpy(2) = std::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(0) = std::atan2(-rot(2, 0), rot(2, 2)); - return rpy; + vec3 rpy; + rpy(2) = std::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(0) = std::atan2(-rot(2, 0), rot(2, 2)); + return rpy; } } diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp index 97e60c120..f7526b500 100644 --- a/src/BulletInverseDynamics/IDMath.hpp +++ b/src/BulletInverseDynamics/IDMath.hpp @@ -1,5 +1,5 @@ /// @file Math utility functions used in inverse dynamics library. -/// Defined here as they may not be provided by the math library. +/// Defined here as they may not be provided by the math library. #ifndef IDMATH_HPP_ #define IDMATH_HPP_ @@ -39,7 +39,7 @@ bool isPositiveSemiDefiniteFuzzy(const mat33& m); /// Determinant of 3x3 matrix /// NOTE: implemented here for portability, as determinant operation -/// will be implemented differently for various matrix/vector libraries +/// will be implemented differently for various matrix/vector libraries /// @param m a 3x3 matrix /// @return det(m) idScalar determinant(const mat33& m); diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index 5c0af64e9..f5488b62a 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -11,54 +11,54 @@ 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 = new InitCache(); + : 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 = new InitCache(); } MultiBodyTree::~MultiBodyTree() { - delete m_impl; - delete m_init_cache; + delete m_impl; + delete m_init_cache; } void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { - m_accept_invalid_mass_parameters = flag; + m_accept_invalid_mass_parameters = flag; } bool MultiBodyTree::getAcceptInvalidMassProperties() const { - return m_accept_invalid_mass_parameters; + return m_accept_invalid_mass_parameters; } int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { - return m_impl->getBodyOrigin(body_index, world_origin); + return m_impl->getBodyOrigin(body_index, world_origin); } int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const { - return m_impl->getBodyCoM(body_index, world_com); + return m_impl->getBodyCoM(body_index, world_com); } int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const { - return m_impl->getBodyTransform(body_index, world_T_body); + return m_impl->getBodyTransform(body_index, world_T_body); } int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const { - return m_impl->getBodyAngularVelocity(body_index, world_omega); + return m_impl->getBodyAngularVelocity(body_index, world_omega); } int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const { - return m_impl->getBodyLinearVelocity(body_index, world_velocity); + return m_impl->getBodyLinearVelocity(body_index, world_velocity); } int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const { - return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); + return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); } int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const { - return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); + return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); } int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const { - return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); + return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); } void MultiBodyTree::printTree() { m_impl->printTree(); } @@ -69,263 +69,263 @@ 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) { - error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { - error_message("error in inverse dynamics calculation\n"); - return -1; - } - return 0; + vecx *joint_forces) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + error_message("error in inverse dynamics calculation\n"); + return -1; + } + return 0; } 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) { - error_message("system has not been initialized\n"); - return -1; - } - if (-1 == - m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, - set_lower_triangular_matrix, mass_matrix)) { - error_message("error in mass matrix calculation\n"); - return -1; - } - return 0; + const bool initialize_matrix, + const bool set_lower_triangular_matrix, matxx *mass_matrix) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == + m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, + set_lower_triangular_matrix, mass_matrix)) { + error_message("error in mass matrix calculation\n"); + return -1; + } + return 0; } int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { - return calculateMassMatrix(q, true, true, true, mass_matrix); + return calculateMassMatrix(q, true, true, true, mass_matrix); } 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) { - 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) { - case REVOLUTE: - case PRISMATIC: - // check if axis is unit vector - if (!isUnitVector(body_axis_of_motion)) { - 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 = std::sqrt(std::pow(body_axis_of_motion(0), 2) + - std::pow(body_axis_of_motion(1), 2) + - std::pow(body_axis_of_motion(2), 2)); - if (length < std::sqrt(std::numeric_limits::min())) { - error_message("axis of motion vector too short (%e)\n", length); - return -1; - } - body_axis_of_motion = (1.0 / length) * body_axis_of_motion; - } - break; - case FIXED: - break; - case FLOATING: - break; - default: - error_message("unknown joint type %d\n", joint_type); - return -1; - } + 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) { + 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) { + case REVOLUTE: + case PRISMATIC: + // check if axis is unit vector + if (!isUnitVector(body_axis_of_motion)) { + 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 = std::sqrt(std::pow(body_axis_of_motion(0), 2) + + std::pow(body_axis_of_motion(1), 2) + + std::pow(body_axis_of_motion(2), 2)); + if (length < std::sqrt(std::numeric_limits::min())) { + error_message("axis of motion vector too short (%e)\n", length); + return -1; + } + body_axis_of_motion = (1.0 / length) * body_axis_of_motion; + } + break; + case FIXED: + break; + case FLOATING: + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } - // sanity check for mass properties. Zero mass is OK. - if (mass < 0) { - m_mass_parameters_are_valid = false; - error_message("Body %d has invalid mass %e\n", body_index, mass); - if (!m_accept_invalid_mass_parameters) { - return -1; - } - } + // sanity check for mass properties. Zero mass is OK. + if (mass < 0) { + m_mass_parameters_are_valid = false; + error_message("Body %d has invalid mass %e\n", body_index, mass); + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } - 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) { - return -1; - } - } + 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) { + return -1; + } + } - if (!isValidTransformMatrix(body_T_parent_ref)) { - return -1; - } + if (!isValidTransformMatrix(body_T_parent_ref)) { + return -1; + } - return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, - body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, - body_I_body, user_int, user_ptr); + return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, + body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr); } int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { - return m_impl->getParentIndex(body_index, parent_index); + return m_impl->getParentIndex(body_index, parent_index); } int MultiBodyTree::getUserInt(const int body_index, int *user_int) const { - return m_impl->getUserInt(body_index, user_int); + return m_impl->getUserInt(body_index, user_int); } int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const { - return m_impl->getUserPtr(body_index, user_ptr); + return m_impl->getUserPtr(body_index, user_ptr); } int MultiBodyTree::setUserInt(const int body_index, const int user_int) { - return m_impl->setUserInt(body_index, user_int); + return m_impl->setUserInt(body_index, user_int); } int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) { - return m_impl->setUserPtr(body_index, user_ptr); + return m_impl->setUserPtr(body_index, user_ptr); } int MultiBodyTree::finalize() { - const int &num_bodies = m_init_cache->numBodies(); - const int &num_dofs = m_init_cache->numDoFs(); + const int &num_bodies = m_init_cache->numBodies(); + const int &num_dofs = m_init_cache->numDoFs(); - // 1 allocate internal MultiBody structure - m_impl = new MultiBodyImpl(num_bodies, num_dofs); + // 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()) { - return -1; - } - m_init_cache->getParentIndexArray(&m_impl->m_parent_index); + // 2 build new index set assuring index(parent) < index(child) + 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++) { - InertiaData inertia; - JointData joint; - if (-1 == m_init_cache->getInertiaData(index, &inertia)) { - return -1; - } - if (-1 == m_init_cache->getJointData(index, &joint)) { - return -1; - } + // 3 setup internal kinematic and dynamic data + for (int index = 0; index < num_bodies; index++) { + InertiaData inertia; + JointData joint; + if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + return -1; + } + if (-1 == m_init_cache->getJointData(index, &joint)) { + return -1; + } - RigidBody &rigid_body = m_impl->m_body_list[index]; + RigidBody &rigid_body = m_impl->m_body_list[index]; - rigid_body.m_mass = inertia.m_mass; - rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; - rigid_body.m_body_I_body = inertia.m_body_I_body; - rigid_body.m_joint_type = joint.m_type; - rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; - rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; - rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; - rigid_body.m_joint_type = joint.m_type; + rigid_body.m_mass = inertia.m_mass; + rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; + rigid_body.m_body_I_body = inertia.m_body_I_body; + rigid_body.m_joint_type = joint.m_type; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_joint_type = joint.m_type; - // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized - // matrices. - 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); - rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - case PRISMATIC: - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); - rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); - rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); - break; - case FIXED: - // NOTE/TODO: dimension really should be zero .. - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - case FLOATING: - // NOTE/TODO: this is not really correct. - // the Jacobians should be 3x3 matrices here ! - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - default: - error_message("unsupported joint type %d\n", rigid_body.m_joint_type); - return -1; - } - } + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized + // matrices. + 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); + rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case PRISMATIC: + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); + break; + case FIXED: + // NOTE/TODO: dimension really should be zero .. + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case FLOATING: + // NOTE/TODO: this is not really correct. + // the Jacobians should be 3x3 matrices here ! + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + default: + error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + return -1; + } + } - // 4 assign degree of freedom indices & build per-joint-type index arrays - if (-1 == m_impl->generateIndexSets()) { - error_message("generating index sets\n"); - return -1; - } + // 4 assign degree of freedom indices & build per-joint-type index arrays + if (-1 == m_impl->generateIndexSets()) { + error_message("generating index sets\n"); + return -1; + } - // 5 do some pre-computations .. - m_impl->calculateStaticData(); + // 5 do some pre-computations .. + m_impl->calculateStaticData(); - // 6. make sure all user forces are set to zero, as this might not happen - // in the vector ctors. - m_impl->clearAllUserForcesAndMoments(); + // 6. make sure all user forces are set to zero, as this might not happen + // in the vector ctors. + m_impl->clearAllUserForcesAndMoments(); - m_is_finalized = true; - return 0; + m_is_finalized = true; + return 0; } int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { - return m_impl->setGravityInWorldFrame(gravity); + return m_impl->setGravityInWorldFrame(gravity); } int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { - return m_impl->getJointType(body_index, joint_type); + return m_impl->getJointType(body_index, joint_type); } int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const { - return m_impl->getJointTypeStr(body_index, joint_type); + return m_impl->getJointTypeStr(body_index, joint_type); } int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const { - return m_impl->getDoFOffset(body_index, q_offset); + return m_impl->getDoFOffset(body_index, q_offset); } int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { - return m_impl->setBodyMass(body_index, mass); + return m_impl->setBodyMass(body_index, mass); } int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) { - return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); + return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); } int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) { - return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); + return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); } int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const { - return m_impl->getBodyMass(body_index, mass); + return m_impl->getBodyMass(body_index, mass); } int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const { - return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); + return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); } int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const { - return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); + 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) { - return m_impl->addUserForce(body_index, body_force); + return m_impl->addUserForce(body_index, body_force); } int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) { - return m_impl->addUserMoment(body_index, body_moment); + return m_impl->addUserMoment(body_index, body_moment); } } diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp index 9f3400b40..d33e60e12 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -8,14 +8,14 @@ namespace btInverseDynamics { /// Enumeration of supported joint types enum JointType { - /// no degree of freedom, moves with parent - FIXED = 0, - /// one rotational degree of freedom relative to parent - REVOLUTE, - /// one translational degree of freedom relative to parent - PRISMATIC, - /// six degrees of freedom relative to parent - FLOATING + /// no degree of freedom, moves with parent + FIXED = 0, + /// one rotational degree of freedom relative to parent + REVOLUTE, + /// one translational degree of freedom relative to parent + PRISMATIC, + /// six degrees of freedom relative to parent + FLOATING }; /// Interface class for calculating inverse dynamics for tree structured @@ -25,284 +25,284 @@ enum JointType { /// The q vector contains the generalized coordinate set defining the tree's configuration. /// Every joint adds elements that define the corresponding link's frame pose relative to /// its parent. For the joint types that is: -/// - FIXED: none -/// - REVOLUTE: angle of rotation [rad] -/// - PRISMATIC: displacement [m] -/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] -/// (in that order) +/// - FIXED: none +/// - REVOLUTE: angle of rotation [rad] +/// - PRISMATIC: displacement [m] +/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] +/// (in that order) /// The u vector contains the generalized speeds, which are -/// - FIXED: none -/// - REVOLUTE: time derivative of angle of rotation [rad/s] -/// - PRISMATIC: time derivative of displacement [m/s] -/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) -/// and time derivative of displacement in parent frame [m/s] +/// - FIXED: none +/// - REVOLUTE: time derivative of angle of rotation [rad/s] +/// - PRISMATIC: time derivative of displacement [m/s] +/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) +/// and time derivative of displacement in parent frame [m/s] /// /// The q and u vectors are obtained by stacking contributions of all bodies in one /// vector in the order of body indices. /// /// Note on generalized forces: analogous to u, i.e., -/// - FIXED: none -/// - REVOLUTE: moment [Nm], about joint axis -/// - PRISMATIC: force [N], along joint axis -/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame -/// (in that order) +/// - FIXED: none +/// - REVOLUTE: moment [Nm], about joint axis +/// - PRISMATIC: force [N], along joint axis +/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame +/// (in that order) /// /// TODO - force element interface (friction, springs, dampers, etc) -/// - gears and motor inertia +/// - gears and motor inertia class MultiBodyTree { public: - /// The contructor. - /// Initialization & allocation is via addBody and buildSystem calls. - MultiBodyTree(); - /// the destructor. This also deallocates all memory - ~MultiBodyTree(); + /// The contructor. + /// Initialization & allocation is via addBody and buildSystem calls. + MultiBodyTree(); + /// the destructor. This also deallocates all memory + ~MultiBodyTree(); - /// Add body to the system. this allocates memory and not real-time safe. - /// This only adds the data to an initial cache. After all bodies have been - /// added, - /// the system is setup using the buildSystem call - /// @param body_index index of the body to be added. Must >=0, =dim(u) - /// @param dot_u time derivative of u - /// @param joint_forces this is where the resulting joint forces will be - /// stored. dim(joint_forces) = dim(u) - /// @return 0 on success, -1 on error - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - /// Calculate joint space mass matrix - /// @param q generalized coordinates - /// @param initialize_matrix if true, initialize mass matrix with zero. - /// If mass_matrix is initialized to zero externally and only used - /// for mass matrix computations for the same system, it is safe to - /// set this to false. - /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix - /// is also populated, otherwise not. - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); + /// Add body to the system. this allocates memory and not real-time safe. + /// This only adds the data to an initial cache. After all bodies have been + /// added, + /// the system is setup using the buildSystem call + /// @param body_index index of the body to be added. Must >=0, =dim(u) + /// @param dot_u time derivative of u + /// @param joint_forces this is where the resulting joint forces will be + /// stored. dim(joint_forces) = dim(u) + /// @return 0 on success, -1 on error + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + /// Calculate joint space mass matrix + /// @param q generalized coordinates + /// @param initialize_matrix if true, initialize mass matrix with zero. + /// If mass_matrix is initialized to zero externally and only used + /// for mass matrix computations for the same system, it is safe to + /// set this to false. + /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix + /// is also populated, otherwise not. + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); - /// Calculate joint space mass matrix. - /// This version will update kinematics, initialize all mass_matrix elements to zero and - /// populate all mass matrix entries. - /// @param q generalized coordinates - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, matxx* mass_matrix); + /// Calculate joint space mass matrix. + /// This version will update kinematics, initialize all mass_matrix elements to zero and + /// populate all mass matrix entries. + /// @param q generalized coordinates + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, matxx* mass_matrix); - /// set gravitational acceleration - /// the default is [0;0;-9.8] in the world frame - /// @param gravity the gravitational acceleration in world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// returns number of bodies in tree - int numBodies() const; - /// returns number of mechanical degrees of freedom (dimension of q-vector) - int numDoFs() const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// get center of mass of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyCoM(const int body_index, vec3* world_com) const; - /// get transform from of a body-fixed frame to the world frame - /// @param body_index index for frame/body - /// @param world_T_body pointer for return data - /// @return 0 on success, -1 on error - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// get absolute angular velocity for a body, represented in the world frame - /// @param body_index index for frame/body - /// @param world_omega pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// get linear velocity of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_velocity pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// get linear velocity of a body's CoM, represented in world frame - /// (not required for inverse dynamics, provided for convenience) - /// @param body_index index for frame/body - /// @param world_vel_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// get origin of a body-fixed frame, represented in world frame - /// NOTE: this will include the gravitational acceleration, so the actual acceleration is - /// obtainened by setting gravitational acceleration to zero, or subtracting it. - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - /// returns the (internal) index of body - /// @param body_index is the index of a body (internal: TODO: fix/clarify - /// indexing!) - /// @param parent_index pointer to where parent index will be stored - /// @return 0 on success, -1 on error - int getParentIndex(const int body_index, int* parent_index) const; - /// get joint type - /// @param body_index index of the body - /// @param joint_type the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointType(const int body_index, JointType* joint_type) const; - /// get joint type as string - /// @param body_index index of the body - /// @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 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 - /// @return -1 on error, 0 on success - int getDoFOffset(const int body_index, int* q_offset) const; - /// get user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int getUserInt(const int body_index, int* user_int) const; - /// get user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int getUserPtr(const int body_index, void** user_ptr) const; - /// set user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int setUserInt(const int body_index, const int user_int); - /// set user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int setUserPtr(const int body_index, void* const user_ptr); - /// set mass for a body - /// @param body_index index of the body - /// @param mass the mass to set - /// @return 0 on success, -1 on failure - int setBodyMass(const int body_index, const idScalar mass); - /// set first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_mass_moment the vector to set - /// @return 0 on success, -1 on failure - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - /// set second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - /// get mass for a body - /// @param body_index index of the body - /// @param mass the mass - /// @return 0 on success, -1 on failure - int getBodyMass(const int body_index, idScalar* mass) const; - /// get first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_moment the vector - /// @return 0 on success, -1 on failure - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - /// get second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// set all user forces and moments to zero - void clearAllUserForcesAndMoments(); - /// Add an external force to a body, acting at the origin of the body-fixed frame. - /// Calls to addUserForce are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_force the force represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserForce(const int body_index, const vec3& body_force); - /// Add an external moment to a body. - /// Calls to addUserMoment are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_moment the moment represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserMoment(const int body_index, const vec3& body_moment); + /// set gravitational acceleration + /// the default is [0;0;-9.8] in the world frame + /// @param gravity the gravitational acceleration in world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// returns number of bodies in tree + int numBodies() const; + /// returns number of mechanical degrees of freedom (dimension of q-vector) + int numDoFs() const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// get center of mass of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyCoM(const int body_index, vec3* world_com) const; + /// get transform from of a body-fixed frame to the world frame + /// @param body_index index for frame/body + /// @param world_T_body pointer for return data + /// @return 0 on success, -1 on error + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// get absolute angular velocity for a body, represented in the world frame + /// @param body_index index for frame/body + /// @param world_omega pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// get linear velocity of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_velocity pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// get linear velocity of a body's CoM, represented in world frame + /// (not required for inverse dynamics, provided for convenience) + /// @param body_index index for frame/body + /// @param world_vel_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// get origin of a body-fixed frame, represented in world frame + /// NOTE: this will include the gravitational acceleration, so the actual acceleration is + /// obtainened by setting gravitational acceleration to zero, or subtracting it. + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// returns the (internal) index of body + /// @param body_index is the index of a body (internal: TODO: fix/clarify + /// indexing!) + /// @param parent_index pointer to where parent index will be stored + /// @return 0 on success, -1 on error + int getParentIndex(const int body_index, int* parent_index) const; + /// get joint type + /// @param body_index index of the body + /// @param joint_type the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointType(const int body_index, JointType* joint_type) const; + /// get joint type as string + /// @param body_index index of the body + /// @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 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 + /// @return -1 on error, 0 on success + int getDoFOffset(const int body_index, int* q_offset) const; + /// get user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int getUserInt(const int body_index, int* user_int) const; + /// get user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int getUserPtr(const int body_index, void** user_ptr) const; + /// set user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int setUserInt(const int body_index, const int user_int); + /// set user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int setUserPtr(const int body_index, void* const user_ptr); + /// set mass for a body + /// @param body_index index of the body + /// @param mass the mass to set + /// @return 0 on success, -1 on failure + int setBodyMass(const int body_index, const idScalar mass); + /// set first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_mass_moment the vector to set + /// @return 0 on success, -1 on failure + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); + /// set second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); + /// get mass for a body + /// @param body_index index of the body + /// @param mass the mass + /// @return 0 on success, -1 on failure + int getBodyMass(const int body_index, idScalar* mass) const; + /// get first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_moment the vector + /// @return 0 on success, -1 on failure + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + /// get second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// set all user forces and moments to zero + void clearAllUserForcesAndMoments(); + /// Add an external force to a body, acting at the origin of the body-fixed frame. + /// Calls to addUserForce are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_force the force represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserForce(const int body_index, const vec3& body_force); + /// Add an external moment to a body. + /// Calls to addUserMoment are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_moment the moment represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserMoment(const int body_index, const vec3& body_moment); private: - // flag indicating if system has been initialized - bool m_is_finalized; - // flag indicating if mass properties are physically valid - bool m_mass_parameters_are_valid; - // flag defining if unphysical mass parameters are accepted - bool m_accept_invalid_mass_parameters; - // This struct implements the inverse dynamics calculations - class MultiBodyImpl; - MultiBodyImpl* m_impl; - // cache data structure for initialization - class InitCache; - InitCache* m_init_cache; + // flag indicating if system has been initialized + bool m_is_finalized; + // flag indicating if mass properties are physically valid + bool m_mass_parameters_are_valid; + // flag defining if unphysical mass parameters are accepted + bool m_accept_invalid_mass_parameters; + // This struct implements the inverse dynamics calculations + class MultiBodyImpl; + MultiBodyImpl* m_impl; + // cache data structure for initialization + class InitCache; + InitCache* m_init_cache; }; } // namespace btInverseDynamics #endif // MULTIBODYTREE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index 2209121ba..c77ba3cba 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -17,29 +17,29 @@ typedef btMatrixX matxx; 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]; } - const int size() const { return 3; } - const vec3& operator=(const btVector3& rhs) { - *static_cast(this) = rhs; - return *this; - } + vec3() : btVector3() {} + vec3(const btVector3& btv) { *this = btv; } + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + const int size() const { return 3; } + const vec3& operator=(const btVector3& rhs) { + *static_cast(this) = rhs; + return *this; + } }; 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) { - *static_cast(this) = rhs; - return *this; - } - friend mat33 operator*(const idScalar& s, const mat33& a); - friend mat33 operator/(const mat33& a, const idScalar& s); + 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) { + *static_cast(this) = rhs; + return *this; + } + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator/(const mat33& a, const idScalar& s); }; inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); } @@ -48,64 +48,64 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } class vecx : public btVectorX { public: - vecx(int size) : btVectorX(size) {} - const vecx& operator=(const btVectorX& rhs) { - *static_cast(this) = rhs; - return *this; - } + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) { + *static_cast(this) = rhs; + return *this; + } - idScalar& operator()(int i) { return (*this)[i]; } - const idScalar& operator()(int i) const { return (*this)[i]; } + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } - friend vecx operator*(const vecx& a, const idScalar& s); - friend vecx operator*(const idScalar& s, const vecx& a); + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); - friend vecx operator+(const vecx& a, const vecx& b); - friend vecx operator-(const vecx& a, const vecx& b); - friend vecx operator/(const vecx& a, const idScalar& s); + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend 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++) { - result(i) = a(i) * s; - } - return result; + vecx result(a.size()); + 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) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result(i) = a(i) + b(i); - } + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) + b(i); + } - return result; + return result; } inline vecx operator-(const vecx& a, const vecx& b) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result(i) = a(i) - b(i); - } - return result; + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + 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) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result(i) = a(i) / s; - } + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) / s; + } - return result; + return result; } } diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp index a4e2ca5a4..de15e9ed4 100644 --- a/src/BulletInverseDynamics/details/IDMatVec.hpp +++ b/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -17,312 +17,312 @@ class matxx; /// want from a "fully featured" linear math library. class vec3 { public: - idScalar& operator()(int i) { return m_data[i]; } - const idScalar& operator()(int i) const { return m_data[i]; } - const int size() const { return 3; } - const vec3& operator=(const vec3& rhs); - const vec3& operator+=(const vec3& b); - const vec3& operator-=(const vec3& b); - vec3 cross(const vec3& b) const; - idScalar dot(const vec3& b) const; + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int size() const { return 3; } + const vec3& operator=(const vec3& rhs); + const vec3& operator+=(const vec3& b); + const vec3& operator-=(const vec3& b); + vec3 cross(const vec3& b) const; + idScalar dot(const vec3& b) const; - friend vec3 operator*(const mat33& a, const vec3& b); - friend vec3 operator*(const vec3& a, const idScalar& s); - friend vec3 operator*(const idScalar& s, const vec3& a); + friend vec3 operator*(const mat33& a, const vec3& b); + friend vec3 operator*(const vec3& a, const idScalar& s); + friend vec3 operator*(const idScalar& s, const vec3& a); - friend vec3 operator+(const vec3& a, const vec3& b); - friend vec3 operator-(const vec3& a, const vec3& b); - friend vec3 operator/(const vec3& a, const idScalar& s); + friend vec3 operator+(const vec3& a, const vec3& b); + friend vec3 operator-(const vec3& a, const vec3& b); + friend vec3 operator/(const vec3& a, const idScalar& s); private: - idScalar m_data[3]; + idScalar m_data[3]; }; 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]; } - const mat33& operator=(const mat33& rhs); - mat33 transpose() const; - const mat33& operator+=(const mat33& b); - const mat33& operator-=(const mat33& b); + 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]; } + const mat33& operator=(const mat33& rhs); + mat33 transpose() const; + const mat33& operator+=(const mat33& b); + const mat33& operator-=(const mat33& b); - friend mat33 operator*(const mat33& a, const mat33& b); - friend vec3 operator*(const mat33& a, const vec3& b); - friend mat33 operator*(const mat33& a, const idScalar& s); - friend mat33 operator*(const idScalar& s, const mat33& a); - friend mat33 operator+(const mat33& a, const mat33& b); - friend mat33 operator-(const mat33& a, const mat33& b); - friend mat33 operator/(const mat33& a, const idScalar& s); + friend mat33 operator*(const mat33& a, const mat33& b); + friend vec3 operator*(const mat33& a, const vec3& b); + friend mat33 operator*(const mat33& a, const idScalar& s); + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator+(const mat33& a, const mat33& b); + friend mat33 operator-(const mat33& a, const mat33& b); + friend mat33 operator/(const mat33& a, const idScalar& s); private: - // layout is [0,1,2;3,4,5;6,7,8] - idScalar m_data[9]; + // layout is [0,1,2;3,4,5;6,7,8] + idScalar m_data[9]; }; class vecx { public: - vecx(int size) : m_size(size) { - m_data = static_cast(idMalloc(sizeof(idScalar) * size)); - } - ~vecx() { idFree(m_data); } - const vecx& operator=(const vecx& rhs); - idScalar& operator()(int i) { return m_data[i]; } - const idScalar& operator()(int i) const { return m_data[i]; } - const int& size() const { return m_size; } + vecx(int size) : m_size(size) { + m_data = static_cast(idMalloc(sizeof(idScalar) * size)); + } + ~vecx() { idFree(m_data); } + const vecx& operator=(const vecx& rhs); + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int& size() const { return m_size; } - friend vecx operator*(const vecx& a, const idScalar& s); - friend vecx operator*(const idScalar& s, const vecx& a); + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); - friend vecx operator+(const vecx& a, const vecx& b); - friend vecx operator-(const vecx& a, const vecx& b); - friend vecx operator/(const vecx& a, const idScalar& s); + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); private: - int m_size; - idScalar* m_data; + int m_size; + idScalar* m_data; }; class matxx { public: - matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { - m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); - } - ~matxx() { idFree(m_data); } - const matxx& operator=(const matxx& rhs); - idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; } - const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; } - const int& rows() const { return m_rows; } - const int& cols() const { return m_cols; } + matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); + } + ~matxx() { idFree(m_data); } + const matxx& operator=(const matxx& rhs); + idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; } + const int& rows() const { return m_rows; } + const int& cols() const { return m_cols; } private: - int m_rows; - int m_cols; - idScalar* m_data; + int m_rows; + int m_cols; + idScalar* m_data; }; inline const vec3& vec3::operator=(const vec3& rhs) { - if (&rhs != this) { - memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); - } - return *this; + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); + } + return *this; } 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]; - result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; + 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]; + result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; - return result; + return result; } 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]; + 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) { - memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); - } - return *this; + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); + } + return *this; } inline mat33 mat33::transpose() const { - mat33 result; - result.m_data[0] = m_data[0]; - result.m_data[1] = m_data[3]; - result.m_data[2] = m_data[6]; - result.m_data[3] = m_data[1]; - result.m_data[4] = m_data[4]; - result.m_data[5] = m_data[7]; - result.m_data[6] = m_data[2]; - result.m_data[7] = m_data[5]; - result.m_data[8] = m_data[8]; + mat33 result; + result.m_data[0] = m_data[0]; + result.m_data[1] = m_data[3]; + result.m_data[2] = m_data[6]; + result.m_data[3] = m_data[1]; + result.m_data[4] = m_data[4]; + result.m_data[5] = m_data[7]; + result.m_data[6] = m_data[2]; + result.m_data[7] = m_data[5]; + result.m_data[8] = m_data[8]; - return result; + return result; } 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]; - result.m_data[1] = - a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; - result.m_data[2] = - a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; - result.m_data[3] = - a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; - result.m_data[4] = - a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; - result.m_data[5] = - a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; - result.m_data[6] = - a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; - result.m_data[7] = - a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; - result.m_data[8] = - a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; + 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]; + result.m_data[1] = + a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; + result.m_data[2] = + a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; + result.m_data[3] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; + result.m_data[4] = + a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; + result.m_data[5] = + a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; + result.m_data[6] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; + result.m_data[7] = + a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; + result.m_data[8] = + a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; - return result; + return result; } inline const mat33& mat33::operator+=(const mat33& b) { - for (int i = 0; i < 9; i++) { - m_data[i] += b.m_data[i]; - } + for (int i = 0; i < 9; i++) { + m_data[i] += b.m_data[i]; + } - return *this; + return *this; } inline const mat33& mat33::operator-=(const mat33& b) { - for (int i = 0; i < 9; i++) { - m_data[i] -= b.m_data[i]; - } - return *this; + 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) { - vec3 result; + vec3 result; - result.m_data[0] = - a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; - result.m_data[1] = - a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; - result.m_data[2] = - a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; + result.m_data[1] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; + result.m_data[2] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; - return result; + return result; } inline const vec3& vec3::operator+=(const vec3& b) { - for (int i = 0; i < 3; i++) { - m_data[i] += b.m_data[i]; - } - return *this; + 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++) { - m_data[i] -= b.m_data[i]; - } - return *this; + 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) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] * s; - } - return result; + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; } inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } inline vec3 operator*(const vec3& a, const idScalar& s) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] * s; - } - return result; + vec3 result; + 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) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - return result; + mat33 result; + 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) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - return result; + vec3 result; + 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) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; + mat33 result; + 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) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; + vec3 result; + 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) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] / s; - } - return result; + mat33 result; + 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) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] / s; - } - return result; + vec3 result; + 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()) { - error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); - abort(); - } - if (&rhs != this) { - memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); - } - return *this; + if (size() != rhs.size()) { + error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); + abort(); + } + if (&rhs != this) { + memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); + } + return *this; } inline vecx operator*(const vecx& a, const idScalar& s) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result.m_data[i] = a.m_data[i] * s; - } - return result; + vecx result(a.size()); + 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) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } - return result; + return result; } inline vecx operator-(const vecx& a, const vecx& b) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + 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) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result.m_data[i] = a.m_data[i] / s; - } + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] / s; + } - return result; + return result; } } #endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 211139459..06a3595be 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -3,482 +3,482 @@ namespace btInverseDynamics { MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) - : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) { - m_body_list.resize(num_bodies_); - m_parent_index.resize(num_bodies_); - m_child_indices.resize(num_bodies_); - m_user_int.resize(num_bodies_); - m_user_ptr.resize(num_bodies_); + : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) { + m_body_list.resize(num_bodies_); + m_parent_index.resize(num_bodies_); + m_child_indices.resize(num_bodies_); + m_user_int.resize(num_bodies_); + m_user_ptr.resize(num_bodies_); - m_world_gravity(0) = 0.0; - m_world_gravity(1) = 0.0; - m_world_gravity(2) = -9.8; + m_world_gravity(0) = 0.0; + m_world_gravity(1) = 0.0; + m_world_gravity(2) = -9.8; } const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { - switch (type) { - case FIXED: - return "fixed"; - case REVOLUTE: - return "revolute"; - case PRISMATIC: - return "prismatic"; - case FLOATING: - return "floating"; - } - return "error: invalid"; + switch (type) { + case FIXED: + return "fixed"; + case REVOLUTE: + return "revolute"; + case PRISMATIC: + return "prismatic"; + case FLOATING: + return "floating"; + } + return "error: invalid"; } inline void indent(const int &level) { - for (int j = 0; j < level; j++) - id_printf(" "); // indent + for (int j = 0; j < level; j++) + id_printf(" "); // indent } void MultiBodyTree::MultiBodyImpl::printTree() { - id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); - printTree(0, 0); + id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); + printTree(0, 0); } void MultiBodyTree::MultiBodyImpl::printTreeData() { - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - id_printf("body: %d\n", static_cast(i)); - id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); - id_printf("q_index= %d\n", body.m_q_index); - id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); - id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + id_printf("body: %d\n", static_cast(i)); + id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); + id_printf("q_index= %d\n", body.m_q_index); + id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); + id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); - id_printf("mass = %f\n", body.m_mass); - id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), - body.m_body_mass_com(2)); - id_printf("I_o= [%f %f %f;\n" - " %f %f %f;\n" - " %f %f %f]\n", - body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), - body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), - body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); + id_printf("mass = %f\n", body.m_mass); + id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), + body.m_body_mass_com(2)); + id_printf("I_o= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), + body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), + body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); - id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), - body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); - } + id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), + body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); + } } int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { - switch (type) { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - } - error_message("unknown joint type %d\n", type); - return 0; + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + error_message("unknown joint type %d\n", type); + return 0; } void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { - // this is adapted from URDF2Bullet. - // TODO: fix this and print proper graph (similar to git --log --graph) - int num_children = m_child_indices[index].size(); + // this is adapted from URDF2Bullet. + // TODO: fix this and print proper graph (similar to git --log --graph) + int num_children = m_child_indices[index].size(); - indentation += 2; - int count = 0; + indentation += 2; + int count = 0; - for (int i = 0; i < num_children; i++) { - int child_index = m_child_indices[index][i]; - indent(indentation); - id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, - jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, - m_body_list[index].m_q_index, - m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); - // first grandchild - printTree(child_index, indentation); - } + for (int i = 0; i < num_children; i++) { + int child_index = m_child_indices[index][i]; + indent(indentation); + id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, + jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, + m_body_list[index].m_q_index, + m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); + // first grandchild + printTree(child_index, indentation); + } } int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { - m_world_gravity = gravity; - return 0; + m_world_gravity = gravity; + return 0; } int MultiBodyTree::MultiBodyImpl::generateIndexSets() { - m_body_revolute_list.resize(0); - m_body_prismatic_list.resize(0); - int q_index = 0; - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - body.m_q_index = -1; - switch (body.m_joint_type) { - case REVOLUTE: - m_body_revolute_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case PRISMATIC: - m_body_prismatic_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case FIXED: - // do nothing - break; - case FLOATING: - m_body_floating_list.push_back(i); - body.m_q_index = q_index; - q_index += 6; - break; - default: - error_message("unsupported joint type %d\n", body.m_joint_type); - return -1; - } - } - // sanity check - if (q_index != m_num_dofs) { - error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); - return -1; - } + m_body_revolute_list.resize(0); + m_body_prismatic_list.resize(0); + int q_index = 0; + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + body.m_q_index = -1; + switch (body.m_joint_type) { + case REVOLUTE: + m_body_revolute_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case PRISMATIC: + m_body_prismatic_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case FIXED: + // do nothing + break; + case FLOATING: + m_body_floating_list.push_back(i); + body.m_q_index = q_index; + q_index += 6; + break; + default: + error_message("unsupported joint type %d\n", body.m_joint_type); + return -1; + } + } + // sanity check + if (q_index != m_num_dofs) { + error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); + return -1; + } - m_child_indices.resize(m_body_list.size()); + m_child_indices.resize(m_body_list.size()); - for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { - const int &parent = m_parent_index[child]; - if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { - m_child_indices[parent].push_back(child); - } else { - if (-1 == parent) { - // multiple bodies are directly linked to the environment, ie, not a single root - error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); - } else { - // should never happen - error_message( - "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", - child, parent, static_cast(m_parent_index.size())); - } - return -1; - } - } + for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { + const int &parent = m_parent_index[child]; + if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { + m_child_indices[parent].push_back(child); + } else { + if (-1 == parent) { + // multiple bodies are directly linked to the environment, ie, not a single root + error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } else { + // should never happen + error_message( + "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", + child, parent, static_cast(m_parent_index.size())); + } + return -1; + } + } - return 0; + return 0; } void MultiBodyTree::MultiBodyImpl::calculateStaticData() { - // relative kinematics that are not a function of q, u, dot_u - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) { - case REVOLUTE: - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - break; - case PRISMATIC: - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - break; - case FIXED: - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - break; - case FLOATING: - // no static data - break; - } - } + // relative kinematics that are not a function of q, u, dot_u + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + switch (body.m_joint_type) { + case REVOLUTE: + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + break; + case PRISMATIC: + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + break; + case FIXED: + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + break; + case FLOATING: + // no static data + break; + } + } } int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, - const vecx &dot_u, vecx *joint_forces) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || - joint_forces->size() != m_num_dofs) { - error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", - m_num_dofs, static_cast(q.size()), static_cast(u.size()), - static_cast(dot_u.size()), static_cast(joint_forces->size())); - return -1; - } - // 1. update relative kinematics - // 1.1 for revolute - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - mat33 T; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); - body.m_body_T_parent = T * body.m_body_T_parent_ref; + const vecx &dot_u, vecx *joint_forces) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || + joint_forces->size() != m_num_dofs) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size()), + static_cast(dot_u.size()), static_cast(joint_forces->size())); + return -1; + } + // 1. update relative kinematics + // 1.1 for revolute + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + mat33 T; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); + body.m_body_T_parent = T * body.m_body_T_parent_ref; - // body.m_parent_r_parent_body= fixed - body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); - // body.m_parent_dot_r_rel = fixed; - // NOTE: this assumes that Jac_JR is constant, which is true for revolute - // joints, but not in the general case (eg, slider-crank type mechanisms) - body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); - // body.m_parent_ddot_r_rel = fixed; - } - // 1.2 for prismatic - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - // body.m_parent_omega_rel = 0; - body.m_parent_vel_rel = - body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); - // body.parent_dot_omega_rel = 0; - // NOTE: this assumes that Jac_JT is constant, which is true for - // prismatic joints, but not in the general case - body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; + // body.m_parent_r_parent_body= fixed + body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); + // body.m_parent_dot_r_rel = fixed; + // NOTE: this assumes that Jac_JR is constant, which is true for revolute + // joints, but not in the general case (eg, slider-crank type mechanisms) + body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); + // body.m_parent_ddot_r_rel = fixed; + } + // 1.2 for prismatic + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + // body.m_parent_omega_rel = 0; + body.m_parent_vel_rel = + body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); + // body.parent_dot_omega_rel = 0; + // NOTE: this assumes that Jac_JT is constant, which is true for + // prismatic joints, but not in the general case + body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); - body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); - body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); + body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); + body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); + body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); - body.m_parent_vel_rel(0) = u(body.m_q_index + 3); - body.m_parent_vel_rel(1) = u(body.m_q_index + 4); - body.m_parent_vel_rel(2) = u(body.m_q_index + 5); + body.m_parent_vel_rel(0) = u(body.m_q_index + 3); + body.m_parent_vel_rel(1) = u(body.m_q_index + 4); + body.m_parent_vel_rel(2) = u(body.m_q_index + 5); - body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); - body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); - body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); + body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); + body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); + body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); - body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); - body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); - body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); + body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); + body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); + body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; - body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; - } + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } - // 3. absolute kinematic quantities & dynamic quantities - // NOTE: this should be optimized by specializing for different body types - // (e.g., relative rotation is always zero for prismatic joints, etc.) + // 3. absolute kinematic quantities & dynamic quantities + // NOTE: this should be optimized by specializing for different body types + // (e.g., relative rotation is always zero for prismatic joints, etc.) - // calculations for root body - { - RigidBody &body = m_body_list[0]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) - // not required right now, added here for debugging purposes - body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_body_T_world = body.m_body_T_parent; + // calculations for root body + { + RigidBody &body = m_body_list[0]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) + // not required right now, added here for debugging purposes + body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_body_T_world = body.m_body_T_parent; - // 3.2 update absolute velocities - body.m_body_ang_vel = body.m_body_ang_vel_rel; - body.m_body_vel = body.m_parent_vel_rel; + // 3.2 update absolute velocities + body.m_body_ang_vel = body.m_body_ang_vel_rel; + body.m_body_vel = body.m_parent_vel_rel; - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = body.m_body_ang_acc_rel; - body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; - // add gravitational acceleration to root body - // this is an efficient way to add gravitational terms, - // but it does mean that the kinematics are no longer - // correct at the acceleration level - // NOTE: To get correct acceleration kinematics, just set world_gravity to zero - body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; - } + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = body.m_body_ang_acc_rel; + body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; + // add gravitational acceleration to root body + // this is an efficient way to add gravitational terms, + // but it does mean that the kinematics are no longer + // correct at the acceleration level + // NOTE: To get correct acceleration kinematics, just set world_gravity to zero + body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; + } - for (idArrayIdx i = 1; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - RigidBody &parent = m_body_list[m_parent_index[i]]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) not required right now added here for debugging purposes - body.m_body_pos = - body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); - body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) not required right now added here for debugging purposes + body.m_body_pos = + body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); + body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; - // 3.2 update absolute velocities - body.m_body_ang_vel = - body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; + // 3.2 update absolute velocities + body.m_body_ang_vel = + body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; - body.m_body_vel = - body.m_body_T_parent * - (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + - body.m_parent_vel_rel); + body.m_body_vel = + body.m_body_T_parent * + (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + + body.m_parent_vel_rel); - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = - body.m_body_T_parent * parent.m_body_ang_acc - - body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + - body.m_body_ang_acc_rel; - body.m_body_acc = - body.m_body_T_parent * - (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + - parent.m_body_ang_vel.cross( - parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + - 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); - } + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = + body.m_body_T_parent * parent.m_body_ang_acc - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + + body.m_body_ang_acc_rel; + body.m_body_acc = + body.m_body_T_parent * + (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross( + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); + } - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - // 3.4 update dynamic terms (rate of change of angular & linear momentum) - body.m_eom_lhs_rotational = - body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + - body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - - body.m_body_moment_user; - body.m_eom_lhs_translational = - body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + - body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - - body.m_body_force_user; - } + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + // 3.4 update dynamic terms (rate of change of angular & linear momentum) + body.m_eom_lhs_rotational = + body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + + body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - + body.m_body_moment_user; + body.m_eom_lhs_translational = + body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + + body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - + body.m_body_force_user; + } - // 4. calculate full set of forces at parent joint - // (not directly calculating the joint force along the free direction - // simplifies inclusion of fixed joints. - // An alternative would be to fuse bodies in a pre-processing step, - // but that would make changing masses online harder (eg, payload masses - // added with fixed joints to a gripper) - // Also, this enables adding zero weight bodies as a way to calculate frame poses - // for force elements, etc. + // 4. calculate full set of forces at parent joint + // (not directly calculating the joint force along the free direction + // simplifies inclusion of fixed joints. + // An alternative would be to fuse bodies in a pre-processing step, + // but that would make changing masses online harder (eg, payload masses + // added with fixed joints to a gripper) + // Also, this enables adding zero weight bodies as a way to calculate frame poses + // for force elements, etc. - for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { - // sum of forces and moments acting on this body from its children - vec3 sum_f_children; - vec3 sum_m_children; - setZero(sum_f_children); - setZero(sum_m_children); - for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); - child_list_idx++) { - const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; - vec3 child_joint_force_in_this_frame = - child.m_body_T_parent.transpose() * child.m_force_at_joint; - sum_f_children -= child_joint_force_in_this_frame; - sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + - child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); - } - RigidBody &body = m_body_list[body_idx]; + for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { + // sum of forces and moments acting on this body from its children + vec3 sum_f_children; + vec3 sum_m_children; + setZero(sum_f_children); + setZero(sum_m_children); + for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); + child_list_idx++) { + const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; + vec3 child_joint_force_in_this_frame = + child.m_body_T_parent.transpose() * child.m_force_at_joint; + sum_f_children -= child_joint_force_in_this_frame; + sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + + child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); + } + RigidBody &body = m_body_list[body_idx]; - body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; - body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; - } + body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; + body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; + } - // 4. Calculate Joint forces. - // These are the components of force_at_joint/moment_at_joint - // in the free directions given by Jac_JT/Jac_JR - // 4.1 revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); - } - // 4.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); - } - // 4.3 floating bodies (6-DoF joints) - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); - (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); - (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); + // 4. Calculate Joint forces. + // These are the components of force_at_joint/moment_at_joint + // in the free directions given by Jac_JT/Jac_JR + // 4.1 revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); + } + // 4.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); + } + // 4.3 floating bodies (6-DoF joints) + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); + (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); + (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); - (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); - (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); - (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); - } + (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); + (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); + (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); + } - return 0; + return 0; } static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { - switch (dof) { - // rotational part - case 0: - Jac_JR(0) = 1; - Jac_JR(1) = 0; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 1: - Jac_JR(0) = 0; - Jac_JR(1) = 1; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 2: - Jac_JR(0) = 0; - Jac_JR(1) = 0; - Jac_JR(2) = 1; - setZero(Jac_JT); - break; - // translational part - case 3: - setZero(Jac_JR); - Jac_JT(0) = 1; - Jac_JT(1) = 0; - Jac_JT(2) = 0; - break; - case 4: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 1; - Jac_JT(2) = 0; - break; - case 5: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 0; - Jac_JT(2) = 1; - break; - } + switch (dof) { + // rotational part + case 0: + Jac_JR(0) = 1; + Jac_JR(1) = 0; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 1: + Jac_JR(0) = 0; + Jac_JR(1) = 1; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 2: + Jac_JR(0) = 0; + Jac_JR(1) = 0; + Jac_JR(2) = 1; + setZero(Jac_JT); + break; + // translational part + case 3: + setZero(Jac_JR); + Jac_JT(0) = 1; + Jac_JT(1) = 0; + Jac_JT(2) = 0; + break; + case 4: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 1; + Jac_JT(2) = 0; + break; + case 5: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 0; + Jac_JT(2) = 1; + break; + } } static inline int jointNumDoFs(const JointType &type) { - switch (type) { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - } - // this should never happen - error_message("invalid joint type\n"); - // TODO add configurable abort/crash function - abort(); + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + // this should never happen + error_message("invalid joint type\n"); + // TODO add configurable abort/crash function + abort(); } int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, - const bool initialize_matrix, - const bool set_lower_triangular_matrix, - matxx *mass_matrix) { + const bool initialize_matrix, + const bool set_lower_triangular_matrix, + matxx *mass_matrix) { // This calculates the joint space mass matrix for the multibody system. // The algorithm is essentially an implementation of "method 3" // in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) @@ -494,351 +494,351 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool #define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val #endif - if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || - mass_matrix->cols() != m_num_dofs) { - error_message("Dimension error. System has %d DOFs,\n" - "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", - m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), - static_cast(mass_matrix->cols())); - return -1; - } + if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || + mass_matrix->cols() != m_num_dofs) { + error_message("Dimension error. System has %d DOFs,\n" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), + static_cast(mass_matrix->cols())); + return -1; + } - // TODO add optimized zeroing function? - if (initialize_matrix) { - for (int i = 0; i < m_num_dofs; i++) { - for (int j = 0; j < m_num_dofs; j++) { - setMassMatrixElem(i, j, 0.0); - } - } - } + // TODO add optimized zeroing function? + if (initialize_matrix) { + for (int i = 0; i < m_num_dofs; i++) { + for (int j = 0; j < m_num_dofs; j++) { + setMassMatrixElem(i, j, 0.0); + } + } + } - if (update_kinematics) { - // 1. update relative kinematics - // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // from reference orientation (q=0) of body-fixed frame to current orientation - mat33 body_T_body_ref; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); - body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; - } - // 1.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; + if (update_kinematics) { + // 1. update relative kinematics + // 1.1 for revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // from reference orientation (q=0) of body-fixed frame to current orientation + mat33 body_T_body_ref; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); + body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; + } + // 1.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * - transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * + transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - } - } - for (int i = m_body_list.size() - 1; i >= 0; i--) { - RigidBody &body = m_body_list[i]; - // calculate mass, center of mass and inertia of "composite rigid body", - // ie, sub-tree starting at current body - body.m_subtree_mass = body.m_mass; - body.m_body_subtree_mass_com = body.m_body_mass_com; - body.m_body_subtree_I_body = body.m_body_I_body; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + } + } + for (int i = m_body_list.size() - 1; i >= 0; i--) { + RigidBody &body = m_body_list[i]; + // calculate mass, center of mass and inertia of "composite rigid body", + // ie, sub-tree starting at current body + body.m_subtree_mass = body.m_mass; + body.m_body_subtree_mass_com = body.m_body_mass_com; + body.m_body_subtree_I_body = body.m_body_I_body; - for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { - RigidBody &child = m_body_list[m_child_indices[i][c]]; - mat33 body_T_child = child.m_body_T_parent.transpose(); + for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { + RigidBody &child = m_body_list[m_child_indices[i][c]]; + mat33 body_T_child = child.m_body_T_parent.transpose(); - body.m_subtree_mass += child.m_subtree_mass; - body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + - child.m_parent_pos_parent_body * child.m_subtree_mass; - body.m_body_subtree_I_body += - body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; + body.m_subtree_mass += child.m_subtree_mass; + body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + + child.m_parent_pos_parent_body * child.m_subtree_mass; + body.m_body_subtree_I_body += + body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; - if (child.m_subtree_mass > 0) { - // Shift the reference point for the child subtree inertia using the - // Huygens-Steiner ("parallel axis") theorem. - // (First shift from child origin to child com, then from there to this body's - // origin) - vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; - mat33 tilde_r_child_com = tildeOperator(r_com); - mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); - body.m_body_subtree_I_body += - child.m_subtree_mass * - (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); - } - } - } + if (child.m_subtree_mass > 0) { + // Shift the reference point for the child subtree inertia using the + // Huygens-Steiner ("parallel axis") theorem. + // (First shift from child origin to child com, then from there to this body's + // origin) + vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; + mat33 tilde_r_child_com = tildeOperator(r_com); + mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); + body.m_body_subtree_I_body += + child.m_subtree_mass * + (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); + } + } + } - for (int i = m_body_list.size() - 1; i >= 0; i--) { - const RigidBody &body = m_body_list[i]; + for (int i = m_body_list.size() - 1; i >= 0; i--) { + const RigidBody &body = m_body_list[i]; - // determine DoF-range for body - const int q_index_min = body.m_q_index; - const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; - // loop over the DoFs used by this body - // local joint jacobians (ok as is for 1-DoF joints) - vec3 Jac_JR = body.m_Jac_JR; - vec3 Jac_JT = body.m_Jac_JT; - for (int col = q_index_max; col >= q_index_min; col--) { - // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) { - setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); - } + // determine DoF-range for body + const int q_index_min = body.m_q_index; + const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; + // loop over the DoFs used by this body + // local joint jacobians (ok as is for 1-DoF joints) + vec3 Jac_JR = body.m_Jac_JR; + vec3 Jac_JT = body.m_Jac_JT; + for (int col = q_index_max; col >= q_index_min; col--) { + // set jacobians for 6-DoF joints + if (FLOATING == body.m_joint_type) { + setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); + } - vec3 body_eom_rot = - body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); - vec3 body_eom_trans = - body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); - setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans)); + vec3 body_eom_rot = + body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); + vec3 body_eom_trans = + body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); + setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans)); - // rest of the mass matrix column upwards - { - // 1. for multi-dof joints, rest of the dofs of this body - for (int row = col - 1; row >= q_index_min; row--) { - if (FLOATING != body.m_joint_type) { - error_message("??\n"); - return -1; - } - setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMassMatrixElem(col, row, Mrc); - } - // 2. ancestor dofs - int child_idx = i; - int parent_idx = m_parent_index[i]; - while (parent_idx >= 0) { - const RigidBody &child_body = m_body_list[child_idx]; - const RigidBody &parent_body = m_body_list[parent_idx]; + // rest of the mass matrix column upwards + { + // 1. for multi-dof joints, rest of the dofs of this body + for (int row = col - 1; row >= q_index_min; row--) { + if (FLOATING != body.m_joint_type) { + error_message("??\n"); + return -1; + } + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMassMatrixElem(col, row, Mrc); + } + // 2. ancestor dofs + int child_idx = i; + int parent_idx = m_parent_index[i]; + while (parent_idx >= 0) { + const RigidBody &child_body = m_body_list[child_idx]; + const RigidBody &parent_body = m_body_list[parent_idx]; - const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); - body_eom_rot = parent_T_child * body_eom_rot; - body_eom_trans = parent_T_child * body_eom_trans; - body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); + const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); + body_eom_rot = parent_T_child * body_eom_rot; + body_eom_trans = parent_T_child * body_eom_trans; + body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); - const int parent_body_q_index_min = parent_body.m_q_index; - const int parent_body_q_index_max = - parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; - vec3 Jac_JR = parent_body.m_Jac_JR; - vec3 Jac_JT = parent_body.m_Jac_JT; - for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { - // set jacobians for 6-DoF joints - if (FLOATING == parent_body.m_joint_type) { - setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); - } - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMassMatrixElem(col, row, Mrc); - } + const int parent_body_q_index_min = parent_body.m_q_index; + const int parent_body_q_index_max = + parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; + vec3 Jac_JR = parent_body.m_Jac_JR; + vec3 Jac_JT = parent_body.m_Jac_JT; + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { + // set jacobians for 6-DoF joints + if (FLOATING == parent_body.m_joint_type) { + setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMassMatrixElem(col, row, Mrc); + } - child_idx = parent_idx; - parent_idx = m_parent_index[child_idx]; - } - } - } - } + child_idx = parent_idx; + parent_idx = m_parent_index[child_idx]; + } + } + } + } - if (set_lower_triangular_matrix) { - for (int col = 0; col < m_num_dofs; col++) { - for (int row = 0; row < col; row++) { - setMassMatrixElem(row, col, (*mass_matrix)(col, row)); - } - } - } + if (set_lower_triangular_matrix) { + for (int col = 0; col < m_num_dofs; col++) { + for (int row = 0; row < col; row++) { + setMassMatrixElem(row, col, (*mass_matrix)(col, row)); + } + } + } #undef setMassMatrixElem - return 0; + return 0; } // utility macro -#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ - do { \ - if (index < 0 || index >= m_num_bodies) { \ - error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ - return -1; \ - } \ - } while (0) +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do { \ + if (index < 0 || index >= m_num_bodies) { \ + error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ + return -1; \ + } \ + } while (0) int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *p = m_parent_index[body_index]; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *p = m_parent_index[body_index]; + return 0; } int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_int = m_user_int[body_index]; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_int = m_user_int[body_index]; + return 0; } int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_ptr = m_user_ptr[body_index]; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_ptr = m_user_ptr[body_index]; + return 0; } int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_int[body_index] = user_int; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_int[body_index] = user_int; + return 0; } int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_ptr[body_index] = user_ptr; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_ptr[body_index] = user_ptr; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - if (body.m_mass > 0) { - *world_com = body.m_body_T_world.transpose() * - (body.m_body_pos + body.m_body_mass_com / body.m_mass); - } else { - *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); - } - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + if (body.m_mass > 0) { + *world_com = body.m_body_T_world.transpose() * + (body.m_body_pos + body.m_body_mass_com / body.m_mass); + } else { + *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); + } + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_T_body = body.m_body_T_world.transpose(); - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_T_body = body.m_body_T_world.transpose(); + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, - vec3 *world_velocity) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; - return 0; + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - vec3 com; - if (body.m_mass > 0) { - com = body.m_body_mass_com / body.m_mass; - } else { - com(0) = 0; - com(1) = 0; - com(2) = 0; - } + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + vec3 com; + if (body.m_mass > 0) { + com = body.m_body_mass_com / body.m_mass; + } else { + com(0) = 0; + com(1) = 0; + com(2) = 0; + } - *world_velocity = - body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); - return 0; + *world_velocity = + body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; - return 0; + vec3 *world_dot_omega) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, - vec3 *world_acceleration) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; - return 0; + vec3 *world_acceleration) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; + return 0; } int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *joint_type = m_body_list[body_index].m_joint_type; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = m_body_list[body_index].m_joint_type; + return 0; } int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, - const char **joint_type) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); - return 0; + const char **joint_type) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); + return 0; } int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *q_index = m_body_list[body_index].m_q_index; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *q_index = m_body_list[body_index].m_q_index; + return 0; } int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_mass = mass; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_mass = mass; + return 0; } int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3& first_mass_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_mass_com = first_mass_moment; - return 0; + const vec3& first_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_mass_com = first_mass_moment; + return 0; } int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, - const mat33& second_mass_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_I_body = second_mass_moment; - return 0; + const mat33& second_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_I_body = second_mass_moment; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *mass = m_body_list[body_index].m_mass; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *mass = m_body_list[body_index].m_mass; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, - vec3 *first_mass_moment) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *first_mass_moment = m_body_list[body_index].m_body_mass_com; - return 0; + vec3 *first_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *first_mass_moment = m_body_list[body_index].m_body_mass_com; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, - mat33 *second_mass_moment) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *second_mass_moment = m_body_list[body_index].m_body_I_body; - return 0; + mat33 *second_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *second_mass_moment = m_body_list[body_index].m_body_I_body; + return 0; } void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() { - for (int index = 0; index < m_num_bodies; index++) { - RigidBody &body = m_body_list[index]; - setZero(body.m_body_force_user); - setZero(body.m_body_moment_user); - } + for (int index = 0; index < m_num_bodies; index++) { + RigidBody &body = m_body_list[index]; + setZero(body.m_body_force_user); + setZero(body.m_body_moment_user); + } } int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_force_user += body_force; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_force_user += body_force; + return 0; } int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_moment_user += body_moment; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_moment_user += body_moment; + return 0; } } diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 286b19d26..7787a4665 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -14,223 +14,223 @@ namespace btInverseDynamics { /// 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 { - ID_DECLARE_ALIGNED_ALLOCATOR(); - // 1 Inertial properties - /// Mass - idScalar m_mass; - /// Mass times center of gravity in body-fixed frame - vec3 m_body_mass_com; - /// Moment of inertia w.r.t. body-fixed frame - mat33 m_body_I_body; + ID_DECLARE_ALIGNED_ALLOCATOR(); + // 1 Inertial properties + /// Mass + idScalar m_mass; + /// Mass times center of gravity in body-fixed frame + vec3 m_body_mass_com; + /// Moment of inertia w.r.t. body-fixed frame + mat33 m_body_I_body; - // 2 dynamic properties - /// Left-hand side of the body equation of motion, translational part - vec3 m_eom_lhs_translational; - /// Left-hand side of the body equation of motion, rotational part - vec3 m_eom_lhs_rotational; - /// Force acting at the joint when the body is cut from its parent; - /// includes impressed joint force in J_JT direction, - /// as well as constraint force, - /// in body-fixed frame - vec3 m_force_at_joint; - /// Moment acting at the joint when the body is cut from its parent; - /// includes impressed joint moment in J_JR direction, and constraint moment - /// in body-fixed frame - vec3 m_moment_at_joint; - /// external (user provided) force acting at the body-fixed frame's origin, written in that - /// frame - vec3 m_body_force_user; - /// external (user provided) moment acting at the body-fixed frame's origin, written in that - /// frame - vec3 m_body_moment_user; - // 3 absolute kinematic properties - /// Position of body-fixed frame relative to world frame - /// this is currently only for debugging purposes - vec3 m_body_pos; - /// Absolute velocity of body-fixed frame - vec3 m_body_vel; - /// Absolute acceleration of body-fixed frame - /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational - /// acceleration! - vec3 m_body_acc; - /// Absolute angular velocity - vec3 m_body_ang_vel; - /// Absolute angular acceleration - /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational - /// acceleration! - vec3 m_body_ang_acc; + // 2 dynamic properties + /// Left-hand side of the body equation of motion, translational part + vec3 m_eom_lhs_translational; + /// Left-hand side of the body equation of motion, rotational part + vec3 m_eom_lhs_rotational; + /// Force acting at the joint when the body is cut from its parent; + /// includes impressed joint force in J_JT direction, + /// as well as constraint force, + /// in body-fixed frame + vec3 m_force_at_joint; + /// Moment acting at the joint when the body is cut from its parent; + /// includes impressed joint moment in J_JR direction, and constraint moment + /// in body-fixed frame + vec3 m_moment_at_joint; + /// external (user provided) force acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_force_user; + /// external (user provided) moment acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_moment_user; + // 3 absolute kinematic properties + /// Position of body-fixed frame relative to world frame + /// this is currently only for debugging purposes + vec3 m_body_pos; + /// Absolute velocity of body-fixed frame + vec3 m_body_vel; + /// Absolute acceleration of body-fixed frame + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_acc; + /// Absolute angular velocity + vec3 m_body_ang_vel; + /// Absolute angular acceleration + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_ang_acc; - // 4 relative kinematic properties. - // these are in the parent body frame - /// Transform from world to body-fixed frame; - /// this is currently only for debugging purposes - mat33 m_body_T_world; - /// Transform from parent to body-fixed frame - mat33 m_body_T_parent; - /// Vector from parent to child frame in parent frame - vec3 m_parent_pos_parent_body; - /// Relative angular velocity - vec3 m_body_ang_vel_rel; - /// Relative linear velocity - vec3 m_parent_vel_rel; - /// Relative angular acceleration - vec3 m_body_ang_acc_rel; - /// Relative linear acceleration - vec3 m_parent_acc_rel; + // 4 relative kinematic properties. + // these are in the parent body frame + /// Transform from world to body-fixed frame; + /// this is currently only for debugging purposes + mat33 m_body_T_world; + /// Transform from parent to body-fixed frame + mat33 m_body_T_parent; + /// Vector from parent to child frame in parent frame + vec3 m_parent_pos_parent_body; + /// Relative angular velocity + vec3 m_body_ang_vel_rel; + /// Relative linear velocity + vec3 m_parent_vel_rel; + /// Relative angular acceleration + vec3 m_body_ang_acc_rel; + /// Relative linear acceleration + vec3 m_parent_acc_rel; - // 5 Data describing the joint type and geometry - /// Type of joint - JointType m_joint_type; - /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame - /// Components are in body-fixed frame of the parent - vec3 m_parent_pos_parent_body_ref; - /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame - mat33 m_body_T_parent_ref; - /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute - /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. - /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. - /// (NOTE: dimensions will have to be dynamic for additional joint types!) - vec3 m_Jac_JR; - /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute - /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. - /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. - /// (NOTE: dimensions might have to be dynamic for additional joint types!) - vec3 m_Jac_JT; - /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT - vec3 m_parent_Jac_JT; - /// Start of index range for the position degree(s) of freedom describing this body's motion - /// relative to - /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. - int m_q_index; + // 5 Data describing the joint type and geometry + /// Type of joint + JointType m_joint_type; + /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame + /// Components are in body-fixed frame of the parent + vec3 m_parent_pos_parent_body_ref; + /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame + mat33 m_body_T_parent_ref; + /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. + /// (NOTE: dimensions will have to be dynamic for additional joint types!) + vec3 m_Jac_JR; + /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. + /// (NOTE: dimensions might have to be dynamic for additional joint types!) + vec3 m_Jac_JT; + /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT + vec3 m_parent_Jac_JT; + /// Start of index range for the position degree(s) of freedom describing this body's motion + /// relative to + /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. + int m_q_index; - // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" - /// mass of the subtree rooted in this body - idScalar m_subtree_mass; - /// center of mass * mass for subtree rooted in this body, in body-fixed frame - vec3 m_body_subtree_mass_com; - /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame - mat33 m_body_subtree_I_body; + // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" + /// mass of the subtree rooted in this body + idScalar m_subtree_mass; + /// center of mass * mass for subtree rooted in this body, in body-fixed frame + vec3 m_body_subtree_mass_com; + /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame + mat33 m_body_subtree_I_body; }; /// The MBS implements a tree structured multibody system class MultiBodyTree::MultiBodyImpl { - friend class MultiBodyTree; + friend class MultiBodyTree; public: - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); - /// constructor - /// @param num_bodies the number of bodies in the system - /// @param num_dofs number of degrees of freedom in the system - MultiBodyImpl(int num_bodies_, int num_dofs_); + /// constructor + /// @param num_bodies the number of bodies in the system + /// @param num_dofs number of degrees of freedom in the system + MultiBodyImpl(int num_bodies_, int num_dofs_); - /// \copydoc MultiBodyTree::calculateInverseDynamics - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - ///\copydoc MultiBodyTree::calculateMassMatrix - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); - /// generate additional index sets from the parent_index array - /// @return -1 on error, 0 on success - int generateIndexSets(); - /// set gravity acceleration in world frame - /// @param gravity gravity vector in the world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// pretty print tree - void printTree(); - /// print tree data - void printTreeData(); - /// initialize fixed data - void calculateStaticData(); - /// \copydoc MultiBodyTree::getBodyFrame - int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; - /// \copydoc MultiBodyTree::getParentIndex - int getParentIndex(const int body_index, int* m_parent_index); - /// \copydoc MultiBodyTree::getJointType - 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:getDoFOffset - int getDoFOffset(const int body_index, int* q_index) const; - /// \copydoc MultiBodyTree::getBodyOrigin - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// \copydoc MultiBodyTree::getBodyCoM - int getBodyCoM(const int body_index, vec3* world_com) const; - /// \copydoc MultiBodyTree::getBodyTransform - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// \copydoc MultiBodyTree::getBodyAngularVelocity - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// \copydoc MultiBodyTree::getBodyLinearVelocity - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// \copydoc MultiBodyTree::getBodyAngularAcceleration - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// \copydoc MultiBodyTree::getBodyLinearAcceleration - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - /// \copydoc MultiBodyTree::getUserInt - int getUserInt(const int body_index, int* user_int) const; - /// \copydoc MultiBodyTree::getUserPtr - int getUserPtr(const int body_index, void** user_ptr) const; - /// \copydoc MultiBodyTree::setUserInt - int setUserInt(const int body_index, const int user_int); - /// \copydoc MultiBodyTree::setUserPtr - int setUserPtr(const int body_index, void* const user_ptr); - ///\copydoc MultiBodytTree::setBodyMass - int setBodyMass(const int body_index, const idScalar mass); - ///\copydoc MultiBodytTree::setBodyFirstMassMoment - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - ///\copydoc MultiBodytTree::setBodySecondMassMoment - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - ///\copydoc MultiBodytTree::getBodyMass - int getBodyMass(const int body_index, idScalar* mass) const; - ///\copydoc MultiBodytTree::getBodyFirstMassMoment - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - ///\copydoc MultiBodytTree::getBodySecondMassMoment - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments - void clearAllUserForcesAndMoments(); - /// \copydoc MultiBodyTree::addUserForce - int addUserForce(const int body_index, const vec3& body_force); - /// \copydoc MultiBodyTree::addUserMoment - int addUserMoment(const int body_index, const vec3& body_moment); + /// \copydoc MultiBodyTree::calculateInverseDynamics + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + ///\copydoc MultiBodyTree::calculateMassMatrix + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + /// generate additional index sets from the parent_index array + /// @return -1 on error, 0 on success + int generateIndexSets(); + /// set gravity acceleration in world frame + /// @param gravity gravity vector in the world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// pretty print tree + void printTree(); + /// print tree data + void printTreeData(); + /// initialize fixed data + void calculateStaticData(); + /// \copydoc MultiBodyTree::getBodyFrame + int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; + /// \copydoc MultiBodyTree::getParentIndex + int getParentIndex(const int body_index, int* m_parent_index); + /// \copydoc MultiBodyTree::getJointType + 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:getDoFOffset + int getDoFOffset(const int body_index, int* q_index) const; + /// \copydoc MultiBodyTree::getBodyOrigin + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// \copydoc MultiBodyTree::getBodyCoM + int getBodyCoM(const int body_index, vec3* world_com) const; + /// \copydoc MultiBodyTree::getBodyTransform + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// \copydoc MultiBodyTree::getBodyAngularVelocity + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocity + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyAngularAcceleration + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearAcceleration + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// \copydoc MultiBodyTree::getUserInt + int getUserInt(const int body_index, int* user_int) const; + /// \copydoc MultiBodyTree::getUserPtr + int getUserPtr(const int body_index, void** user_ptr) const; + /// \copydoc MultiBodyTree::setUserInt + int setUserInt(const int body_index, const int user_int); + /// \copydoc MultiBodyTree::setUserPtr + int setUserPtr(const int body_index, void* const user_ptr); + ///\copydoc MultiBodytTree::setBodyMass + int setBodyMass(const int body_index, const idScalar mass); + ///\copydoc MultiBodytTree::setBodyFirstMassMoment + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); + ///\copydoc MultiBodytTree::setBodySecondMassMoment + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); + ///\copydoc MultiBodytTree::getBodyMass + int getBodyMass(const int body_index, idScalar* mass) const; + ///\copydoc MultiBodytTree::getBodyFirstMassMoment + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + ///\copydoc MultiBodytTree::getBodySecondMassMoment + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments + void clearAllUserForcesAndMoments(); + /// \copydoc MultiBodyTree::addUserForce + int addUserForce(const int body_index, const vec3& body_force); + /// \copydoc MultiBodyTree::addUserMoment + int addUserMoment(const int body_index, const vec3& body_moment); private: - // debug function. print tree structure to stdout - void printTree(int index, int indentation); - // get string representation of JointType (for debugging) - const char* jointTypeToString(const JointType& type) const; - // get number of degrees of freedom from joint type - int bodyNumDoFs(const JointType& type) const; - // number of bodies in the system - int m_num_bodies; - // number of degrees of freedom - int m_num_dofs; - // Gravitational acceleration (in world frame) - vec3 m_world_gravity; - // vector of bodies in the system - // body 0 is used as an environment body and is allways fixed. - // The bodies are ordered such that a parent body always has an index - // smaller than its child. - idArray::type m_body_list; - // Parent_index[i] is the index for i's parent body in body_list. - // This fully describes the tree. - idArray::type m_parent_index; - // child_indices[i] contains a vector of indices of - // all children of the i-th body - idArray::type>::type m_child_indices; - // Indices of rotary joints - idArray::type m_body_revolute_list; - // Indices of prismatic joints - idArray::type m_body_prismatic_list; - // Indices of floating joints - idArray::type m_body_floating_list; - // a user-provided integer - idArray::type m_user_int; - // a user-provided pointer - idArray::type m_user_ptr; + // debug function. print tree structure to stdout + void printTree(int index, int indentation); + // get string representation of JointType (for debugging) + const char* jointTypeToString(const JointType& type) const; + // get number of degrees of freedom from joint type + int bodyNumDoFs(const JointType& type) const; + // number of bodies in the system + int m_num_bodies; + // number of degrees of freedom + int m_num_dofs; + // Gravitational acceleration (in world frame) + vec3 m_world_gravity; + // vector of bodies in the system + // body 0 is used as an environment body and is allways fixed. + // The bodies are ordered such that a parent body always has an index + // smaller than its child. + idArray::type m_body_list; + // Parent_index[i] is the index for i's parent body in body_list. + // This fully describes the tree. + idArray::type m_parent_index; + // child_indices[i] contains a vector of indices of + // all children of the i-th body + idArray::type>::type m_child_indices; + // Indices of rotary joints + idArray::type m_body_revolute_list; + // Indices of prismatic joints + idArray::type m_body_prismatic_list; + // Indices of floating joints + idArray::type m_body_floating_list; + // a user-provided integer + idArray::type m_user_int; + // a user-provided pointer + idArray::type m_user_ptr; }; } #endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index 37063bfd7..47b4ab389 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -3,111 +3,111 @@ namespace btInverseDynamics { MultiBodyTree::InitCache::InitCache() { - m_inertias.resize(0); - m_joints.resize(0); - m_num_dofs = 0; - m_root_index=-1; + m_inertias.resize(0); + m_joints.resize(0); + m_num_dofs = 0; + m_root_index=-1; } int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, - const JointType joint_type, - const vec3& parent_r_parent_body_ref, - 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) { - case REVOLUTE: - case PRISMATIC: - m_num_dofs += 1; - break; - case FIXED: - // does not add a degree of freedom - // m_num_dofs+=0; - break; - case FLOATING: - m_num_dofs += 6; - break; - default: - error_message("unknown joint type %d\n", joint_type); - return -1; - } + const JointType joint_type, + const vec3& parent_r_parent_body_ref, + 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) { + case REVOLUTE: + case PRISMATIC: + m_num_dofs += 1; + break; + case FIXED: + // does not add a degree of freedom + // m_num_dofs+=0; + break; + case FLOATING: + m_num_dofs += 6; + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } - if(-1 == parent_index) { - if(m_root_index>=0) { - error_message("trying to add body %d as root, but already added %d as root body\n", - body_index, m_root_index); - return -1; - } - m_root_index=body_index; - } + if(-1 == parent_index) { + if(m_root_index>=0) { + error_message("trying to add body %d as root, but already added %d as root body\n", + body_index, m_root_index); + return -1; + } + m_root_index=body_index; + } - JointData joint; - joint.m_child = body_index; - joint.m_parent = parent_index; - joint.m_type = joint_type; - joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; - joint.m_child_T_parent_ref = body_T_parent_ref; - joint.m_child_axis_of_motion = body_axis_of_motion; + JointData joint; + joint.m_child = body_index; + joint.m_parent = parent_index; + joint.m_type = joint_type; + joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; + joint.m_child_T_parent_ref = body_T_parent_ref; + joint.m_child_axis_of_motion = body_axis_of_motion; - InertiaData body; - body.m_mass = mass; - body.m_body_pos_body_com = body_r_body_com; - body.m_body_I_body = body_I_body; + InertiaData body; + body.m_mass = mass; + body.m_body_pos_body_com = body_r_body_com; + body.m_body_I_body = body_I_body; - m_inertias.push_back(body); - m_joints.push_back(joint); - m_user_int.push_back(user_int); - m_user_ptr.push_back(user_ptr); - return 0; + m_inertias.push_back(body); + m_joints.push_back(joint); + m_user_int.push_back(user_int); + 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(m_inertias.size())) { - error_message("index out of range\n"); - return -1; - } + if (index < 0 || index > static_cast(m_inertias.size())) { + error_message("index out of range\n"); + return -1; + } - *inertia = m_inertias[index]; - return 0; + *inertia = m_inertias[index]; + return 0; } int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { - if (index < 0 || index > static_cast(m_user_int.size())) { - error_message("index out of range\n"); - return -1; - } - *user_int = m_user_int[index]; - return 0; + if (index < 0 || index > static_cast(m_user_int.size())) { + error_message("index out of range\n"); + return -1; + } + *user_int = m_user_int[index]; + return 0; } int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { - if (index < 0 || index > static_cast(m_user_ptr.size())) { - error_message("index out of range\n"); - return -1; - } - *user_ptr = m_user_ptr[index]; - return 0; + if (index < 0 || index > static_cast(m_user_ptr.size())) { + error_message("index out of range\n"); + return -1; + } + *user_ptr = m_user_ptr[index]; + return 0; } int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { - if (index < 0 || index > static_cast(m_joints.size())) { - error_message("index out of range\n"); - return -1; - } - *joint = m_joints[index]; - return 0; + if (index < 0 || index > static_cast(m_joints.size())) { + error_message("index out of range\n"); + return -1; + } + *joint = m_joints[index]; + return 0; } int MultiBodyTree::InitCache::buildIndexSets() { - // NOTE: This function assumes that proper indices were provided - // User2InternalIndex from utils can be used to facilitate this. + // 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++) { - const JointData& joint = m_joints[j]; - m_parent_index[joint.m_child] = joint.m_parent; - } + m_parent_index.resize(numBodies()); + 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; + return 0; } } diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp index a46cc16bb..0d2aa4a07 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -8,102 +8,102 @@ namespace btInverseDynamics { /// Mass properties of a rigid body struct InertiaData { - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); - /// mass - idScalar m_mass; - /// vector from body-fixed frame to center of mass, - /// in body-fixed frame, multiplied by the mass - vec3 m_body_pos_body_com; - /// moment of inertia w.r.t. the origin of the body-fixed - /// frame, represented in that frame - mat33 m_body_I_body; + /// mass + idScalar m_mass; + /// vector from body-fixed frame to center of mass, + /// in body-fixed frame, multiplied by the mass + vec3 m_body_pos_body_com; + /// moment of inertia w.r.t. the origin of the body-fixed + /// frame, represented in that frame + mat33 m_body_I_body; }; /// Joint properties struct JointData { - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); - /// type of joint - JointType m_type; - /// index of parent body - int m_parent; - /// index of child body - int m_child; - /// vector from parent's body-fixed frame to child's body-fixed - /// frame for q=0, written in the parent's body fixed frame - vec3 m_parent_pos_parent_child_ref; - /// Transform matrix converting vectors written in the parent's frame - /// into vectors written in the child's frame for q=0 - /// ie, child_vector = child_T_parent_ref * parent_vector; - mat33 m_child_T_parent_ref; - /// Axis of motion for 1 degree-of-freedom joints, - /// written in the child's frame - /// For revolute joints, the q-value is positive for a positive - /// rotation about this axis. - /// For prismatic joints, the q-value is positive for a positive - /// translation is this direction. - vec3 m_child_axis_of_motion; + /// type of joint + JointType m_type; + /// index of parent body + int m_parent; + /// index of child body + int m_child; + /// vector from parent's body-fixed frame to child's body-fixed + /// frame for q=0, written in the parent's body fixed frame + vec3 m_parent_pos_parent_child_ref; + /// Transform matrix converting vectors written in the parent's frame + /// into vectors written in the child's frame for q=0 + /// ie, child_vector = child_T_parent_ref * parent_vector; + mat33 m_child_T_parent_ref; + /// Axis of motion for 1 degree-of-freedom joints, + /// written in the child's frame + /// For revolute joints, the q-value is positive for a positive + /// rotation about this axis. + /// For prismatic joints, the q-value is positive for a positive + /// translation is this direction. + vec3 m_child_axis_of_motion; }; /// Data structure to store data passed by the user. /// This is used in MultiBodyTree::finalize to build internal data structures. class MultiBodyTree::InitCache { public: - ID_DECLARE_ALIGNED_ALLOCATOR(); - /// constructor - InitCache(); - ///\copydoc MultiBodyTree::addBody - int addBody(const int body_index, const int parent_index, const 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); - /// build index arrays - /// @return 0 on success, -1 on failure - int buildIndexSets(); - /// @return number of degrees of freedom - int numDoFs() const { return m_num_dofs; } - /// @return number of bodies - int numBodies() const { return m_inertias.size(); } - /// get inertia data for index - /// @param index of the body - /// @param inertia pointer for return data - /// @return 0 on success, -1 on failure - int getInertiaData(const int index, InertiaData *inertia) const; - /// get joint data for index - /// @param index of the body - /// @param joint pointer for return data - /// @return 0 on success, -1 on failure - int getJointData(const int index, JointData *joint) const; - /// get parent index array (paren_index[i] is the index of the parent of i) - /// @param parent_index pointer for return data - void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } - /// get user integer - /// @param index body index - /// @param user_int user integer - /// @return 0 on success, -1 on failure - int getUserInt(const int index, int *user_int) const; - /// get user pointer - /// @param index body index - /// @param user_int user pointer - /// @return 0 on success, -1 on failure - int getUserPtr(const int index, void **user_ptr) const; + ID_DECLARE_ALIGNED_ALLOCATOR(); + /// constructor + InitCache(); + ///\copydoc MultiBodyTree::addBody + int addBody(const int body_index, const int parent_index, const 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); + /// build index arrays + /// @return 0 on success, -1 on failure + int buildIndexSets(); + /// @return number of degrees of freedom + int numDoFs() const { return m_num_dofs; } + /// @return number of bodies + int numBodies() const { return m_inertias.size(); } + /// get inertia data for index + /// @param index of the body + /// @param inertia pointer for return data + /// @return 0 on success, -1 on failure + int getInertiaData(const int index, InertiaData *inertia) const; + /// get joint data for index + /// @param index of the body + /// @param joint pointer for return data + /// @return 0 on success, -1 on failure + int getJointData(const int index, JointData *joint) const; + /// get parent index array (paren_index[i] is the index of the parent of i) + /// @param parent_index pointer for return data + void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } + /// get user integer + /// @param index body index + /// @param user_int user integer + /// @return 0 on success, -1 on failure + int getUserInt(const int index, int *user_int) const; + /// get user pointer + /// @param index body index + /// @param user_int user pointer + /// @return 0 on success, -1 on failure + int getUserPtr(const int index, void **user_ptr) const; private: - // vector of bodies - idArray::type m_inertias; - // vector of joints - idArray::type m_joints; - // number of mechanical degrees of freedom - int m_num_dofs; - // parent index array - idArray::type m_parent_index; - // user integers - idArray::type m_user_int; - // user pointers - idArray::type m_user_ptr; - // index of root body (or -1 if not set) - int m_root_index; + // vector of bodies + idArray::type m_inertias; + // vector of joints + idArray::type m_joints; + // number of mechanical degrees of freedom + int m_num_dofs; + // parent index array + idArray::type m_parent_index; + // user integers + idArray::type m_user_int; + // user pointers + idArray::type m_user_ptr; + // index of root body (or -1 if not set) + int m_root_index; }; } #endif // MULTIBODYTREEINITCACHE_HPP_ diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp index 4ee728a0d..d067123e9 100644 --- a/test/InverseDynamics/test_invdyn_kinematics.cpp +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -248,7 +248,11 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar // first test: absolute difference between numerical and numerial // differentiation should be small TEST(InvDynKinematicsDifferentiation, errorAbsolute) { +#ifdef BT_ID_USE_DOUBLE_PRECISION const idScalar kDeltaT = 1e-7; +#else + const idScalar kDeltaT = 1e-2; +#endif const idScalar kDuration = 1e-2; const idScalar kAcceptableError = 1e-4;