diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics')
15 files changed, 1504 insertions, 970 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp index ebb10e7a16..b662b80152 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp @@ -32,10 +32,10 @@ #define BT_ID_POW(x, y) btPow(x, y) #define BT_ID_PI SIMD_PI #ifdef _WIN32 - #define BT_ID_SNPRINTF _snprintf +#define BT_ID_SNPRINTF _snprintf #else - #define BT_ID_SNPRINTF snprintf -#endif // +#define BT_ID_SNPRINTF snprintf +#endif // #endif // error messages #include "IDErrorMessages.hpp" @@ -52,8 +52,8 @@ #error "custom inverse dynamics config, but no custom namespace defined" #endif -#define BT_ID_MAX(a,b) std::max(a,b) -#define BT_ID_MIN(a,b) std::min(a,b) +#define BT_ID_MAX(a, b) std::max(a, b) +#define BT_ID_MIN(a, b) std::min(a, b) #else #define btInverseDynamics btInverseDynamicsBullet3 @@ -62,8 +62,8 @@ #include "LinearMath/btScalar.h" typedef btScalar idScalar; #include "LinearMath/btMinMax.h" -#define BT_ID_MAX(a,b) btMax(a,b) -#define BT_ID_MIN(a,b) btMin(a,b) +#define BT_ID_MAX(a, b) btMax(a, b) +#define BT_ID_MIN(a, b) btMin(a, b) #ifdef BT_USE_DOUBLE_PRECISION #define BT_ID_USE_DOUBLE_PRECISION @@ -71,31 +71,31 @@ typedef btScalar idScalar; #ifndef BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 - // use bullet types for arrays and array indices #include "Bullet3Common/b3AlignedObjectArray.h" // this is to make it work with C++2003, otherwise we could do this: // template <typename T> // using idArray = b3AlignedObjectArray<T>; template <typename T> -struct idArray { +struct idArray +{ typedef b3AlignedObjectArray<T> type; }; typedef int idArrayIdx; #define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR() -#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 +#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 #include "LinearMath/btAlignedObjectArray.h" template <typename T> -struct idArray { +struct idArray +{ typedef btAlignedObjectArray<T> type; }; typedef int idArrayIdx; #define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR() -#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 - +#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 // use bullet's allocator functions #define idMalloc btAllocFunc diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp index 130c19c6d6..6392367924 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -14,7 +14,8 @@ typedef float idScalar; // template <typename T> // using idArray = std::vector<T>; template <typename T> -struct idArray { +struct idArray +{ typedef std::vector<T> type; }; typedef std::vector<int>::size_type idArrayIdx; @@ -23,14 +24,14 @@ typedef std::vector<int>::size_type idArrayIdx; #define idMalloc ::malloc #define idFree ::free // currently not aligned at all... -#define ID_DECLARE_ALIGNED_ALLOCATOR() \ - inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete(void* ptr) { idFree(ptr); } \ - inline void* operator new(std::size_t, void* ptr) { return ptr; } \ - inline void operator delete(void*, void*) {} \ - inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete[](void* ptr) { idFree(ptr); } \ - inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ +#define ID_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete(void* ptr) { idFree(ptr); } \ + inline void* operator new(std::size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) {} \ + inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete[](void* ptr) { idFree(ptr); } \ + inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ inline void operator delete[](void*, void*) {} #include "details/IDMatVec.hpp" diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp index cbd7e8a9c4..cfb308ee55 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp @@ -15,7 +15,8 @@ typedef float idScalar; // template <typename T> // using idArray = std::vector<T>; template <typename T> -struct idArray { +struct idArray +{ typedef std::vector<T> type; }; typedef std::vector<int>::size_type idArrayIdx; diff --git a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp b/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp index 1b3fd268a0..5a98f01498 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp @@ -13,16 +13,18 @@ #else // BT_ID_WO_BULLET #include <cstdio> /// print error message with file/line information -#define bt_id_error_message(...) \ - do { \ - fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ +#define bt_id_error_message(...) \ + do \ + { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ } while (0) /// print warning message with file/line information -#define bt_id_warning_message(...) \ - do { \ - fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ +#define bt_id_warning_message(...) \ + do \ + { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ } while (0) #define id_printf(...) printf(__VA_ARGS__) #endif // BT_ID_WO_BULLET diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp index d279d3435c..2f120ed489 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp @@ -3,25 +3,30 @@ #include <cmath> #include <limits> -namespace btInverseDynamics { +namespace btInverseDynamics +{ static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon(); // requirements for axis length deviation from 1.0 // experimentally set from random euler angle rotation matrices static const idScalar kAxisLengthEpsilon = 10 * kIsZero; -void setZero(vec3 &v) { +void setZero(vec3 &v) +{ v(0) = 0; v(1) = 0; v(2) = 0; } -void setZero(vecx &v) { - for (int i = 0; i < v.size(); i++) { +void setZero(vecx &v) +{ + for (int i = 0; i < v.size(); i++) + { v(i) = 0; } } -void setZero(mat33 &m) { +void setZero(mat33 &m) +{ m(0, 0) = 0; m(0, 1) = 0; m(0, 2) = 0; @@ -33,7 +38,8 @@ void setZero(mat33 &m) { m(2, 2) = 0; } -void skew(vec3& v, mat33* result) { +void skew(vec3 &v, mat33 *result) +{ (*result)(0, 0) = 0.0; (*result)(0, 1) = -v(2); (*result)(0, 2) = v(1); @@ -45,22 +51,28 @@ void skew(vec3& v, mat33* result) { (*result)(2, 2) = 0.0; } -idScalar maxAbs(const vecx &v) { +idScalar maxAbs(const vecx &v) +{ idScalar result = 0.0; - for (int i = 0; i < v.size(); i++) { + for (int i = 0; i < v.size(); i++) + { const idScalar tmp = BT_ID_FABS(v(i)); - if (tmp > result) { + if (tmp > result) + { result = tmp; } } return result; } -idScalar maxAbs(const vec3 &v) { +idScalar maxAbs(const vec3 &v) +{ idScalar result = 0.0; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { const idScalar tmp = BT_ID_FABS(v(i)); - if (tmp > result) { + if (tmp > result) + { result = tmp; } } @@ -68,60 +80,75 @@ idScalar maxAbs(const vec3 &v) { } #if (defined BT_ID_HAVE_MAT3X) -idScalar maxAbsMat3x(const mat3x &m) { - // only used for tests -- so just loop here for portability - idScalar result = 0.0; - for (idArrayIdx col = 0; col < m.cols(); col++) { - for (idArrayIdx row = 0; row < 3; row++) { - result = BT_ID_MAX(result, std::fabs(m(row, col))); - } - } - return result; +idScalar maxAbsMat3x(const mat3x &m) +{ + // only used for tests -- so just loop here for portability + idScalar result = 0.0; + for (idArrayIdx col = 0; col < m.cols(); col++) + { + for (idArrayIdx row = 0; row < 3; row++) + { + result = BT_ID_MAX(result, std::fabs(m(row, col))); + } + } + return result; } -void mul(const mat33 &a, const mat3x &b, mat3x *result) { - if (b.cols() != result->cols()) { - bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n", - static_cast<int>(b.cols()), static_cast<int>(result->cols())); - abort(); - } - - for (idArrayIdx col = 0; col < b.cols(); col++) { - const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col); - const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col); - const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col); - setMat3xElem(0, col, x, result); - setMat3xElem(1, col, y, result); - setMat3xElem(2, col, z, result); - } +void mul(const mat33 &a, const mat3x &b, mat3x *result) +{ + if (b.cols() != result->cols()) + { + bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n", + static_cast<int>(b.cols()), static_cast<int>(result->cols())); + abort(); + } + + for (idArrayIdx col = 0; col < b.cols(); col++) + { + const idScalar x = a(0, 0) * b(0, col) + a(0, 1) * b(1, col) + a(0, 2) * b(2, col); + const idScalar y = a(1, 0) * b(0, col) + a(1, 1) * b(1, col) + a(1, 2) * b(2, col); + const idScalar z = a(2, 0) * b(0, col) + a(2, 1) * b(1, col) + a(2, 2) * b(2, col); + setMat3xElem(0, col, x, result); + setMat3xElem(1, col, y, result); + setMat3xElem(2, col, z, result); + } } -void add(const mat3x &a, const mat3x &b, mat3x *result) { - if (a.cols() != b.cols()) { - bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", - static_cast<int>(a.cols()), static_cast<int>(b.cols())); - abort(); - } - for (idArrayIdx col = 0; col < b.cols(); col++) { - for (idArrayIdx row = 0; row < 3; row++) { - setMat3xElem(row, col, a(row, col) + b(row, col), result); - } - } +void add(const mat3x &a, const mat3x &b, mat3x *result) +{ + if (a.cols() != b.cols()) + { + bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast<int>(a.cols()), static_cast<int>(b.cols())); + abort(); + } + for (idArrayIdx col = 0; col < b.cols(); col++) + { + for (idArrayIdx row = 0; row < 3; row++) + { + setMat3xElem(row, col, a(row, col) + b(row, col), result); + } + } } -void sub(const mat3x &a, const mat3x &b, mat3x *result) { - if (a.cols() != b.cols()) { - bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", - static_cast<int>(a.cols()), static_cast<int>(b.cols())); - abort(); - } - for (idArrayIdx col = 0; col < b.cols(); col++) { - for (idArrayIdx row = 0; row < 3; row++) { - setMat3xElem(row, col, a(row, col) - b(row, col), result); - } - } +void sub(const mat3x &a, const mat3x &b, mat3x *result) +{ + if (a.cols() != b.cols()) + { + bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast<int>(a.cols()), static_cast<int>(b.cols())); + abort(); + } + for (idArrayIdx col = 0; col < b.cols(); col++) + { + for (idArrayIdx row = 0; row < 3; row++) + { + setMat3xElem(row, col, a(row, col) - b(row, col), result); + } + } } #endif -mat33 transformX(const idScalar &alpha) { +mat33 transformX(const idScalar &alpha) +{ mat33 T; const idScalar cos_alpha = BT_ID_COS(alpha); const idScalar sin_alpha = BT_ID_SIN(alpha); @@ -143,7 +170,8 @@ mat33 transformX(const idScalar &alpha) { return T; } -mat33 transformY(const idScalar &beta) { +mat33 transformY(const idScalar &beta) +{ mat33 T; const idScalar cos_beta = BT_ID_COS(beta); const idScalar sin_beta = BT_ID_SIN(beta); @@ -165,7 +193,8 @@ mat33 transformY(const idScalar &beta) { return T; } -mat33 transformZ(const idScalar &gamma) { +mat33 transformZ(const idScalar &gamma) +{ mat33 T; const idScalar cos_gamma = BT_ID_COS(gamma); const idScalar sin_gamma = BT_ID_SIN(gamma); @@ -187,7 +216,8 @@ mat33 transformZ(const idScalar &gamma) { return T; } -mat33 tildeOperator(const vec3 &v) { +mat33 tildeOperator(const vec3 &v) +{ mat33 m; m(0, 0) = 0.0; m(0, 1) = -v(2); @@ -201,7 +231,8 @@ mat33 tildeOperator(const vec3 &v) { return m; } -void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { +void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) +{ const idScalar sa = BT_ID_SIN(alpha); const idScalar ca = BT_ID_COS(alpha); const idScalar st = BT_ID_SIN(theta); @@ -224,7 +255,8 @@ void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec (*T)(2, 2) = ca; } -void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { +void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) +{ const idScalar c = BT_ID_COS(angle); const idScalar s = -BT_ID_SIN(angle); const idScalar one_m_c = 1.0 - c; @@ -246,175 +278,214 @@ void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) (*T)(2, 2) = z * z * one_m_c + c; } -bool isPositiveDefinite(const mat33 &m) { +bool isPositiveDefinite(const mat33 &m) +{ // test if all upper left determinants are positive - if (m(0, 0) <= 0) { // upper 1x1 + if (m(0, 0) <= 0) + { // upper 1x1 return false; } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2 + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) + { // upper 2x2 return false; } if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) + { return false; } return true; } -bool isPositiveSemiDefinite(const mat33 &m) { +bool isPositiveSemiDefinite(const mat33 &m) +{ // test if all upper left determinants are positive - if (m(0, 0) < 0) { // upper 1x1 + if (m(0, 0) < 0) + { // upper 1x1 return false; } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2 + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) + { // upper 2x2 return false; } if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) + { return false; } return true; } -bool isPositiveSemiDefiniteFuzzy(const mat33 &m) { +bool isPositiveSemiDefiniteFuzzy(const mat33 &m) +{ // test if all upper left determinants are positive - if (m(0, 0) < -kIsZero) { // upper 1x1 + if (m(0, 0) < -kIsZero) + { // upper 1x1 return false; } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2 + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) + { // upper 2x2 return false; } if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) { + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) + { return false; } return true; } -idScalar determinant(const mat33 &m) { +idScalar determinant(const mat33 &m) +{ return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2); } -bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) { +bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) +{ // TODO(Thomas) do we really want this? // in cases where the inertia tensor about the center of mass is zero, // the determinant of the inertia tensor about the joint axis is almost // zero and can have a very small negative value. - if (!isPositiveSemiDefiniteFuzzy(I)) { - bt_id_error_message("invalid inertia matrix for body %d, not positive definite " - "(fixed joint)\n", - index); - bt_id_error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); + if (!isPositiveSemiDefiniteFuzzy(I)) + { + bt_id_error_message( + "invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + bt_id_error_message( + "matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); return false; } // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) - if (!has_fixed_joint) { - if (I(0, 0) + I(1, 1) < I(2, 2)) { + if (!has_fixed_joint) + { + if (I(0, 0) + I(1, 1) < I(2, 2)) + { bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); - bt_id_error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); + bt_id_error_message( + "matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); return false; } - if (I(0, 0) + I(1, 1) < I(2, 2)) { + if (I(0, 0) + I(1, 1) < I(2, 2)) + { bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); - bt_id_error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); + bt_id_error_message( + "matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); return false; } - if (I(1, 1) + I(2, 2) < I(0, 0)) { + if (I(1, 1) + I(2, 2) < I(0, 0)) + { bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); - bt_id_error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); + bt_id_error_message( + "matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); return false; } } // check positive/zero diagonal elements - for (int i = 0; i < 3; i++) { - if (I(i, i) < 0) { // accept zero + for (int i = 0; i < 3; i++) + { + if (I(i, i) < 0) + { // accept zero bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); return false; } } // check symmetry - if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) { - bt_id_error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " - "%e\n", - index, I(1, 0) - I(0, 1)); + if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) + { + bt_id_error_message( + "invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " + "%e\n", + index, I(1, 0) - I(0, 1)); return false; } - if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) { - bt_id_error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " - "%e\n", - index, I(2, 0) - I(0, 2)); + if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) + { + bt_id_error_message( + "invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " + "%e\n", + index, I(2, 0) - I(0, 2)); return false; } - if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) { + if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) + { bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, - I(1, 2) - I(2, 1)); + I(1, 2) - I(2, 1)); return false; } return true; } -bool isValidTransformMatrix(const mat33 &m) { -#define print_mat(x) \ - bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ - x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) +bool isValidTransformMatrix(const mat33 &m) +{ +#define print_mat(x) \ + bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) // check for unit length column vectors - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { const idScalar length_minus_1 = BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0); - if (length_minus_1 > kAxisLengthEpsilon) { - bt_id_error_message("Not a valid rotation matrix (column %d not unit length)\n" - "column = [%.18e %.18e %.18e]\n" - "length-1.0= %.18e\n", - i, m(0, i), m(1, i), m(2, i), length_minus_1); + if (length_minus_1 > kAxisLengthEpsilon) + { + bt_id_error_message( + "Not a valid rotation matrix (column %d not unit length)\n" + "column = [%.18e %.18e %.18e]\n" + "length-1.0= %.18e\n", + i, m(0, i), m(1, i), m(2, i), length_minus_1); print_mat(m); return false; } } // check for orthogonal column vectors - if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) { + if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) + { bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n"); print_mat(m); return false; } - if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) { + if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) + { bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); print_mat(m); return false; } - if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) { + if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) + { bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); print_mat(m); return false; } // check determinant (rotation not reflection) - if (determinant(m) <= 0) { + if (determinant(m) <= 0) + { bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n"); print_mat(m); return false; @@ -422,16 +493,18 @@ bool isValidTransformMatrix(const mat33 &m) { return true; } -bool isUnitVector(const vec3 &vector) { +bool isUnitVector(const vec3 &vector) +{ return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < kIsZero; } -vec3 rpyFromMatrix(const mat33 &rot) { +vec3 rpyFromMatrix(const mat33 &rot) +{ vec3 rpy; rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0)); rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0)); rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2)); return rpy; } -} +} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp index b355474d44..40bee5302b 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp @@ -5,7 +5,8 @@ #define IDMATH_HPP_ #include "IDConfig.hpp" -namespace btInverseDynamics { +namespace btInverseDynamics +{ /// set all elements to zero void setZero(vec3& v); /// set all elements to zero @@ -23,11 +24,11 @@ idScalar maxAbs(const vec3& v); #if (defined BT_ID_HAVE_MAT3X) idScalar maxAbsMat3x(const mat3x& m); -void setZero(mat3x&m); +void setZero(mat3x& m); // define math functions on mat3x here to avoid allocations in operators. -void mul(const mat33&a, const mat3x&b, mat3x* result); -void add(const mat3x&a, const mat3x&b, mat3x* result); -void sub(const mat3x&a, const mat3x&b, mat3x* result); +void mul(const mat33& a, const mat3x& b, mat3x* result); +void add(const mat3x& a, const mat3x& b, mat3x* result); +void sub(const mat3x& a, const mat3x& b, mat3x* result); #endif /// get offset vector & transform matrix from DH parameters @@ -94,6 +95,6 @@ mat33 transformZ(const idScalar& gamma); ///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix /// @param rot rotation matrix /// @returns x-y-z Euler angles -vec3 rpyFromMatrix(const mat33&rot); -} +vec3 rpyFromMatrix(const mat33& rot); +} // namespace btInverseDynamics #endif // IDMATH_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp index becfe0f4a2..f150b5ae4c 100644 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp @@ -8,69 +8,84 @@ #include "details/MultiBodyTreeImpl.hpp" #include "details/MultiBodyTreeInitCache.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ MultiBodyTree::MultiBodyTree() : m_is_finalized(false), m_mass_parameters_are_valid(true), m_accept_invalid_mass_parameters(false), m_impl(0x0), - m_init_cache(0x0) { + m_init_cache(0x0) +{ m_init_cache = new InitCache(); } -MultiBodyTree::~MultiBodyTree() { +MultiBodyTree::~MultiBodyTree() +{ delete m_impl; delete m_init_cache; } -void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { +void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) +{ m_accept_invalid_mass_parameters = flag; } -bool MultiBodyTree::getAcceptInvalidMassProperties() const { +bool MultiBodyTree::getAcceptInvalidMassProperties() const +{ return m_accept_invalid_mass_parameters; } -int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { +int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const +{ return m_impl->getBodyOrigin(body_index, world_origin); } -int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const { +int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const +{ return m_impl->getBodyCoM(body_index, world_com); } -int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const { +int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const +{ return m_impl->getBodyTransform(body_index, world_T_body); } -int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const { +int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const +{ return m_impl->getBodyAngularVelocity(body_index, world_omega); } -int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const { +int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const +{ return m_impl->getBodyLinearVelocity(body_index, world_velocity); } -int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const { +int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const +{ return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); } -int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const { +int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const +{ return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); } -int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const { +int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const +{ return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); } -int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const { - return m_impl->getParentRParentBodyRef(body_index, r); +int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const +{ + return m_impl->getParentRParentBodyRef(body_index, r); } -int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const { - return m_impl->getBodyTParentRef(body_index, T); +int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const +{ + return m_impl->getBodyTParentRef(body_index, T); } -int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const { - return m_impl->getBodyAxisOfMotion(body_index, axis); +int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const +{ + return m_impl->getBodyAxisOfMotion(body_index, axis); } void MultiBodyTree::printTree() { m_impl->printTree(); } @@ -81,12 +96,15 @@ int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; } int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; } int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u, - vecx *joint_forces) { - if (false == m_is_finalized) { + vecx *joint_forces) +{ + if (false == m_is_finalized) + { bt_id_error_message("system has not been initialized\n"); return -1; } - if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) + { bt_id_error_message("error in inverse dynamics calculation\n"); return -1; } @@ -95,141 +113,164 @@ int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, const bool initialize_matrix, - const bool set_lower_triangular_matrix, matxx *mass_matrix) { - if (false == m_is_finalized) { + const bool set_lower_triangular_matrix, matxx *mass_matrix) +{ + if (false == m_is_finalized) + { bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, - set_lower_triangular_matrix, mass_matrix)) { + set_lower_triangular_matrix, mass_matrix)) + { bt_id_error_message("error in mass matrix calculation\n"); return -1; } return 0; } -int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { +int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) +{ return calculateMassMatrix(q, true, true, true, mass_matrix); } +int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u) +{ + vec3 world_gravity(m_impl->m_world_gravity); + // temporarily set gravity to zero, to ensure we get the actual accelerations + setZero(m_impl->m_world_gravity); + if (false == m_is_finalized) + { + bt_id_error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, dot_u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) + { + bt_id_error_message("error in kinematics calculation\n"); + return -1; + } -int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) { - vec3 world_gravity(m_impl->m_world_gravity); - // temporarily set gravity to zero, to ensure we get the actual accelerations - setZero(m_impl->m_world_gravity); - - if (false == m_is_finalized) { - bt_id_error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateKinematics(q, u, dot_u, - MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) { - bt_id_error_message("error in kinematics calculation\n"); - return -1; - } - - m_impl->m_world_gravity=world_gravity; - return 0; + m_impl->m_world_gravity = world_gravity; + return 0; } - -int MultiBodyTree::calculatePositionKinematics(const vecx& q) { - if (false == m_is_finalized) { +int MultiBodyTree::calculatePositionKinematics(const vecx &q) +{ + if (false == m_is_finalized) + { bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateKinematics(q, q, q, - MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) + { bt_id_error_message("error in kinematics calculation\n"); return -1; } return 0; } -int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) { - if (false == m_is_finalized) { +int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u) +{ + if (false == m_is_finalized) + { bt_id_error_message("system has not been initialized\n"); return -1; } if (-1 == m_impl->calculateKinematics(q, u, u, - MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) + { bt_id_error_message("error in kinematics calculation\n"); return -1; } return 0; } - #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) -int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) { - if (false == m_is_finalized) { - bt_id_error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateJacobians(q, u, - MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { - bt_id_error_message("error in jacobian calculation\n"); - return -1; - } - return 0; +int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u) +{ + if (false == m_is_finalized) + { + bt_id_error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) + { + bt_id_error_message("error in jacobian calculation\n"); + return -1; + } + return 0; } -int MultiBodyTree::calculateJacobians(const vecx& q){ - if (false == m_is_finalized) { - bt_id_error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateJacobians(q, q, - MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) { - bt_id_error_message("error in jacobian calculation\n"); - return -1; - } - return 0; +int MultiBodyTree::calculateJacobians(const vecx &q) +{ + if (false == m_is_finalized) + { + bt_id_error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, q, + MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) + { + bt_id_error_message("error in jacobian calculation\n"); + return -1; + } + return 0; } -int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { - return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u); +int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const +{ + return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u); } -int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const { - return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u); +int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const +{ + return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u); } -int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const { - return m_impl->getBodyJacobianTrans(body_index,world_jac_trans); +int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const +{ + return m_impl->getBodyJacobianTrans(body_index, world_jac_trans); } -int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const { - return m_impl->getBodyJacobianRot(body_index,world_jac_rot); +int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const +{ + return m_impl->getBodyJacobianRot(body_index, world_jac_rot); } - #endif int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type, const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, const vec3 &body_axis_of_motion_, idScalar mass, const vec3 &body_r_body_com, const mat33 &body_I_body, - const int user_int, void *user_ptr) { - if (body_index < 0) { + const int user_int, void *user_ptr) +{ + if (body_index < 0) + { bt_id_error_message("body index must be positive (got %d)\n", body_index); return -1; } vec3 body_axis_of_motion(body_axis_of_motion_); - switch (joint_type) { + switch (joint_type) + { case REVOLUTE: case PRISMATIC: // check if axis is unit vector - if (!isUnitVector(body_axis_of_motion)) { + if (!isUnitVector(body_axis_of_motion)) + { bt_id_warning_message( "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) + - BT_ID_POW(body_axis_of_motion(1), 2) + - BT_ID_POW(body_axis_of_motion(2), 2)); - if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) { + BT_ID_POW(body_axis_of_motion(1), 2) + + BT_ID_POW(body_axis_of_motion(2), 2)); + if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) + { bt_id_error_message("axis of motion vector too short (%e)\n", length); return -1; } @@ -240,29 +281,36 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ break; case FLOATING: break; + case SPHERICAL: + break; default: bt_id_error_message("unknown joint type %d\n", joint_type); return -1; } // sanity check for mass properties. Zero mass is OK. - if (mass < 0) { + if (mass < 0) + { m_mass_parameters_are_valid = false; bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass); - if (!m_accept_invalid_mass_parameters) { + if (!m_accept_invalid_mass_parameters) + { return -1; } } - if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) { + if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) + { m_mass_parameters_are_valid = false; // error message printed in function call - if (!m_accept_invalid_mass_parameters) { + if (!m_accept_invalid_mass_parameters) + { return -1; } } - if (!isValidTransformMatrix(body_T_parent_ref)) { + if (!isValidTransformMatrix(body_T_parent_ref)) + { return -1; } @@ -271,52 +319,63 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ body_I_body, user_int, user_ptr); } -int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { +int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const +{ return m_impl->getParentIndex(body_index, parent_index); } -int MultiBodyTree::getUserInt(const int body_index, int *user_int) const { +int MultiBodyTree::getUserInt(const int body_index, int *user_int) const +{ return m_impl->getUserInt(body_index, user_int); } -int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const { +int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const +{ return m_impl->getUserPtr(body_index, user_ptr); } -int MultiBodyTree::setUserInt(const int body_index, const int user_int) { +int MultiBodyTree::setUserInt(const int body_index, const int user_int) +{ return m_impl->setUserInt(body_index, user_int); } -int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) { +int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) +{ return m_impl->setUserPtr(body_index, user_ptr); } -int MultiBodyTree::finalize() { +int MultiBodyTree::finalize() +{ const int &num_bodies = m_init_cache->numBodies(); const int &num_dofs = m_init_cache->numDoFs(); - if(num_dofs<=0) { - bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); - //return -1; - } + if (num_dofs <= 0) + { + bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); + //return -1; + } // 1 allocate internal MultiBody structure m_impl = new MultiBodyImpl(num_bodies, num_dofs); // 2 build new index set assuring index(parent) < index(child) - if (-1 == m_init_cache->buildIndexSets()) { + if (-1 == m_init_cache->buildIndexSets()) + { return -1; } m_init_cache->getParentIndexArray(&m_impl->m_parent_index); // 3 setup internal kinematic and dynamic data - for (int index = 0; index < num_bodies; index++) { + for (int index = 0; index < num_bodies; index++) + { InertiaData inertia; JointData joint; - if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + if (-1 == m_init_cache->getInertiaData(index, &inertia)) + { return -1; } - if (-1 == m_init_cache->getJointData(index, &joint)) { + if (-1 == m_init_cache->getJointData(index, &joint)) + { return -1; } @@ -332,24 +391,29 @@ int MultiBodyTree::finalize() { rigid_body.m_joint_type = joint.m_type; int user_int; - if (-1 == m_init_cache->getUserInt(index, &user_int)) { + if (-1 == m_init_cache->getUserInt(index, &user_int)) + { return -1; } - if (-1 == m_impl->setUserInt(index, user_int)) { + if (-1 == m_impl->setUserInt(index, user_int)) + { return -1; } - void* user_ptr; - if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) { + void *user_ptr; + if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) + { return -1; } - if (-1 == m_impl->setUserPtr(index, user_ptr)) { + if (-1 == m_impl->setUserPtr(index, user_ptr)) + { return -1; } // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized // matrices. - switch (rigid_body.m_joint_type) { + switch (rigid_body.m_joint_type) + { case REVOLUTE: rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); @@ -375,6 +439,16 @@ int MultiBodyTree::finalize() { rigid_body.m_Jac_JT(1) = 0.0; rigid_body.m_Jac_JT(2) = 0.0; break; + case SPHERICAL: + // 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; case FLOATING: // NOTE/TODO: this is not really correct. // the Jacobians should be 3x3 matrices here ! @@ -392,7 +466,8 @@ int MultiBodyTree::finalize() { } // 4 assign degree of freedom indices & build per-joint-type index arrays - if (-1 == m_impl->generateIndexSets()) { + if (-1 == m_impl->generateIndexSets()) + { bt_id_error_message("generating index sets\n"); return -1; } @@ -408,54 +483,66 @@ int MultiBodyTree::finalize() { return 0; } -int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { +int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) +{ return m_impl->setGravityInWorldFrame(gravity); } -int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { +int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const +{ return m_impl->getJointType(body_index, joint_type); } -int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const { +int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const +{ return m_impl->getJointTypeStr(body_index, joint_type); } -int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const { +int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const +{ return m_impl->getDoFOffset(body_index, q_offset); } -int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { +int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) +{ return m_impl->setBodyMass(body_index, mass); } -int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) { +int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment) +{ return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); } -int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) { +int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment) +{ return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); } -int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const { +int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const +{ return m_impl->getBodyMass(body_index, mass); } -int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const { +int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const +{ return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); } -int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const { +int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const +{ return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); } void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); } -int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) { +int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) +{ return m_impl->addUserForce(body_index, body_force); } -int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) { +int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) +{ return m_impl->addUserMoment(body_index, body_moment); } -} +} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp index d235aa6e76..7b852f976c 100644 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp @@ -4,10 +4,11 @@ #include "IDConfig.hpp" #include "IDMath.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ /// Enumeration of supported joint types -enum JointType { +enum JointType +{ /// no degree of freedom, moves with parent FIXED = 0, /// one rotational degree of freedom relative to parent @@ -15,7 +16,9 @@ enum JointType { /// one translational degree of freedom relative to parent PRISMATIC, /// six degrees of freedom relative to parent - FLOATING + FLOATING, + /// three degrees of freedom, relative to parent + SPHERICAL }; /// Interface class for calculating inverse dynamics for tree structured @@ -30,12 +33,14 @@ enum JointType { /// - PRISMATIC: displacement [m] /// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] /// (in that order) +/// - SPHERICAL: Euler x-y-z angles [rad] /// 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] +// - SPHERICAL: angular velocity [rad/s] /// /// The q and u vectors are obtained by stacking contributions of all bodies in one /// vector in the order of body indices. @@ -46,12 +51,13 @@ enum JointType { /// - PRISMATIC: force [N], along joint axis /// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame /// (in that order) -/// +/// - SPHERICAL: moment vector [Nm] /// TODO - force element interface (friction, springs, dampers, etc) /// - gears and motor inertia -class MultiBodyTree { +class MultiBodyTree +{ public: - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); /// The contructor. /// Initialization & allocation is via addBody and buildSystem calls. MultiBodyTree(); @@ -119,9 +125,9 @@ public: /// print tree data to stdout void printTreeData(); /// Calculate joint forces for given generalized state & derivatives. - /// This also updates kinematic terms computed in calculateKinematics. - /// If gravity is not set to zero, acceleration terms will contain - /// gravitational acceleration. + /// This also updates kinematic terms computed in calculateKinematics. + /// If gravity is not set to zero, acceleration terms will contain + /// gravitational acceleration. /// @param q generalized coordinates /// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u) /// @param dot_u time derivative of u @@ -152,30 +158,28 @@ public: /// @return -1 on error, 0 on success int calculateMassMatrix(const vecx& q, matxx* mass_matrix); - - /// Calculates kinematics also calculated in calculateInverseDynamics, - /// but not dynamics. - /// This function ensures that correct accelerations are computed that do not - /// contain gravitational acceleration terms. - /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations) - int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u); - /// Calculate position kinematics - int calculatePositionKinematics(const vecx& q); - /// Calculate position and velocity kinematics - int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u); + /// Calculates kinematics also calculated in calculateInverseDynamics, + /// but not dynamics. + /// This function ensures that correct accelerations are computed that do not + /// contain gravitational acceleration terms. + /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations) + int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u); + /// Calculate position kinematics + int calculatePositionKinematics(const vecx& q); + /// Calculate position and velocity kinematics + int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u); #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components - /// d(Jacobian)/dt*u - /// This function assumes that calculateInverseDynamics was called, or calculateKinematics, - /// or calculatePositionAndVelocityKinematics - int calculateJacobians(const vecx& q, const vecx& u); - /// Calculate Jacobians (dvel/du) - /// This function assumes that calculateInverseDynamics was called, or - /// one of the calculateKineamtics functions - int calculateJacobians(const vecx& q); -#endif // BT_ID_HAVE_MAT3X - + /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components + /// d(Jacobian)/dt*u + /// This function assumes that calculateInverseDynamics was called, or calculateKinematics, + /// or calculatePositionAndVelocityKinematics + int calculateJacobians(const vecx& q, const vecx& u); + /// Calculate Jacobians (dvel/du) + /// This function assumes that calculateInverseDynamics was called, or + /// one of the calculateKineamtics functions + int calculateJacobians(const vecx& q); +#endif // BT_ID_HAVE_MAT3X /// set gravitational acceleration /// the default is [0;0;-9.8] in the world frame @@ -231,15 +235,15 @@ public: int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - // get translational jacobian, in world frame (dworld_velocity/du) - int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const; - // get rotational jacobian, in world frame (dworld_omega/du) - int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; - // get product of translational jacobian derivative * generatlized velocities - int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const; - // get product of rotational jacobian derivative * generatlized velocities - int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; -#endif // BT_ID_HAVE_MAT3X + // get translational jacobian, in world frame (dworld_velocity/du) + int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const; + // get rotational jacobian, in world frame (dworld_omega/du) + int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; + // get product of translational jacobian derivative * generatlized velocities + int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const; + // get product of rotational jacobian derivative * generatlized velocities + int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; +#endif // BT_ID_HAVE_MAT3X /// returns the (internal) index of body /// @param body_index is the index of a body @@ -256,21 +260,21 @@ public: /// @param joint_type string naming the corresponding joint type /// @return 0 on success, -1 on failure int getJointTypeStr(const int body_index, const char** joint_type) const; - /// get offset translation to parent body (see addBody) + /// get offset translation to parent body (see addBody) /// @param body_index index of the body /// @param r the offset translation (see above) /// @return 0 on success, -1 on failure - int getParentRParentBodyRef(const int body_index, vec3* r) const; + int getParentRParentBodyRef(const int body_index, vec3* r) const; /// get offset rotation to parent body (see addBody) /// @param body_index index of the body /// @param T the transform (see above) /// @return 0 on success, -1 on failure - int getBodyTParentRef(const int body_index, mat33* T) const; + int getBodyTParentRef(const int body_index, mat33* T) const; /// get axis of motion (see addBody) /// @param body_index index of the body /// @param axis the axis (see above) /// @return 0 on success, -1 on failure - int getBodyAxisOfMotion(const int body_index, vec3* axis) const; + int getBodyAxisOfMotion(const int body_index, vec3* axis) const; /// get offset for degrees of freedom of this body into the q-vector /// @param body_index index of the body /// @param q_offset offset the q vector diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp index 836395cea2..fe4f102513 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -1,8 +1,8 @@ #ifndef INVDYNEIGENINTERFACE_HPP_ #define INVDYNEIGENINTERFACE_HPP_ #include "../IDConfig.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ #define BT_ID_HAVE_MAT3X #ifdef BT_USE_DOUBLE_PRECISION @@ -19,18 +19,21 @@ typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> m typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x; #endif -inline void resize(mat3x &m, Eigen::Index size) { - m.resize(3, size); - m.setZero(); +inline void resize(mat3x &m, Eigen::Index size) +{ + m.resize(3, size); + m.setZero(); } -inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ - (*m)(row, col) = val; +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx *m) +{ + (*m)(row, col) = val; } -inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ - (*m)(row, col) = val; +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x *m) +{ + (*m)(row, col) = val; } -} +} // namespace btInverseDynamics #endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp index c179daeec6..0c398a3727 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -10,32 +10,37 @@ #include "../../LinearMath/btMatrixX.h" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; typedef btMatrixX<idScalar> matxx; -class vec3 : public btVector3 { +class vec3 : public btVector3 +{ public: vec3() : btVector3() {} vec3(const btVector3& btv) { *this = btv; } idScalar& operator()(int i) { return (*this)[i]; } const idScalar& operator()(int i) const { return (*this)[i]; } int size() const { return 3; } - const vec3& operator=(const btVector3& rhs) { + const vec3& operator=(const btVector3& rhs) + { *static_cast<btVector3*>(this) = rhs; return *this; } }; -class mat33 : public btMatrix3x3 { +class mat33 : public btMatrix3x3 +{ public: mat33() : btMatrix3x3() {} mat33(const btMatrix3x3& btm) { *this = btm; } idScalar& operator()(int i, int j) { return (*this)[i][j]; } const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } - const mat33& operator=(const btMatrix3x3& rhs) { + const mat33& operator=(const btMatrix3x3& rhs) + { *static_cast<btMatrix3x3*>(this) = rhs; return *this; } @@ -47,10 +52,12 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s) inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -class vecx : public btVectorX<idScalar> { +class vecx : public btVectorX<idScalar> +{ public: vecx(int size) : btVectorX<idScalar>(size) {} - const vecx& operator=(const btVectorX<idScalar>& rhs) { + const vecx& operator=(const btVectorX<idScalar>& rhs) + { *static_cast<btVectorX<idScalar>*>(this) = rhs; return *this; } @@ -66,43 +73,53 @@ public: friend vecx operator/(const vecx& a, const idScalar& s); }; -inline vecx operator*(const vecx& a, const idScalar& s) { +inline vecx operator*(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) * s; } return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) { +inline vecx operator+(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result(i) = a(i) + b(i); } return result; } -inline vecx operator-(const vecx& a, const vecx& b) { +inline vecx operator-(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result(i) = a(i) - b(i); } return result; } -inline vecx operator/(const vecx& a, const idScalar& s) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) / s; } @@ -110,63 +127,76 @@ inline vecx operator/(const vecx& a, const idScalar& s) { } // use btMatrixX to implement 3xX matrix -class mat3x : public matxx { +class mat3x : public matxx +{ public: - mat3x(){} - mat3x(const mat3x&rhs) { - matxx::resize(rhs.rows(), rhs.cols()); - *this = rhs; - } - mat3x(int rows, int cols): matxx(3,cols) { - } - void operator=(const mat3x& rhs) { - if (m_cols != rhs.m_cols) { - bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); - abort(); + mat3x() {} + mat3x(const mat3x& rhs) + { + matxx::resize(rhs.rows(), rhs.cols()); + *this = rhs; + } + mat3x(int rows, int cols) : matxx(3, cols) + { + } + void operator=(const mat3x& rhs) + { + if (m_cols != rhs.m_cols) + { + bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for (int i = 0; i < rows(); i++) + { + for (int k = 0; k < cols(); k++) + { + setElem(i, k, rhs(i, k)); + } + } + } + void setZero() + { + matxx::setZero(); } - for(int i=0;i<rows();i++) { - for(int k=0;k<cols();k++) { - setElem(i,k,rhs(i,k)); - } - } - } - void setZero() { - matxx::setZero(); - } }; - -inline vec3 operator*(const mat3x& a, const vecx& b) { - vec3 result; - if (a.cols() != b.size()) { - bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); - abort(); - } - result(0)=0.0; - result(1)=0.0; - result(2)=0.0; - for(int i=0;i<b.size();i++) { - for(int k=0;k<3;k++) { - result(k)+=a(k,i)*b(i); - } - } - return result; +inline vec3 operator*(const mat3x& a, const vecx& b) +{ + vec3 result; + if (a.cols() != b.size()) + { + bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); + abort(); + } + result(0) = 0.0; + result(1) = 0.0; + result(2) = 0.0; + for (int i = 0; i < b.size(); i++) + { + for (int k = 0; k < 3; k++) + { + result(k) += a(k, i) * b(i); + } + } + return result; } - -inline void resize(mat3x &m, idArrayIdx size) { - m.resize(3, size); - m.setZero(); +inline void resize(mat3x& m, idArrayIdx size) +{ + m.resize(3, size); + m.setZero(); } -inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ - m->setElem(row, col, val); +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m) +{ + m->setElem(row, col, val); } -inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ - m->setElem(row, col, val); +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) +{ + m->setElem(row, col, val); } -} +} // namespace btInverseDynamics #endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp index c89db5e123..1c786095e7 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp @@ -7,7 +7,8 @@ #include "../IDConfig.hpp" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; @@ -17,7 +18,8 @@ class mat3x; /// This is a very basic implementation to enable stand-alone use of the library. /// The implementation is not really optimized and misses many features that you would /// want from a "fully featured" linear math library. -class vec3 { +class vec3 +{ public: idScalar& operator()(int i) { return m_data[i]; } const idScalar& operator()(int i) const { return m_data[i]; } @@ -40,7 +42,8 @@ private: idScalar m_data[3]; }; -class mat33 { +class mat33 +{ public: idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } @@ -62,9 +65,11 @@ private: idScalar m_data[9]; }; -class vecx { +class vecx +{ public: - vecx(int size) : m_size(size) { + vecx(int size) : m_size(size) + { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size)); } ~vecx() { idFree(m_data); } @@ -85,14 +90,17 @@ private: idScalar* m_data; }; -class matxx { +class matxx +{ public: - matxx() { - m_data = 0x0; - m_cols=0; - m_rows=0; - } - matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + matxx() + { + m_data = 0x0; + m_cols = 0; + m_rows = 0; + } + matxx(int rows, int cols) : m_rows(rows), m_cols(cols) + { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols)); } ~matxx() { idFree(m_data); } @@ -107,69 +115,86 @@ private: idScalar* m_data; }; -class mat3x { +class mat3x +{ public: - mat3x() { - m_data = 0x0; - m_cols=0; - } - mat3x(const mat3x&rhs) { - m_cols=rhs.m_cols; - allocate(); - *this = rhs; - } - mat3x(int rows, int cols): m_cols(cols) { - allocate(); - }; - void operator=(const mat3x& rhs) { - if (m_cols != rhs.m_cols) { - bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); - abort(); - } - for(int i=0;i<3*m_cols;i++) { - m_data[i] = rhs.m_data[i]; - } - } - - ~mat3x() { - free(); - } - idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; } - const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; } - int rows() const { return m_rows; } - const int& cols() const { return m_cols; } - void resize(int rows, int cols) { - m_cols=cols; - free(); - allocate(); - } - void setZero() { - memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols); - } - // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead + mat3x() + { + m_data = 0x0; + m_cols = 0; + } + mat3x(const mat3x& rhs) + { + m_cols = rhs.m_cols; + allocate(); + *this = rhs; + } + mat3x(int rows, int cols) : m_cols(cols) + { + allocate(); + }; + void operator=(const mat3x& rhs) + { + if (m_cols != rhs.m_cols) + { + bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for (int i = 0; i < 3 * m_cols; i++) + { + m_data[i] = rhs.m_data[i]; + } + } + + ~mat3x() + { + free(); + } + idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; } + int rows() const { return m_rows; } + const int& cols() const { return m_cols; } + void resize(int rows, int cols) + { + m_cols = cols; + free(); + allocate(); + } + void setZero() + { + memset(m_data, 0x0, sizeof(idScalar) * m_rows * m_cols); + } + // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead private: - void allocate(){m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));} - void free() { idFree(m_data);} - enum {m_rows=3}; - int m_cols; - idScalar* m_data; + void allocate() { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols)); } + void free() { idFree(m_data); } + enum + { + m_rows = 3 + }; + int m_cols; + idScalar* m_data; }; -inline void resize(mat3x &m, idArrayIdx size) { - m.resize(3, size); - m.setZero(); +inline void resize(mat3x& m, idArrayIdx size) +{ + m.resize(3, size); + m.setZero(); } ////////////////////////////////////////////////// // Implementations -inline const vec3& vec3::operator=(const vec3& rhs) { - if (&rhs != this) { +inline const vec3& vec3::operator=(const vec3& rhs) +{ + if (&rhs != this) + { memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); } return *this; } -inline vec3 vec3::cross(const vec3& b) const { +inline vec3 vec3::cross(const vec3& b) const +{ vec3 result; result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; @@ -178,17 +203,21 @@ inline vec3 vec3::cross(const vec3& b) const { return result; } -inline idScalar vec3::dot(const vec3& b) const { +inline idScalar vec3::dot(const vec3& b) const +{ return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; } -inline const mat33& mat33::operator=(const mat33& rhs) { - if (&rhs != this) { +inline const mat33& mat33::operator=(const mat33& rhs) +{ + if (&rhs != this) + { memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); } return *this; } -inline mat33 mat33::transpose() const { +inline mat33 mat33::transpose() const +{ mat33 result; result.m_data[0] = m_data[0]; result.m_data[1] = m_data[3]; @@ -203,7 +232,8 @@ inline mat33 mat33::transpose() const { return result; } -inline mat33 operator*(const mat33& a, const mat33& b) { +inline mat33 operator*(const mat33& a, const mat33& b) +{ mat33 result; result.m_data[0] = a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; @@ -227,22 +257,27 @@ inline mat33 operator*(const mat33& a, const mat33& b) { return result; } -inline const mat33& mat33::operator+=(const mat33& b) { - for (int i = 0; i < 9; i++) { +inline const mat33& mat33::operator+=(const mat33& b) +{ + for (int i = 0; i < 9; i++) + { m_data[i] += b.m_data[i]; } return *this; } -inline const mat33& mat33::operator-=(const mat33& b) { - for (int i = 0; i < 9; i++) { +inline const mat33& mat33::operator-=(const mat33& b) +{ + for (int i = 0; i < 9; i++) + { m_data[i] -= b.m_data[i]; } return *this; } -inline vec3 operator*(const mat33& a, const vec3& b) { +inline vec3 operator*(const mat33& a, const vec3& b) +{ vec3 result; result.m_data[0] = @@ -255,23 +290,29 @@ inline vec3 operator*(const mat33& a, const vec3& b) { return result; } -inline const vec3& vec3::operator+=(const vec3& b) { - for (int i = 0; i < 3; i++) { +inline const vec3& vec3::operator+=(const vec3& b) +{ + for (int i = 0; i < 3; i++) + { m_data[i] += b.m_data[i]; } return *this; } -inline const vec3& vec3::operator-=(const vec3& b) { - for (int i = 0; i < 3; i++) { +inline const vec3& vec3::operator-=(const vec3& b) +{ + for (int i = 0; i < 3; i++) + { m_data[i] -= b.m_data[i]; } return *this; } -inline mat33 operator*(const mat33& a, const idScalar& s) { +inline mat33 operator*(const mat33& a, const idScalar& s) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] * s; } return result; @@ -279,137 +320,170 @@ inline mat33 operator*(const mat33& a, const idScalar& s) { inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -inline vec3 operator*(const vec3& a, const idScalar& s) { +inline vec3 operator*(const vec3& a, const idScalar& s) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] * s; } return result; } inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; } -inline mat33 operator+(const mat33& a, const mat33& b) { +inline mat33 operator+(const mat33& a, const mat33& b) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] + b.m_data[i]; } return result; } -inline vec3 operator+(const vec3& a, const vec3& b) { +inline vec3 operator+(const vec3& a, const vec3& b) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] + b.m_data[i]; } return result; } -inline mat33 operator-(const mat33& a, const mat33& b) { +inline mat33 operator-(const mat33& a, const mat33& b) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] - b.m_data[i]; } return result; } -inline vec3 operator-(const vec3& a, const vec3& b) { +inline vec3 operator-(const vec3& a, const vec3& b) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] - b.m_data[i]; } return result; } -inline mat33 operator/(const mat33& a, const idScalar& s) { +inline mat33 operator/(const mat33& a, const idScalar& s) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] / s; } return result; } -inline vec3 operator/(const vec3& a, const idScalar& s) { +inline vec3 operator/(const vec3& a, const idScalar& s) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] / s; } return result; } -inline const vecx& vecx::operator=(const vecx& rhs) { - if (size() != rhs.size()) { +inline const vecx& vecx::operator=(const vecx& rhs) +{ + if (size() != rhs.size()) + { bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); abort(); } - if (&rhs != this) { + if (&rhs != this) + { memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); } return *this; } -inline vecx operator*(const vecx& a, const idScalar& s) { +inline vecx operator*(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result.m_data[i] = a.m_data[i] * s; } return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) { +inline vecx operator+(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result.m_data[i] = a.m_data[i] + b.m_data[i]; } return result; } -inline vecx operator-(const vecx& a, const vecx& b) { +inline vecx operator-(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result.m_data[i] = a.m_data[i] - b.m_data[i]; } return result; } -inline vecx operator/(const vecx& a, const idScalar& s) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result.m_data[i] = a.m_data[i] / s; } return result; } -inline vec3 operator*(const mat3x& a, const vecx& b) { - vec3 result; - if (a.cols() != b.size()) { - bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); - abort(); - } - result(0)=0.0; - result(1)=0.0; - result(2)=0.0; - for(int i=0;i<b.size();i++) { - for(int k=0;k<3;k++) { - result(k)+=a(k,i)*b(i); - } - } - return result; +inline vec3 operator*(const mat3x& a, const vecx& b) +{ + vec3 result; + if (a.cols() != b.size()) + { + bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); + abort(); + } + result(0) = 0.0; + result(1) = 0.0; + result(2) = 0.0; + for (int i = 0; i < b.size(); i++) + { + for (int k = 0; k < 3; k++) + { + result(k) += a(k, i) * b(i); + } + } + return result; } -inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ - (*m)(row, col) = val; +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m) +{ + (*m)(row, col) = val; } -inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ - (*m)(row, col) = val; +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) +{ + (*m)(row, col) = val; } -} // namespace btInverseDynamcis +} // namespace btInverseDynamics #endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index e8563238c3..befbc2e2a4 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -1,16 +1,16 @@ #include "MultiBodyTreeImpl.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - ,m_m3x(3,m_num_dofs) + , + m_m3x(3, m_num_dofs) #endif { - #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - resize(m_m3x,m_num_dofs); + resize(m_m3x, m_num_dofs); #endif m_body_list.resize(num_bodies_); m_parent_index.resize(num_bodies_); @@ -23,8 +23,10 @@ MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) m_world_gravity(2) = -9.8; } -const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { - switch (type) { +const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const +{ + switch (type) + { case FIXED: return "fixed"; case REVOLUTE: @@ -33,22 +35,28 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ return "prismatic"; case FLOATING: return "floating"; + case SPHERICAL: + return "spherical"; } return "error: invalid"; } -inline void indent(const int &level) { +inline void indent(const int &level) +{ for (int j = 0; j < level; j++) id_printf(" "); // indent } -void MultiBodyTree::MultiBodyImpl::printTree() { +void MultiBodyTree::MultiBodyImpl::printTree() +{ 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++) { +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<int>(i)); id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); @@ -59,19 +67,22 @@ void MultiBodyTree::MultiBodyImpl::printTreeData() { 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( + "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)); } } -int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { - switch (type) { +int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const +{ + switch (type) + { case FIXED: return 0; case REVOLUTE: @@ -79,12 +90,15 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { return 1; case FLOATING: return 6; + case SPHERICAL: + return 3; } bt_id_error_message("unknown joint type %d\n", type); return 0; } -void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { +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(); @@ -92,7 +106,8 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { indentation += 2; int count = 0; - for (int i = 0; i < num_children; i++) { + 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, @@ -104,19 +119,23 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { } } -int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { +int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) +{ m_world_gravity = gravity; return 0; } -int MultiBodyTree::MultiBodyImpl::generateIndexSets() { +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++) { + 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) { + switch (body.m_joint_type) + { case REVOLUTE: m_body_revolute_list.push_back(i); body.m_q_index = q_index; @@ -135,28 +154,41 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { body.m_q_index = q_index; q_index += 6; break; + case SPHERICAL: + m_body_spherical_list.push_back(i); + body.m_q_index = q_index; + q_index += 3; + break; default: bt_id_error_message("unsupported joint type %d\n", body.m_joint_type); return -1; } } // sanity check - if (q_index != m_num_dofs) { + if (q_index != m_num_dofs) + { bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); return -1; } m_child_indices.resize(m_body_list.size()); - for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { + for (idArrayIdx child = 1; child < m_parent_index.size(); child++) + { const int &parent = m_parent_index[child]; - if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) { + if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) + { m_child_indices[parent].push_back(child); - } else { - if (-1 == parent) { + } + else + { + if (-1 == parent) + { // multiple bodies are directly linked to the environment, ie, not a single root bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); - } else { + } + else + { // should never happen bt_id_error_message( "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", @@ -169,11 +201,14 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { return 0; } -void MultiBodyTree::MultiBodyImpl::calculateStaticData() { +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++) { + for (idArrayIdx i = 0; i < m_body_list.size(); i++) + { RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) { + switch (body.m_joint_type) + { case REVOLUTE: body.m_parent_vel_rel(0) = 0; body.m_parent_vel_rel(1) = 0; @@ -212,41 +247,56 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() { case FLOATING: // no static data break; + case SPHERICAL: + //todo: review + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + 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; + break; } - // resize & initialize jacobians to zero. + // resize & initialize jacobians to zero. #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - body.m_body_dot_Jac_T_u(0) = 0.0; - body.m_body_dot_Jac_T_u(1) = 0.0; - body.m_body_dot_Jac_T_u(2) = 0.0; - body.m_body_dot_Jac_R_u(0) = 0.0; - body.m_body_dot_Jac_R_u(1) = 0.0; - body.m_body_dot_Jac_R_u(2) = 0.0; - resize(body.m_body_Jac_T,m_num_dofs); - resize(body.m_body_Jac_R,m_num_dofs); - body.m_body_Jac_T.setZero(); - body.m_body_Jac_R.setZero(); -#endif // + body.m_body_dot_Jac_T_u(0) = 0.0; + body.m_body_dot_Jac_T_u(1) = 0.0; + body.m_body_dot_Jac_T_u(2) = 0.0; + body.m_body_dot_Jac_R_u(0) = 0.0; + body.m_body_dot_Jac_R_u(1) = 0.0; + body.m_body_dot_Jac_R_u(2) = 0.0; + resize(body.m_body_Jac_T, m_num_dofs); + resize(body.m_body_Jac_R, m_num_dofs); + body.m_body_Jac_T.setZero(); + body.m_body_Jac_R.setZero(); +#endif // } } int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, - const vecx &dot_u, vecx *joint_forces) { + const vecx &dot_u, vecx *joint_forces) +{ if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || - joint_forces->size() != m_num_dofs) { - bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), - static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size())); + joint_forces->size() != m_num_dofs) + { + bt_id_error_message( + "wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), + static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size())); return -1; } // 1. relative kinematics - if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { - bt_id_error_message("error in calculateKinematics\n"); - return -1; - } - // 2. update contributions to equations of motion for every body. - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION)) + { + bt_id_error_message("error in calculateKinematics\n"); + return -1; + } + // 2. update contributions to equations of motion for every body. + 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 = @@ -268,14 +318,16 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const // 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--) { + 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++) { + 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; @@ -293,19 +345,22 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const // 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++) { + 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++) { + 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++) { + 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); @@ -316,84 +371,133 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); } + // 4.4 spherical bodies (3-DoF joints) + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_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); + } return 0; } -int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u, - const KinUpdateType type) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) { - bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), - static_cast<int>(dot_u.size())); +int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u, + const KinUpdateType type) +{ + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs) + { + bt_id_error_message( + "wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), + static_cast<int>(dot_u.size())); + return -1; + } + if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) + { + bt_id_error_message("invalid type %d\n", type); return -1; } - if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { - bt_id_error_message("invalid type %d\n", type); - return -1; - } // 1. update relative kinematics // 1.1 for revolute - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + 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; - if(type >= POSITION_VELOCITY) { - body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); - } + if (type >= POSITION_VELOCITY) + { + body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); + } + if (type >= POSITION_VELOCITY_ACCELERATION) + { + body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); + } } // 1.2 for prismatic - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) + { RigidBody &body = m_body_list[m_body_prismatic_list[i]]; body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - if(type >= POSITION_VELOCITY) { - body.m_parent_vel_rel = - body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); - } + if (type >= POSITION_VELOCITY) + { + body.m_parent_vel_rel = + body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); + } + if (type >= POSITION_VELOCITY_ACCELERATION) + { + 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++) { + 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)); + 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; - if(type >= POSITION_VELOCITY) { - 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); + if (type >= POSITION_VELOCITY) + { + 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_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - 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_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + } + if (type >= POSITION_VELOCITY_ACCELERATION) + { + 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_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; - } + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } + } + + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_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 = body.m_body_T_parent * body.m_parent_pos_parent_body; + + if (type >= POSITION_VELOCITY) + { + 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 = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + } + if (type >= POSITION_VELOCITY_ACCELERATION) + { + 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 = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } } // 2. absolute kinematic quantities (vector valued) @@ -410,26 +514,29 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx 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; - if(type >= POSITION_VELOCITY) { - // 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; - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - // 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; - } + if (type >= POSITION_VELOCITY) + { + // 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; + } + if (type >= POSITION_VELOCITY_ACCELERATION) + { + // 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++) { + 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]]; // 2.1 update absolute positions and orientations: @@ -439,121 +546,159 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx 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; - if(type >= POSITION_VELOCITY) { - // 2.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); - } - if(type >= POSITION_VELOCITY_ACCELERATION) { - // 2.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); - } + if (type >= POSITION_VELOCITY) + { + // 2.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); + } + if (type >= POSITION_VELOCITY_ACCELERATION) + { + // 2.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); + } } - return 0; + return 0; } #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) -void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) { - const int& idx=body.m_q_index; - switch(body.m_joint_type) { - case FIXED: - break; - case REVOLUTE: - setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R); - setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R); - setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R); - break; - case PRISMATIC: - setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0) - +body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1) - +body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2), - &body.m_body_Jac_T); - setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0) - +body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1) - +body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2), - &body.m_body_Jac_T); - setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0) - +body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1) - +body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2), - &body.m_body_Jac_T); - break; - case FLOATING: - setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R); - setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R); - setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R); - // body_Jac_T = body_T_parent.transpose(); - setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T); - setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T); - setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T); - - setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T); - setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T); - setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T); - - setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T); - setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T); - setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T); - - break; - } +void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body) +{ + const int &idx = body.m_q_index; + switch (body.m_joint_type) + { + case FIXED: + break; + case REVOLUTE: + setMat3xElem(0, idx, body.m_Jac_JR(0), &body.m_body_Jac_R); + setMat3xElem(1, idx, body.m_Jac_JR(1), &body.m_body_Jac_R); + setMat3xElem(2, idx, body.m_Jac_JR(2), &body.m_body_Jac_R); + break; + case PRISMATIC: + setMat3xElem(0, idx, body.m_body_T_parent_ref(0, 0) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 0) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 0) * body.m_Jac_JT(2), + &body.m_body_Jac_T); + setMat3xElem(1, idx, body.m_body_T_parent_ref(0, 1) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 1) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 1) * body.m_Jac_JT(2), + &body.m_body_Jac_T); + setMat3xElem(2, idx, body.m_body_T_parent_ref(0, 2) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 2) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 2) * body.m_Jac_JT(2), + &body.m_body_Jac_T); + break; + case FLOATING: + setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R); + setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R); + setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R); + // body_Jac_T = body_T_parent.transpose(); + setMat3xElem(0, idx + 3, body.m_body_T_parent(0, 0), &body.m_body_Jac_T); + setMat3xElem(0, idx + 4, body.m_body_T_parent(1, 0), &body.m_body_Jac_T); + setMat3xElem(0, idx + 5, body.m_body_T_parent(2, 0), &body.m_body_Jac_T); + + setMat3xElem(1, idx + 3, body.m_body_T_parent(0, 1), &body.m_body_Jac_T); + setMat3xElem(1, idx + 4, body.m_body_T_parent(1, 1), &body.m_body_Jac_T); + setMat3xElem(1, idx + 5, body.m_body_T_parent(2, 1), &body.m_body_Jac_T); + + setMat3xElem(2, idx + 3, body.m_body_T_parent(0, 2), &body.m_body_Jac_T); + setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T); + setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T); + + break; + case SPHERICAL: + //todo: review + setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R); + setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R); + setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R); + break; + } } -int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs) { - bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); - return -1; - } - if(type != POSITION_ONLY && type != POSITION_VELOCITY) { - bt_id_error_message("invalid type %d\n", type); - return -1; - } - - addRelativeJacobianComponent(m_body_list[0]); - 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]]; - - mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R); - body.m_body_Jac_T = parent.m_body_Jac_T; - mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x); - sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T); - - addRelativeJacobianComponent(body); - mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T); - - if(type >= POSITION_VELOCITY) { - body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u - - body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel); - body.m_body_dot_Jac_T_u = body.m_body_T_parent * - (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.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)); - } - } - return 0; +int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type) +{ + if (q.size() != m_num_dofs || u.size() != m_num_dofs) + { + bt_id_error_message( + "wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); + return -1; + } + if (type != POSITION_ONLY && type != POSITION_VELOCITY) + { + bt_id_error_message("invalid type %d\n", type); + return -1; + } + + addRelativeJacobianComponent(m_body_list[0]); + 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]]; + + mul(body.m_body_T_parent, parent.m_body_Jac_R, &body.m_body_Jac_R); + body.m_body_Jac_T = parent.m_body_Jac_T; + mul(tildeOperator(body.m_parent_pos_parent_body), parent.m_body_Jac_R, &m_m3x); + sub(body.m_body_Jac_T, m_m3x, &body.m_body_Jac_T); + + addRelativeJacobianComponent(body); + mul(body.m_body_T_parent, body.m_body_Jac_T, &body.m_body_Jac_T); + + if (type >= POSITION_VELOCITY) + { + body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel); + body.m_body_dot_Jac_T_u = body.m_body_T_parent * + (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.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)); + } + } + return 0; } #endif -static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { - switch (dof) { +static inline void setThreeDoFJacobians(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; + } +} + +static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) +{ + switch (dof) + { // rotational part case 0: Jac_JR(0) = 1; @@ -595,8 +740,10 @@ static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) } } -static inline int jointNumDoFs(const JointType &type) { - switch (type) { +static inline int jointNumDoFs(const JointType &type) +{ + switch (type) + { case FIXED: return 0; case REVOLUTE: @@ -604,6 +751,8 @@ static inline int jointNumDoFs(const JointType &type) { return 1; case FLOATING: return 6; + case SPHERICAL: + return 3; } // this should never happen bt_id_error_message("invalid joint type\n"); @@ -615,37 +764,45 @@ static inline int jointNumDoFs(const JointType &type) { int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, 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) -// (Later named "Composite Rigid Body Algorithm" by Featherstone). -// -// This implementation, however, handles branched systems and uses a formulation centered -// on the origin of the body-fixed frame to avoid re-computing various quantities at the com. + 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) + // (Later named "Composite Rigid Body Algorithm" by Featherstone). + // + // This implementation, however, handles branched systems and uses a formulation centered + // on the origin of the body-fixed frame to avoid re-computing various quantities at the com. if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || - mass_matrix->cols() != m_num_dofs) { - bt_id_error_message("Dimension error. System has %d DOFs,\n" - "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()), - static_cast<int>(mass_matrix->cols())); + mass_matrix->cols() != m_num_dofs) + { + bt_id_error_message( + "Dimension error. System has %d DOFs,\n" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()), + static_cast<int>(mass_matrix->cols())); 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++) { - setMatxxElem(i, j, 0.0, mass_matrix); + if (initialize_matrix) + { + for (int i = 0; i < m_num_dofs; i++) + { + for (int j = 0; j < m_num_dofs; j++) + { + setMatxxElem(i, j, 0.0, mass_matrix); } } } - if (update_kinematics) { + if (update_kinematics) + { // 1. update relative kinematics // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + 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; @@ -653,7 +810,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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++) { + 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 = @@ -661,7 +819,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } // 1.3 fixed joints: nothing to do // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + 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)) * @@ -674,7 +833,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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--) { + 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 @@ -682,7 +842,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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++) { + 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(); @@ -692,7 +853,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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) { + 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 @@ -707,7 +869,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } } - for (int i = m_body_list.size() - 1; i >= 0; i--) { + for (int i = m_body_list.size() - 1; i >= 0; i--) + { const RigidBody &body = m_body_list[i]; // determine DoF-range for body @@ -717,11 +880,18 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // 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--) { + for (int col = q_index_max; col >= q_index_min; col--) + { // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) { + if (FLOATING == body.m_joint_type) + { setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); } + if (SPHERICAL == body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(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); @@ -732,19 +902,27 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // 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) { - bt_id_error_message("??\n"); - return -1; + for (int row = col - 1; row >= q_index_min; row--) + { + if (SPHERICAL == body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMatxxElem(col, row, Mrc, mass_matrix); + } + if (FLOATING == body.m_joint_type) + { + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMatxxElem(col, row, Mrc, mass_matrix); } - setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMatxxElem(col, row, Mrc, mass_matrix); } // 2. ancestor dofs int child_idx = i; int parent_idx = m_parent_index[i]; - while (parent_idx >= 0) { + while (parent_idx >= 0) + { const RigidBody &child_body = m_body_list[child_idx]; const RigidBody &parent_body = m_body_list[parent_idx]; @@ -758,9 +936,16 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool 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--) { + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) + { + if (SPHERICAL == parent_body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } // set jacobians for 6-DoF joints - if (FLOATING == parent_body.m_joint_type) { + 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); @@ -774,10 +959,13 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } } - if (set_lower_triangular_matrix) { - for (int col = 0; col < m_num_dofs; col++) { - for (int row = 0; row < col; row++) { - setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); + if (set_lower_triangular_matrix) + { + for (int col = 0; col < m_num_dofs; col++) + { + for (int row = 0; row < col; row++) + { + setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); } } } @@ -785,76 +973,91 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } // utility macro -#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ - do { \ - if (index < 0 || index >= m_num_bodies) { \ - bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ - return -1; \ - } \ +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do \ + { \ + if (index < 0 || index >= m_num_bodies) \ + { \ + bt_id_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) { +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; } -int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { +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; } -int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { +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; } -int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { +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; } -int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { +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; } -int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { +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; } -int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { +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) { + 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 { + } + 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 { +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; } -int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { +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; } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, - vec3 *world_velocity) const { + 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; @@ -862,13 +1065,17 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const { + 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) { + if (body.m_mass > 0) + { com = body.m_body_mass_com / body.m_mass; - } else { + } + else + { com(0) = 0; com(1) = 0; com(2) = 0; @@ -880,149 +1087,173 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, } int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const { + 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 { + 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 { +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; } int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, - const char **joint_type) const { + 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::getParentRParentBodyRef(const int body_index, vec3* r) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *r=m_body_list[body_index].m_parent_pos_parent_body_ref; - return 0; +int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3 *r) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *r = m_body_list[body_index].m_parent_pos_parent_body_ref; + return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *T=m_body_list[body_index].m_body_T_parent_ref; - return 0; +int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33 *T) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *T = m_body_list[body_index].m_body_T_parent_ref; + return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - if(m_body_list[body_index].m_joint_type == REVOLUTE) { - *axis = m_body_list[body_index].m_Jac_JR; - return 0; - } - if(m_body_list[body_index].m_joint_type == PRISMATIC) { - *axis = m_body_list[body_index].m_Jac_JT; - return 0; - } - setZero(*axis); - return 0; +int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3 *axis) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + if (m_body_list[body_index].m_joint_type == REVOLUTE) + { + *axis = m_body_list[body_index].m_Jac_JR; + return 0; + } + if (m_body_list[body_index].m_joint_type == PRISMATIC) + { + *axis = m_body_list[body_index].m_Jac_JT; + return 0; + } + setZero(*axis); + return 0; } -int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { +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; } -int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { +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; } int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3& first_mass_moment) { + 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) { + 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 { +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; } int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, - vec3 *first_mass_moment) const { + 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 { + 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++) { +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); } } -int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { +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; } -int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { +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; } #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) -int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u; - return 0; +int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u; + return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u; - return 0; +int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u; + return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans); - return 0; +int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans); + return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot); - return 0; +int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const +{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + mul(body.m_body_T_world.transpose(), body.m_body_Jac_R, world_jac_rot); + return 0; } #endif -} +} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 3efe9d0492..eabdbe161b 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -8,12 +8,13 @@ #include "../IDConfig.hpp" #include "../MultiBodyTree.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ /// Structure for for rigid body mass properties, connectivity and kinematic state /// all vectors and matrices are in body-fixed frame, if not indicated otherwise. /// The body-fixed frame is located in the joint connecting the body to its parent. -struct RigidBody { +struct RigidBody +{ ID_DECLARE_ALIGNED_ALLOCATOR(); // 1 Inertial properties /// Mass @@ -112,31 +113,33 @@ struct RigidBody { mat33 m_body_subtree_I_body; #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - /// translational jacobian in body-fixed frame d(m_body_vel)/du - mat3x m_body_Jac_T; - /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du - mat3x m_body_Jac_R; - /// components of linear acceleration depending on u - /// (same as is d(m_Jac_T)/dt*u) - vec3 m_body_dot_Jac_T_u; - /// components of angular acceleration depending on u - /// (same as is d(m_Jac_T)/dt*u) - vec3 m_body_dot_Jac_R_u; + /// translational jacobian in body-fixed frame d(m_body_vel)/du + mat3x m_body_Jac_T; + /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du + mat3x m_body_Jac_R; + /// components of linear acceleration depending on u + /// (same as is d(m_Jac_T)/dt*u) + vec3 m_body_dot_Jac_T_u; + /// components of angular acceleration depending on u + /// (same as is d(m_Jac_T)/dt*u) + vec3 m_body_dot_Jac_R_u; #endif }; /// The MBS implements a tree structured multibody system -class MultiBodyTree::MultiBodyImpl { +class MultiBodyTree::MultiBodyImpl +{ friend class MultiBodyTree; public: ID_DECLARE_ALIGNED_ALLOCATOR(); - enum KinUpdateType { - POSITION_ONLY, - POSITION_VELOCITY, - POSITION_VELOCITY_ACCELERATION - }; + enum KinUpdateType + { + POSITION_ONLY, + POSITION_VELOCITY, + POSITION_VELOCITY_ACCELERATION + }; /// constructor /// @param num_bodies the number of bodies in the system @@ -150,24 +153,24 @@ public: int calculateMassMatrix(const vecx& q, const bool update_kinematics, const bool initialize_matrix, const bool set_lower_triangular_matrix, matxx* mass_matrix); - /// calculate kinematics (vector quantities) - /// Depending on type, update positions only, positions & velocities, or positions, velocities - /// and accelerations. - int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type); + /// calculate kinematics (vector quantities) + /// Depending on type, update positions only, positions & velocities, or positions, velocities + /// and accelerations. + int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type); #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - /// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms. - int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type); - /// \copydoc MultiBodyTree::getBodyDotJacobianTransU - int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ; - /// \copydoc MultiBodyTree::getBodyDotJacobianRotU - int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; - /// \copydoc MultiBodyTree::getBodyJacobianTrans - int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ; - /// \copydoc MultiBodyTree::getBodyJacobianRot - int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; - /// Add relative Jacobian component from motion relative to parent body - /// @param body the body to add the Jacobian component for - void addRelativeJacobianComponent(RigidBody&body); + /// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms. + int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type); + /// \copydoc MultiBodyTree::getBodyDotJacobianTransU + int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const; + /// \copydoc MultiBodyTree::getBodyDotJacobianRotU + int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; + /// \copydoc MultiBodyTree::getBodyJacobianTrans + int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const; + /// \copydoc MultiBodyTree::getBodyJacobianRot + int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; + /// Add relative Jacobian component from motion relative to parent body + /// @param body the body to add the Jacobian component for + void addRelativeJacobianComponent(RigidBody& body); #endif /// generate additional index sets from the parent_index array /// @return -1 on error, 0 on success @@ -190,12 +193,12 @@ public: int getJointType(const int body_index, JointType* joint_type) const; /// \copydoc MultiBodyTree::getJointTypeStr int getJointTypeStr(const int body_index, const char** joint_type) const; - /// \copydoc MultiBodyTree::getParentRParentBodyRef - int getParentRParentBodyRef(const int body_index, vec3* r) const; - /// \copydoc MultiBodyTree::getBodyTParentRef - int getBodyTParentRef(const int body_index, mat33* T) const; - /// \copydoc MultiBodyTree::getBodyAxisOfMotion - int getBodyAxisOfMotion(const int body_index, vec3* axis) const; + /// \copydoc MultiBodyTree::getParentRParentBodyRef + int getParentRParentBodyRef(const int body_index, vec3* r) const; + /// \copydoc MultiBodyTree::getBodyTParentRef + int getBodyTParentRef(const int body_index, mat33* T) const; + /// \copydoc MultiBodyTree::getBodyAxisOfMotion + int getBodyAxisOfMotion(const int body_index, vec3* axis) const; /// \copydoc MultiBodyTree:getDoFOffset int getDoFOffset(const int body_index, int* q_index) const; /// \copydoc MultiBodyTree::getBodyOrigin @@ -271,13 +274,15 @@ private: idArray<int>::type m_body_prismatic_list; // Indices of floating joints idArray<int>::type m_body_floating_list; + // Indices of spherical joints + idArray<int>::type m_body_spherical_list; // a user-provided integer idArray<int>::type m_user_int; // a user-provided pointer idArray<void*>::type m_user_ptr; #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - mat3x m_m3x; + mat3x m_m3x; #endif }; -} +} // namespace btInverseDynamics #endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index e9511b7076..a718db051e 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -1,12 +1,13 @@ #include "MultiBodyTreeInitCache.hpp" -namespace btInverseDynamics { - -MultiBodyTree::InitCache::InitCache() { +namespace btInverseDynamics +{ +MultiBodyTree::InitCache::InitCache() +{ m_inertias.resize(0); m_joints.resize(0); m_num_dofs = 0; - m_root_index=-1; + m_root_index = -1; } int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, @@ -15,8 +16,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind const mat33& body_T_parent_ref, const vec3& body_axis_of_motion, const idScalar mass, const vec3& body_r_body_com, const mat33& body_I_body, - const int user_int, void* user_ptr) { - switch (joint_type) { + const int user_int, void* user_ptr) +{ + switch (joint_type) + { case REVOLUTE: case PRISMATIC: m_num_dofs += 1; @@ -25,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind // does not add a degree of freedom // m_num_dofs+=0; break; + case SPHERICAL: + m_num_dofs += 3; + break; case FLOATING: m_num_dofs += 6; break; @@ -33,13 +39,15 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind return -1; } - if(-1 == parent_index) { - if(m_root_index>=0) { + if (-1 == parent_index) + { + if (m_root_index >= 0) + { bt_id_error_message("trying to add body %d as root, but already added %d as root body\n", - body_index, m_root_index); + body_index, m_root_index); return -1; } - m_root_index=body_index; + m_root_index = body_index; } JointData joint; @@ -61,8 +69,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind m_user_ptr.push_back(user_ptr); return 0; } -int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { - if (index < 0 || index > static_cast<int>(m_inertias.size())) { +int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const +{ + if (index < 0 || index > static_cast<int>(m_inertias.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -71,8 +81,10 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert return 0; } -int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { - if (index < 0 || index > static_cast<int>(m_user_int.size())) { +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const +{ + if (index < 0 || index > static_cast<int>(m_user_int.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -80,8 +92,10 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { return 0; } -int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { - if (index < 0 || index > static_cast<int>(m_user_ptr.size())) { +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const +{ + if (index < 0 || index > static_cast<int>(m_user_ptr.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -89,8 +103,10 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const return 0; } -int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { - if (index < 0 || index > static_cast<int>(m_joints.size())) { +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const +{ + if (index < 0 || index > static_cast<int>(m_joints.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -98,16 +114,18 @@ int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) co return 0; } -int MultiBodyTree::InitCache::buildIndexSets() { +int MultiBodyTree::InitCache::buildIndexSets() +{ // NOTE: This function assumes that proper indices were provided // User2InternalIndex from utils can be used to facilitate this. m_parent_index.resize(numBodies()); - for (idArrayIdx j = 0; j < m_joints.size(); j++) { + for (idArrayIdx j = 0; j < m_joints.size(); j++) + { const JointData& joint = m_joints[j]; m_parent_index[joint.m_child] = joint.m_parent; } return 0; } -} +} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp index 0d2aa4a071..dbdb3ff604 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -5,9 +5,11 @@ #include "../IDMath.hpp" #include "../MultiBodyTree.hpp" -namespace btInverseDynamics { +namespace btInverseDynamics +{ /// Mass properties of a rigid body -struct InertiaData { +struct InertiaData +{ ID_DECLARE_ALIGNED_ALLOCATOR(); /// mass @@ -21,7 +23,8 @@ struct InertiaData { }; /// Joint properties -struct JointData { +struct JointData +{ ID_DECLARE_ALIGNED_ALLOCATOR(); /// type of joint @@ -48,7 +51,8 @@ struct JointData { /// Data structure to store data passed by the user. /// This is used in MultiBodyTree::finalize to build internal data structures. -class MultiBodyTree::InitCache { +class MultiBodyTree::InitCache +{ public: ID_DECLARE_ALIGNED_ALLOCATOR(); /// constructor @@ -105,5 +109,5 @@ private: // index of root body (or -1 if not set) int m_root_index; }; -} +} // namespace btInverseDynamics #endif // MULTIBODYTREEINITCACHE_HPP_ |