diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics')
15 files changed, 0 insertions, 4281 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp deleted file mode 100644 index b662b80152..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp +++ /dev/null @@ -1,107 +0,0 @@ -///@file Configuration for Inverse Dynamics Library, -/// such as choice of linear algebra library and underlying scalar type -#ifndef IDCONFIG_HPP_ -#define IDCONFIG_HPP_ - -// If true, enable jacobian calculations. -// This adds a 3xN matrix to every body, + 2 3-Vectors. -// so it is not advised for large systems if it is not absolutely necessary. -// Also, this is not required for standard inverse dynamics calculations. -// Will only work with vector math libraries that support 3xN matrices. -#define BT_ID_WITH_JACOBIANS - -// If we have a custom configuration, compile without using other parts of bullet. -#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H -#include <cmath> -#define BT_ID_WO_BULLET -#define BT_ID_SQRT(x) std::sqrt(x) -#define BT_ID_FABS(x) std::fabs(x) -#define BT_ID_COS(x) std::cos(x) -#define BT_ID_SIN(x) std::sin(x) -#define BT_ID_ATAN2(x, y) std::atan2(x, y) -#define BT_ID_POW(x, y) std::pow(x, y) -#define BT_ID_SNPRINTF snprintf -#define BT_ID_PI M_PI -#define BT_ID_USE_DOUBLE_PRECISION -#else -#define BT_ID_SQRT(x) btSqrt(x) -#define BT_ID_FABS(x) btFabs(x) -#define BT_ID_COS(x) btCos(x) -#define BT_ID_SIN(x) btSin(x) -#define BT_ID_ATAN2(x, y) btAtan2(x, y) -#define BT_ID_POW(x, y) btPow(x, y) -#define BT_ID_PI SIMD_PI -#ifdef _WIN32 -#define BT_ID_SNPRINTF _snprintf -#else -#define BT_ID_SNPRINTF snprintf -#endif // -#endif -// error messages -#include "IDErrorMessages.hpp" - -#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H -/* -#include "IDConfigEigen.hpp" -#include "IDConfigBuiltin.hpp" -*/ -#define INVDYN_INCLUDE_HELPER_2(x) #x -#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) -#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) -#ifndef btInverseDynamics -#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) - -#else -#define btInverseDynamics btInverseDynamicsBullet3 -// Use default configuration with bullet's types -// Use the same scalar type as rest of bullet library -#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) - -#ifdef BT_USE_DOUBLE_PRECISION -#define BT_ID_USE_DOUBLE_PRECISION -#endif - -#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 -{ - typedef b3AlignedObjectArray<T> type; -}; -typedef int idArrayIdx; -#define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR() - -#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 - -#include "LinearMath/btAlignedObjectArray.h" -template <typename T> -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 - -// use bullet's allocator functions -#define idMalloc btAllocFunc -#define idFree btFreeFunc - -#define ID_LINEAR_MATH_USE_BULLET -#include "details/IDLinearMathInterface.hpp" -#endif -#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp deleted file mode 100644 index 6392367924..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp +++ /dev/null @@ -1,38 +0,0 @@ -///@file Configuration for Inverse Dynamics Library without external dependencies -#ifndef INVDYNCONFIG_BUILTIN_HPP_ -#define INVDYNCONFIG_BUILTIN_HPP_ -#define btInverseDynamics btInverseDynamicsBuiltin -#ifdef BT_USE_DOUBLE_PRECISION -// choose double/single precision version -typedef double idScalar; -#else -typedef float idScalar; -#endif -// use std::vector for arrays -#include <vector> -// this is to make it work with C++2003, otherwise we could do this -// template <typename T> -// using idArray = std::vector<T>; -template <typename T> -struct idArray -{ - typedef std::vector<T> type; -}; -typedef std::vector<int>::size_type idArrayIdx; -// default to standard malloc/free -#include <cstdlib> -#define idMalloc ::malloc -#define idFree ::free -// currently not aligned at all... -#define ID_DECLARE_ALIGNED_ALLOCATOR() \ - inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete(void* ptr) { idFree(ptr); } \ - inline void* operator new(std::size_t, void* ptr) { return ptr; } \ - inline void operator delete(void*, void*) {} \ - inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete[](void* ptr) { idFree(ptr); } \ - inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ - inline void operator delete[](void*, void*) {} - -#include "details/IDMatVec.hpp" -#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp deleted file mode 100644 index cfb308ee55..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp +++ /dev/null @@ -1,32 +0,0 @@ -///@file Configuration for Inverse Dynamics Library with Eigen -#ifndef INVDYNCONFIG_EIGEN_HPP_ -#define INVDYNCONFIG_EIGEN_HPP_ -#define btInverseDynamics btInverseDynamicsEigen -#ifdef BT_USE_DOUBLE_PRECISION -// choose double/single precision version -typedef double idScalar; -#else -typedef float idScalar; -#endif - -// use std::vector for arrays -#include <vector> -// this is to make it work with C++2003, otherwise we could do this -// template <typename T> -// using idArray = std::vector<T>; -template <typename T> -struct idArray -{ - typedef std::vector<T> type; -}; -typedef std::vector<int>::size_type idArrayIdx; -// default to standard malloc/free -#include <cstdlib> -#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW -// Note on interfaces: -// Eigen::Matrix has data(), to get c-array storage -// HOWEVER: default storage is column-major! -#define ID_LINEAR_MATH_USE_EIGEN -#include "Eigen/Eigen" -#include "details/IDEigenInterface.hpp" -#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp b/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp deleted file mode 100644 index 5a98f01498..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp +++ /dev/null @@ -1,31 +0,0 @@ -///@file error message utility functions -#ifndef IDUTILS_HPP_ -#define IDUTILS_HPP_ -#include <cstring> -/// name of file being compiled, without leading path components -#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) - -#if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2) -#include "Bullet3Common/b3Logging.h" -#define bt_id_error_message(...) b3Error(__VA_ARGS__) -#define bt_id_warning_message(...) b3Warning(__VA_ARGS__) -#define id_printf(...) b3Printf(__VA_ARGS__) -#else // BT_ID_WO_BULLET -#include <cstdio> -/// print error message with file/line information -#define 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__); \ - } while (0) -#define id_printf(...) printf(__VA_ARGS__) -#endif // BT_ID_WO_BULLET -#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp deleted file mode 100644 index 2f120ed489..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "IDMath.hpp" - -#include <cmath> -#include <limits> - -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) -{ - v(0) = 0; - v(1) = 0; - v(2) = 0; -} - -void setZero(vecx &v) -{ - for (int i = 0; i < v.size(); i++) - { - v(i) = 0; - } -} - -void setZero(mat33 &m) -{ - m(0, 0) = 0; - m(0, 1) = 0; - m(0, 2) = 0; - m(1, 0) = 0; - m(1, 1) = 0; - m(1, 2) = 0; - m(2, 0) = 0; - m(2, 1) = 0; - m(2, 2) = 0; -} - -void skew(vec3 &v, mat33 *result) -{ - (*result)(0, 0) = 0.0; - (*result)(0, 1) = -v(2); - (*result)(0, 2) = v(1); - (*result)(1, 0) = v(2); - (*result)(1, 1) = 0.0; - (*result)(1, 2) = -v(0); - (*result)(2, 0) = -v(1); - (*result)(2, 1) = v(0); - (*result)(2, 2) = 0.0; -} - -idScalar maxAbs(const vecx &v) -{ - idScalar result = 0.0; - for (int i = 0; i < v.size(); i++) - { - const idScalar tmp = BT_ID_FABS(v(i)); - if (tmp > result) - { - result = tmp; - } - } - return result; -} - -idScalar maxAbs(const vec3 &v) -{ - idScalar result = 0.0; - for (int i = 0; i < 3; i++) - { - const idScalar tmp = BT_ID_FABS(v(i)); - if (tmp > result) - { - result = tmp; - } - } - return result; -} - -#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; -} - -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 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 T; - const idScalar cos_alpha = BT_ID_COS(alpha); - const idScalar sin_alpha = BT_ID_SIN(alpha); - // [1 0 0] - // [0 c s] - // [0 -s c] - T(0, 0) = 1.0; - T(0, 1) = 0.0; - T(0, 2) = 0.0; - - T(1, 0) = 0.0; - T(1, 1) = cos_alpha; - T(1, 2) = sin_alpha; - - T(2, 0) = 0.0; - T(2, 1) = -sin_alpha; - T(2, 2) = cos_alpha; - - return T; -} - -mat33 transformY(const idScalar &beta) -{ - mat33 T; - const idScalar cos_beta = BT_ID_COS(beta); - const idScalar sin_beta = BT_ID_SIN(beta); - // [c 0 -s] - // [0 1 0] - // [s 0 c] - T(0, 0) = cos_beta; - T(0, 1) = 0.0; - T(0, 2) = -sin_beta; - - T(1, 0) = 0.0; - T(1, 1) = 1.0; - T(1, 2) = 0.0; - - T(2, 0) = sin_beta; - T(2, 1) = 0.0; - T(2, 2) = cos_beta; - - return T; -} - -mat33 transformZ(const idScalar &gamma) -{ - mat33 T; - const idScalar cos_gamma = BT_ID_COS(gamma); - const idScalar sin_gamma = BT_ID_SIN(gamma); - // [ c s 0] - // [-s c 0] - // [ 0 0 1] - T(0, 0) = cos_gamma; - T(0, 1) = sin_gamma; - T(0, 2) = 0.0; - - T(1, 0) = -sin_gamma; - T(1, 1) = cos_gamma; - T(1, 2) = 0.0; - - T(2, 0) = 0.0; - T(2, 1) = 0.0; - T(2, 2) = 1.0; - - return T; -} - -mat33 tildeOperator(const vec3 &v) -{ - mat33 m; - m(0, 0) = 0.0; - m(0, 1) = -v(2); - m(0, 2) = v(1); - m(1, 0) = v(2); - m(1, 1) = 0.0; - m(1, 2) = -v(0); - m(2, 0) = -v(1); - m(2, 1) = v(0); - m(2, 2) = 0.0; - return m; -} - -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); - const idScalar ct = BT_ID_COS(theta); - - (*r)(0) = a; - (*r)(1) = -sa * d; - (*r)(2) = ca * d; - - (*T)(0, 0) = ct; - (*T)(0, 1) = -st; - (*T)(0, 2) = 0.0; - - (*T)(1, 0) = st * ca; - (*T)(1, 1) = ct * ca; - (*T)(1, 2) = -sa; - - (*T)(2, 0) = st * sa; - (*T)(2, 1) = ct * sa; - (*T)(2, 2) = ca; -} - -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; - - const idScalar &x = axis(0); - const idScalar &y = axis(1); - const idScalar &z = axis(2); - - (*T)(0, 0) = x * x * one_m_c + c; - (*T)(0, 1) = x * y * one_m_c - z * s; - (*T)(0, 2) = x * z * one_m_c + y * s; - - (*T)(1, 0) = x * y * one_m_c + z * s; - (*T)(1, 1) = y * y * one_m_c + c; - (*T)(1, 2) = y * z * one_m_c - x * s; - - (*T)(2, 0) = x * z * one_m_c - y * s; - (*T)(2, 1) = y * z * one_m_c + x * s; - (*T)(2, 2) = z * z * one_m_c + c; -} - -bool isPositiveDefinite(const mat33 &m) -{ - // test if all upper left determinants are positive - if (m(0, 0) <= 0) - { // upper 1x1 - return false; - } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) - { // upper 2x2 - return false; - } - if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) - { - return false; - } - return true; -} - -bool isPositiveSemiDefinite(const mat33 &m) -{ - // test if all upper left determinants are positive - if (m(0, 0) < 0) - { // upper 1x1 - return false; - } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) - { // upper 2x2 - return false; - } - if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) - { - return false; - } - return true; -} - -bool isPositiveSemiDefiniteFuzzy(const mat33 &m) -{ - // test if all upper left determinants are positive - if (m(0, 0) < -kIsZero) - { // upper 1x1 - return false; - } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) - { // upper 2x2 - return false; - } - if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) - { - return false; - } - return true; -} - -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) -{ - // 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)); - - 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)) - { - 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)); - return false; - } - 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)); - return false; - } - 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)); - return false; - } - } - // check positive/zero diagonal elements - 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)); - 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)); - return false; - } - 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)); - 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)) - - // check for unit length column vectors - 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); - 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) - { - 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) - { - 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) - { - 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) - { - bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n"); - print_mat(m); - return false; - } - return true; -} - -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 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 deleted file mode 100644 index 40bee5302b..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp +++ /dev/null @@ -1,100 +0,0 @@ -/// @file Math utility functions used in inverse dynamics library. -/// Defined here as they may not be provided by the math library. - -#ifndef IDMATH_HPP_ -#define IDMATH_HPP_ -#include "IDConfig.hpp" - -namespace btInverseDynamics -{ -/// set all elements to zero -void setZero(vec3& v); -/// set all elements to zero -void setZero(vecx& v); -/// set all elements to zero -void setZero(mat33& m); -/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a) -void skew(vec3& v, mat33* result); -/// return maximum absolute value -idScalar maxAbs(const vecx& v); -#ifndef ID_LINEAR_MATH_USE_EIGEN -/// return maximum absolute value -idScalar maxAbs(const vec3& v); -#endif //ID_LINEAR_MATH_USE_EIGEN - -#if (defined BT_ID_HAVE_MAT3X) -idScalar maxAbsMat3x(const 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); -#endif - -/// get offset vector & transform matrix from DH parameters -/// TODO: add documentation -void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T); - -/// Check if a 3x3 matrix is positive definite -/// @param m a 3x3 matrix -/// @return true if m>0, false otherwise -bool isPositiveDefinite(const mat33& m); - -/// Check if a 3x3 matrix is positive semi definite -/// @param m a 3x3 matrix -/// @return true if m>=0, false otherwise -bool isPositiveSemiDefinite(const mat33& m); -/// Check if a 3x3 matrix is positive semi definite within numeric limits -/// @param m a 3x3 matrix -/// @return true if m>=-eps, false otherwise -bool isPositiveSemiDefiniteFuzzy(const mat33& m); - -/// Determinant of 3x3 matrix -/// NOTE: implemented here for portability, as determinant operation -/// will be implemented differently for various matrix/vector libraries -/// @param m a 3x3 matrix -/// @return det(m) -idScalar determinant(const mat33& m); - -/// Test if a 3x3 matrix satisfies some properties of inertia matrices -/// @param I a 3x3 matrix -/// @param index body index (for error messages) -/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted -/// @return true if I satisfies inertia matrix properties, false otherwise. -bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint); - -/// Check if a 3x3 matrix is a valid transform (rotation) matrix -/// @param m a 3x3 matrix -/// @return true if m is a rotation matrix, false otherwise -bool isValidTransformMatrix(const mat33& m); -/// Transform matrix from parent to child frame, -/// when the child frame is rotated about @param axis by @angle -/// (mathematically positive) -/// @param axis the axis of rotation -/// @param angle rotation angle -/// @param T pointer to transform matrix -void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T); - -/// Check if this is a unit vector -/// @param vector -/// @return true if |vector|=1 within numeric limits -bool isUnitVector(const vec3& vector); - -/// @input a vector in R^3 -/// @returns corresponding spin tensor -mat33 tildeOperator(const vec3& v); -/// @param alpha angle in radians -/// @returns transform matrix for ratation with @param alpha about x-axis -mat33 transformX(const idScalar& alpha); -/// @param beta angle in radians -/// @returns transform matrix for ratation with @param beta about y-axis -mat33 transformY(const idScalar& beta); -/// @param gamma angle in radians -/// @returns transform matrix for ratation with @param gamma about z-axis -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); -} // namespace btInverseDynamics -#endif // IDMATH_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp deleted file mode 100644 index 9326b0d098..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp +++ /dev/null @@ -1,548 +0,0 @@ -#include "MultiBodyTree.hpp" - -#include <cmath> -#include <limits> -#include <vector> - -#include "IDMath.hpp" -#include "details/MultiBodyTreeImpl.hpp" -#include "details/MultiBodyTreeInitCache.hpp" - -namespace btInverseDynamics -{ -MultiBodyTree::MultiBodyTree() - : m_is_finalized(false), - m_mass_parameters_are_valid(true), - m_accept_invalid_mass_parameters(false), - m_impl(0x0), - m_init_cache(0x0) -{ - m_init_cache = new InitCache(); -} - -MultiBodyTree::~MultiBodyTree() -{ - delete m_impl; - delete m_init_cache; -} - -void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) -{ - m_accept_invalid_mass_parameters = flag; -} - -bool MultiBodyTree::getAcceptInvalidMassProperties() const -{ - return m_accept_invalid_mass_parameters; -} - -int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const -{ - return m_impl->getBodyOrigin(body_index, world_origin); -} - -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 -{ - return m_impl->getBodyTransform(body_index, world_T_body); -} -int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const -{ - return m_impl->getBodyAngularVelocity(body_index, world_omega); -} -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 -{ - return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); -} - -int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const -{ - return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); -} -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::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); -} - -void MultiBodyTree::printTree() { m_impl->printTree(); } -void MultiBodyTree::printTreeData() { m_impl->printTreeData(); } - -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) - { - bt_id_error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) - { - bt_id_error_message("error in inverse dynamics calculation\n"); - return -1; - } - return 0; -} - -int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, - const bool initialize_matrix, - const bool set_lower_triangular_matrix, matxx *mass_matrix) -{ - if (false == m_is_finalized) - { - 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)) - { - bt_id_error_message("error in mass matrix calculation\n"); - return -1; - } - return 0; -} - -int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) -{ - return calculateMassMatrix(q, true, true, true, mass_matrix); -} - -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; -} - -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)) - { - 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) - { - bt_id_error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateKinematics(q, u, u, - 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) -{ - 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::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::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) - { - 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) - { - case REVOLUTE: - case PRISMATIC: - // check if axis is unit vector - 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_error_message("axis of motion vector too short (%e)\n", length); - return -1; - } - body_axis_of_motion = (1.0 / length) * body_axis_of_motion; - } - break; - case FIXED: - break; - case FLOATING: - break; - 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) - { - 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) - { - return -1; - } - } - - if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) - { - m_mass_parameters_are_valid = false; - // error message printed in function call - if (!m_accept_invalid_mass_parameters) - { - return -1; - } - } - - if (!isValidTransformMatrix(body_T_parent_ref)) - { - return -1; - } - - return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, - body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, - body_I_body, user_int, user_ptr); -} - -int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const -{ - return m_impl->getParentIndex(body_index, parent_index); -} - -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 -{ - return m_impl->getUserPtr(body_index, user_ptr); -} - -int MultiBodyTree::setUserInt(const int body_index, const int user_int) -{ - return m_impl->setUserInt(body_index, user_int); -} - -int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) -{ - return m_impl->setUserPtr(body_index, user_ptr); -} - -int MultiBodyTree::finalize() -{ - const int &num_bodies = m_init_cache->numBodies(); - const int &num_dofs = m_init_cache->numDoFs(); - - 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()) - { - return -1; - } - m_init_cache->getParentIndexArray(&m_impl->m_parent_index); - - // 3 setup internal kinematic and dynamic data - for (int index = 0; index < num_bodies; index++) - { - InertiaData inertia; - JointData joint; - if (-1 == m_init_cache->getInertiaData(index, &inertia)) - { - return -1; - } - if (-1 == m_init_cache->getJointData(index, &joint)) - { - return -1; - } - - RigidBody &rigid_body = m_impl->m_body_list[index]; - - rigid_body.m_mass = inertia.m_mass; - rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; - rigid_body.m_body_I_body = inertia.m_body_I_body; - rigid_body.m_joint_type = joint.m_type; - rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; - rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; - rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; - rigid_body.m_joint_type = joint.m_type; - - int user_int; - if (-1 == m_init_cache->getUserInt(index, &user_int)) - { - return -1; - } - if (-1 == m_impl->setUserInt(index, user_int)) - { - return -1; - } - - void *user_ptr; - if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) - { - return -1; - } - if (-1 == m_impl->setUserPtr(index, user_ptr)) - { - return -1; - } - - // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized - // matrices. - switch (rigid_body.m_joint_type) - { - case REVOLUTE: - rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); - rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); - rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - case PRISMATIC: - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); - rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); - rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); - break; - case FIXED: - // NOTE/TODO: dimension really should be zero .. - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - case 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 ! - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - default: - bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type); - return -1; - } - } - - // 4 assign degree of freedom indices & build per-joint-type index arrays - if (-1 == m_impl->generateIndexSets()) - { - bt_id_error_message("generating index sets\n"); - return -1; - } - - // 5 do some pre-computations .. - m_impl->calculateStaticData(); - - // 6. make sure all user forces are set to zero, as this might not happen - // in the vector ctors. - m_impl->clearAllUserForcesAndMoments(); - - m_is_finalized = true; - return 0; -} - -int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) -{ - return m_impl->setGravityInWorldFrame(gravity); -} - -int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const -{ - return m_impl->getJointType(body_index, joint_type); -} - -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 -{ - return m_impl->getDoFOffset(body_index, q_offset); -} - -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) -{ - return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); -} - -int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment) -{ - return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); -} - -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 -{ - return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); -} - -int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const -{ - return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); -} - -void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); } - -int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) -{ - return m_impl->addUserForce(body_index, body_force); -} - -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 deleted file mode 100644 index 7b852f976c..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp +++ /dev/null @@ -1,367 +0,0 @@ -#ifndef MULTIBODYTREE_HPP_ -#define MULTIBODYTREE_HPP_ - -#include "IDConfig.hpp" -#include "IDMath.hpp" - -namespace btInverseDynamics -{ -/// Enumeration of supported joint types -enum JointType -{ - /// no degree of freedom, moves with parent - FIXED = 0, - /// one rotational degree of freedom relative to parent - REVOLUTE, - /// one translational degree of freedom relative to parent - PRISMATIC, - /// six degrees of freedom relative to parent - FLOATING, - /// three degrees of freedom, relative to parent - SPHERICAL -}; - -/// Interface class for calculating inverse dynamics for tree structured -/// multibody systems -/// -/// Note on degrees of freedom -/// The q vector contains the generalized coordinate set defining the tree's configuration. -/// Every joint adds elements that define the corresponding link's frame pose relative to -/// its parent. For the joint types that is: -/// - FIXED: none -/// - REVOLUTE: angle of rotation [rad] -/// - PRISMATIC: displacement [m] -/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] -/// (in that order) -/// - 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. -/// -/// Note on generalized forces: analogous to u, i.e., -/// - FIXED: none -/// - REVOLUTE: moment [Nm], about joint axis -/// - PRISMATIC: force [N], along joint axis -/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame -/// (in that order) -/// - SPHERICAL: moment vector [Nm] -/// TODO - force element interface (friction, springs, dampers, etc) -/// - gears and motor inertia -class MultiBodyTree -{ -public: - ID_DECLARE_ALIGNED_ALLOCATOR(); - /// The contructor. - /// Initialization & allocation is via addBody and buildSystem calls. - MultiBodyTree(); - /// the destructor. This also deallocates all memory - ~MultiBodyTree(); - - /// Add body to the system. this allocates memory and not real-time safe. - /// This only adds the data to an initial cache. After all bodies have been - /// added, - /// the system is setup using the buildSystem call - /// @param body_index index of the body to be added. Must >=0, <number of bodies, - /// and index of parent must be < index of body - /// @param parent_index index of the parent body - /// The root of the tree has index 0 and its parent (the world frame) - /// is assigned index -1 - /// the rotation and translation relative to the parent are taken as - /// pose of the root body relative to the world frame. Other parameters - /// are ignored - /// @param JointType type of joint connecting the body to the parent - /// @param mass the mass of the body - /// @param body_r_body_com the center of mass of the body relative to and - /// described in - /// the body fixed frame, which is located in the joint axis connecting - /// the body to its parent - /// @param body_I_body the moment of inertia of the body w.r.t the body-fixed - /// frame - /// (ie, the reference point is the origin of the body-fixed frame and - /// the matrix is written - /// w.r.t. those unit vectors) - /// @param parent_r_parent_body_ref position of joint relative to the parent - /// body's reference frame - /// for q=0, written in the parent bodies reference frame - /// @param body_axis_of_motion translation/rotation axis in body-fixed frame. - /// Ignored for joints that are not revolute or prismatic. - /// must be a unit vector. - /// @param body_T_parent_ref transform matrix from parent to body reference - /// frame for q=0. - /// This is the matrix transforming a vector represented in the - /// parent's reference frame into one represented - /// in this body's reference frame. - /// ie, if parent_vec is a vector in R^3 whose components are w.r.t to - /// the parent's reference frame, - /// then the same vector written w.r.t. this body's frame (for q=0) is - /// given by - /// body_vec = parent_R_body_ref * parent_vec - /// @param user_ptr pointer to user data - /// @param user_int pointer to user integer - /// @return 0 on success, -1 on error - int 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); - /// set policy for invalid mass properties - /// @param flag if true, invalid mass properties are accepted, - /// the default is false - void setAcceptInvalidMassParameters(bool flag); - /// @return the mass properties policy flag - bool getAcceptInvalidMassProperties() const; - /// build internal data structures - /// call this after all bodies have been added via addBody - /// @return 0 on success, -1 on error - int finalize(); - /// pretty print ascii description of tree to stdout - void printTree(); - /// 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. - /// @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 - /// @param joint_forces this is where the resulting joint forces will be - /// stored. dim(joint_forces) = dim(u) - /// @return 0 on success, -1 on error - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - /// Calculate joint space mass matrix - /// @param q generalized coordinates - /// @param initialize_matrix if true, initialize mass matrix with zero. - /// If mass_matrix is initialized to zero externally and only used - /// for mass matrix computations for the same system, it is safe to - /// set this to false. - /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix - /// is also populated, otherwise not. - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); - - /// Calculate joint space mass matrix. - /// This version will update kinematics, initialize all mass_matrix elements to zero and - /// populate all mass matrix entries. - /// @param q generalized coordinates - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, matxx* mass_matrix); - - /// 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 - - /// set gravitational acceleration - /// the default is [0;0;-9.8] in the world frame - /// @param gravity the gravitational acceleration in world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// returns number of bodies in tree - int numBodies() const; - /// returns number of mechanical degrees of freedom (dimension of q-vector) - int numDoFs() const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// get center of mass of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyCoM(const int body_index, vec3* world_com) const; - /// get transform from of a body-fixed frame to the world frame - /// @param body_index index for frame/body - /// @param world_T_body pointer for return data - /// @return 0 on success, -1 on error - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// get absolute angular velocity for a body, represented in the world frame - /// @param body_index index for frame/body - /// @param world_omega pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// get linear velocity of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_velocity pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// get linear velocity of a body's CoM, represented in world frame - /// (not required for inverse dynamics, provided for convenience) - /// @param body_index index for frame/body - /// @param world_vel_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// get origin of a body-fixed frame, represented in world frame - /// NOTE: this will include the gravitational acceleration, so the actual acceleration is - /// obtainened by setting gravitational acceleration to zero, or subtracting it. - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - -#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 - - /// returns the (internal) index of body - /// @param body_index is the index of a body - /// @param parent_index pointer to where parent index will be stored - /// @return 0 on success, -1 on error - int getParentIndex(const int body_index, int* parent_index) const; - /// get joint type - /// @param body_index index of the body - /// @param joint_type the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointType(const int body_index, JointType* joint_type) const; - /// get joint type as string - /// @param body_index index of the body - /// @param joint_type string naming the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointTypeStr(const int body_index, const char** joint_type) const; - /// get offset 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; - /// 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; - /// 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; - /// get offset for degrees of freedom of this body into the q-vector - /// @param body_index index of the body - /// @param q_offset offset the q vector - /// @return -1 on error, 0 on success - int getDoFOffset(const int body_index, int* q_offset) const; - /// get user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int getUserInt(const int body_index, int* user_int) const; - /// get user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int getUserPtr(const int body_index, void** user_ptr) const; - /// set user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int setUserInt(const int body_index, const int user_int); - /// set user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int setUserPtr(const int body_index, void* const user_ptr); - /// set mass for a body - /// @param body_index index of the body - /// @param mass the mass to set - /// @return 0 on success, -1 on failure - int setBodyMass(const int body_index, const idScalar mass); - /// set first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_mass_moment the vector to set - /// @return 0 on success, -1 on failure - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - /// set second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - /// get mass for a body - /// @param body_index index of the body - /// @param mass the mass - /// @return 0 on success, -1 on failure - int getBodyMass(const int body_index, idScalar* mass) const; - /// get first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_moment the vector - /// @return 0 on success, -1 on failure - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - /// get second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// set all user forces and moments to zero - void clearAllUserForcesAndMoments(); - /// Add an external force to a body, acting at the origin of the body-fixed frame. - /// Calls to addUserForce are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_force the force represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserForce(const int body_index, const vec3& body_force); - /// Add an external moment to a body. - /// Calls to addUserMoment are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_moment the moment represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserMoment(const int body_index, const vec3& body_moment); - -private: - // flag indicating if system has been initialized - bool m_is_finalized; - // flag indicating if mass properties are physically valid - bool m_mass_parameters_are_valid; - // flag defining if unphysical mass parameters are accepted - bool m_accept_invalid_mass_parameters; - // This struct implements the inverse dynamics calculations - class MultiBodyImpl; - MultiBodyImpl* m_impl; - // cache data structure for initialization - class InitCache; - InitCache* m_init_cache; -}; -} // namespace btInverseDynamics -#endif // MULTIBODYTREE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp deleted file mode 100644 index fe4f102513..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef INVDYNEIGENINTERFACE_HPP_ -#define INVDYNEIGENINTERFACE_HPP_ -#include "../IDConfig.hpp" -namespace btInverseDynamics -{ -#define BT_ID_HAVE_MAT3X - -#ifdef BT_USE_DOUBLE_PRECISION -typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign> vecx; -typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> vec3; -typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> mat33; -typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx; -typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x; -#else -typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::DontAlign> vecx; -typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> vec3; -typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> mat33; -typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx; -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 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; -} - -} // namespace btInverseDynamics -#endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp deleted file mode 100644 index 0c398a3727..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ /dev/null @@ -1,202 +0,0 @@ -#ifndef IDLINEARMATHINTERFACE_HPP_ -#define IDLINEARMATHINTERFACE_HPP_ - -#include <cstdlib> - -#include "../IDConfig.hpp" - -#include "../../LinearMath/btMatrix3x3.h" -#include "../../LinearMath/btVector3.h" -#include "../../LinearMath/btMatrixX.h" -#define BT_ID_HAVE_MAT3X - -namespace btInverseDynamics -{ -class vec3; -class vecx; -class mat33; -typedef btMatrixX<idScalar> matxx; - -class vec3 : public btVector3 -{ -public: - vec3() : btVector3() {} - vec3(const btVector3& btv) { *this = btv; } - idScalar& operator()(int i) { return (*this)[i]; } - const idScalar& operator()(int i) const { return (*this)[i]; } - int size() const { return 3; } - const vec3& operator=(const btVector3& rhs) - { - *static_cast<btVector3*>(this) = rhs; - return *this; - } -}; - -class mat33 : public btMatrix3x3 -{ -public: - mat33() : btMatrix3x3() {} - mat33(const btMatrix3x3& btm) { *this = btm; } - idScalar& operator()(int i, int j) { return (*this)[i][j]; } - const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } - const mat33& operator=(const btMatrix3x3& rhs) - { - *static_cast<btMatrix3x3*>(this) = rhs; - return *this; - } - friend mat33 operator*(const idScalar& s, const mat33& a); - friend mat33 operator/(const mat33& a, const idScalar& s); -}; - -inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); } - -inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } - -class vecx : public btVectorX<idScalar> -{ -public: - vecx(int size) : btVectorX<idScalar>(size) {} - const vecx& operator=(const btVectorX<idScalar>& rhs) - { - *static_cast<btVectorX<idScalar>*>(this) = rhs; - return *this; - } - - idScalar& operator()(int i) { return (*this)[i]; } - const idScalar& operator()(int i) const { return (*this)[i]; } - - friend vecx operator*(const vecx& a, const idScalar& s); - friend vecx operator*(const idScalar& s, const vecx& a); - - friend vecx operator+(const vecx& a, const vecx& b); - friend vecx operator-(const vecx& a, const vecx& b); - friend vecx operator/(const vecx& a, const idScalar& s); -}; - -inline vecx operator*(const vecx& a, const idScalar& s) -{ - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) - { - result(i) = a(i) * s; - } - return result; -} -inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) -{ - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) - { - 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++) - { - result(i) = a(i) + b(i); - } - - return result; -} - -inline vecx operator-(const vecx& a, const vecx& b) -{ - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) - { - 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++) - { - result(i) = a(i) - b(i); - } - return result; -} -inline vecx operator/(const vecx& a, const idScalar& s) -{ - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) - { - result(i) = a(i) / s; - } - - return result; -} - -// use btMatrixX to implement 3xX matrix -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(); - } - 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 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 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 deleted file mode 100644 index 1c786095e7..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp +++ /dev/null @@ -1,489 +0,0 @@ -/// @file Built-In Matrix-Vector functions -#ifndef IDMATVEC_HPP_ -#define IDMATVEC_HPP_ - -#include <cstdlib> - -#include "../IDConfig.hpp" -#define BT_ID_HAVE_MAT3X - -namespace btInverseDynamics -{ -class vec3; -class vecx; -class mat33; -class matxx; -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 -{ -public: - idScalar& operator()(int i) { return m_data[i]; } - const idScalar& operator()(int i) const { return m_data[i]; } - const int size() const { return 3; } - const vec3& operator=(const vec3& rhs); - const vec3& operator+=(const vec3& b); - const vec3& operator-=(const vec3& b); - vec3 cross(const vec3& b) const; - idScalar dot(const vec3& b) const; - - friend vec3 operator*(const mat33& a, const vec3& b); - friend vec3 operator*(const vec3& a, const idScalar& s); - friend vec3 operator*(const idScalar& s, const vec3& a); - - friend vec3 operator+(const vec3& a, const vec3& b); - friend vec3 operator-(const vec3& a, const vec3& b); - friend vec3 operator/(const vec3& a, const idScalar& s); - -private: - idScalar m_data[3]; -}; - -class mat33 -{ -public: - idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } - const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } - const mat33& operator=(const mat33& rhs); - mat33 transpose() const; - const mat33& operator+=(const mat33& b); - const mat33& operator-=(const mat33& b); - - friend mat33 operator*(const mat33& a, const mat33& b); - friend vec3 operator*(const mat33& a, const vec3& b); - friend mat33 operator*(const mat33& a, const idScalar& s); - friend mat33 operator*(const idScalar& s, const mat33& a); - friend mat33 operator+(const mat33& a, const mat33& b); - friend mat33 operator-(const mat33& a, const mat33& b); - friend mat33 operator/(const mat33& a, const idScalar& s); - -private: - // layout is [0,1,2;3,4,5;6,7,8] - idScalar m_data[9]; -}; - -class vecx -{ -public: - vecx(int size) : m_size(size) - { - m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size)); - } - ~vecx() { idFree(m_data); } - const vecx& operator=(const vecx& rhs); - idScalar& operator()(int i) { return m_data[i]; } - const idScalar& operator()(int i) const { return m_data[i]; } - const int& size() const { return m_size; } - - friend vecx operator*(const vecx& a, const idScalar& s); - friend vecx operator*(const idScalar& s, const vecx& a); - - friend vecx operator+(const vecx& a, const vecx& b); - friend vecx operator-(const vecx& a, const vecx& b); - friend vecx operator/(const vecx& a, const idScalar& s); - -private: - int m_size; - idScalar* m_data; -}; - -class matxx -{ -public: - 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); } - 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]; } - const int& rows() const { return m_rows; } - const int& cols() const { return m_cols; } - -private: - int m_rows; - int m_cols; - idScalar* m_data; -}; - -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 -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; -}; - -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) - { - memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); - } - return *this; -} - -inline vec3 vec3::cross(const vec3& b) const -{ - vec3 result; - result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; - result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; - result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; - - return result; -} - -inline idScalar vec3::dot(const vec3& b) const -{ - return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; -} - -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 -{ - mat33 result; - result.m_data[0] = m_data[0]; - result.m_data[1] = m_data[3]; - result.m_data[2] = m_data[6]; - result.m_data[3] = m_data[1]; - result.m_data[4] = m_data[4]; - result.m_data[5] = m_data[7]; - result.m_data[6] = m_data[2]; - result.m_data[7] = m_data[5]; - result.m_data[8] = m_data[8]; - - return result; -} - -inline mat33 operator*(const mat33& a, const mat33& b) -{ - mat33 result; - result.m_data[0] = - a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; - result.m_data[1] = - a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; - result.m_data[2] = - a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; - result.m_data[3] = - a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; - result.m_data[4] = - a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; - result.m_data[5] = - a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; - result.m_data[6] = - a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; - result.m_data[7] = - a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; - result.m_data[8] = - a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; - - return result; -} - -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++) - { - m_data[i] -= b.m_data[i]; - } - return *this; -} - -inline vec3 operator*(const mat33& a, const vec3& b) -{ - vec3 result; - - result.m_data[0] = - a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; - result.m_data[1] = - a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; - result.m_data[2] = - a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; - - return result; -} - -inline const vec3& vec3::operator+=(const vec3& b) -{ - for (int i = 0; i < 3; i++) - { - m_data[i] += b.m_data[i]; - } - return *this; -} - -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) -{ - mat33 result; - for (int i = 0; i < 9; i++) - { - result.m_data[i] = a.m_data[i] * s; - } - return result; -} - -inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } - -inline vec3 operator*(const vec3& a, const idScalar& s) -{ - vec3 result; - for (int i = 0; i < 3; i++) - { - result.m_data[i] = a.m_data[i] * s; - } - return result; -} -inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; } - -inline mat33 operator+(const mat33& a, const mat33& b) -{ - mat33 result; - for (int i = 0; i < 9; i++) - { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - return result; -} -inline vec3 operator+(const vec3& a, const vec3& b) -{ - vec3 result; - for (int i = 0; i < 3; i++) - { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - return result; -} - -inline mat33 operator-(const mat33& a, const mat33& b) -{ - mat33 result; - for (int i = 0; i < 9; i++) - { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; -} -inline vec3 operator-(const vec3& a, const vec3& b) -{ - vec3 result; - for (int i = 0; i < 3; i++) - { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; -} - -inline mat33 operator/(const mat33& a, const idScalar& s) -{ - mat33 result; - for (int i = 0; i < 9; i++) - { - result.m_data[i] = a.m_data[i] / s; - } - return result; -} - -inline vec3 operator/(const vec3& a, const idScalar& s) -{ - vec3 result; - for (int i = 0; i < 3; i++) - { - result.m_data[i] = a.m_data[i] / s; - } - return result; -} - -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) - { - memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); - } - return *this; -} -inline vecx operator*(const vecx& a, const idScalar& s) -{ - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) - { - result.m_data[i] = a.m_data[i] * s; - } - return result; -} -inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) -{ - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) - { - 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++) - { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - - return result; -} -inline vecx operator-(const vecx& a, const vecx& b) -{ - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) - { - 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++) - { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; -} -inline vecx operator/(const vecx& a, const idScalar& s) -{ - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) - { - result.m_data[i] = a.m_data[i] / s; - } - - 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 setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) -{ - (*m)(row, col) = val; -} - -} // namespace btInverseDynamics -#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp deleted file mode 100644 index ec9a562295..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ /dev/null @@ -1,1286 +0,0 @@ -#include "MultiBodyTreeImpl.hpp" - -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) -#endif -{ -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - resize(m_m3x, m_num_dofs); -#endif - m_body_list.resize(num_bodies_); - m_parent_index.resize(num_bodies_); - m_child_indices.resize(num_bodies_); - m_user_int.resize(num_bodies_); - m_user_ptr.resize(num_bodies_); - - m_world_gravity(0) = 0.0; - m_world_gravity(1) = 0.0; - m_world_gravity(2) = -9.8; -} - -const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const -{ - switch (type) - { - case FIXED: - return "fixed"; - case REVOLUTE: - return "revolute"; - case PRISMATIC: - return "prismatic"; - case FLOATING: - return "floating"; - case SPHERICAL: - return "spherical"; - } - return "error: invalid"; -} - -inline void indent(const int &level) -{ - for (int j = 0; j < level; j++) - id_printf(" "); // indent -} - -void MultiBodyTree::MultiBodyImpl::printTree() -{ - id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); - printTree(0, 0); -} - -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)); - id_printf("q_index= %d\n", body.m_q_index); - id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); - id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); - - id_printf("mass = %f\n", body.m_mass); - id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), - body.m_body_mass_com(2)); - id_printf( - "I_o= [%f %f %f;\n" - " %f %f %f;\n" - " %f %f %f]\n", - body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), - body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), - body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); - - id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), - body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); - } -} -int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const -{ - switch (type) - { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - case SPHERICAL: - return 3; - } - bt_id_error_message("unknown joint type %d\n", type); - return 0; -} - -void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) -{ - // this is adapted from URDF2Bullet. - // TODO: fix this and print proper graph (similar to git --log --graph) - int num_children = m_child_indices[index].size(); - - indentation += 2; - int count = 0; - - for (int i = 0; i < num_children; i++) - { - int child_index = m_child_indices[index][i]; - indent(indentation); - id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, - jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, - m_body_list[index].m_q_index, - m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); - // first grandchild - printTree(child_index, indentation); - } -} - -int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) -{ - m_world_gravity = gravity; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::generateIndexSets() -{ - m_body_revolute_list.resize(0); - m_body_prismatic_list.resize(0); - int q_index = 0; - for (idArrayIdx i = 0; i < m_body_list.size(); i++) - { - RigidBody &body = m_body_list[i]; - body.m_q_index = -1; - switch (body.m_joint_type) - { - case REVOLUTE: - m_body_revolute_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case PRISMATIC: - m_body_prismatic_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case FIXED: - // do nothing - break; - case FLOATING: - m_body_floating_list.push_back(i); - body.m_q_index = q_index; - q_index += 6; - break; - 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) - { - 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++) - { - const int &parent = m_parent_index[child]; - if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) - { - m_child_indices[parent].push_back(child); - } - else - { - if (-1 == parent) - { - // multiple bodies are directly linked to the environment, ie, not a single root - bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); - } - else - { - // should never happen - bt_id_error_message( - "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", - child, parent, static_cast<int>(m_parent_index.size())); - } - return -1; - } - } - - return 0; -} - -void MultiBodyTree::MultiBodyImpl::calculateStaticData() -{ - // relative kinematics that are not a function of q, u, dot_u - for (idArrayIdx i = 0; i < m_body_list.size(); i++) - { - RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) - { - case REVOLUTE: - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - break; - case PRISMATIC: - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - break; - case FIXED: - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - break; - case FLOATING: - // no static data - break; - 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. -#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 // - } -} - -int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, - const vecx &dot_u, vecx *joint_forces) -{ - if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || - joint_forces->size() != m_num_dofs) - { - 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++) - { - RigidBody &body = m_body_list[i]; - // 3.4 update dynamic terms (rate of change of angular & linear momentum) - body.m_eom_lhs_rotational = - body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + - body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - - body.m_body_moment_user; - body.m_eom_lhs_translational = - body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + - body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - - body.m_body_force_user; - } - - // 3. calculate full set of forces at parent joint - // (not directly calculating the joint force along the free direction - // simplifies inclusion of fixed joints. - // An alternative would be to fuse bodies in a pre-processing step, - // but that would make changing masses online harder (eg, payload masses - // added with fixed joints to a gripper) - // Also, this enables adding zero weight bodies as a way to calculate frame poses - // for force elements, etc. - - for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) - { - // sum of forces and moments acting on this body from its children - vec3 sum_f_children; - vec3 sum_m_children; - setZero(sum_f_children); - setZero(sum_m_children); - for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); - child_list_idx++) - { - const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; - vec3 child_joint_force_in_this_frame = - child.m_body_T_parent.transpose() * child.m_force_at_joint; - sum_f_children -= child_joint_force_in_this_frame; - sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + - child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); - } - RigidBody &body = m_body_list[body_idx]; - - body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; - body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; - } - - // 4. Calculate Joint forces. - // These are the components of force_at_joint/moment_at_joint - // in the free directions given by Jac_JT/Jac_JR - // 4.1 revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); - } - // 4.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); - } - // 4.3 floating bodies (6-DoF joints) - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); - (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); - (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); - - (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); - (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); - (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); - } - - // 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())); - 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++) - { - 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); - } - } - // 1.2 for prismatic - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - body.m_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); - } - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * - transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - body.m_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(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_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; - } - } - - for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) - { - //todo: review - RigidBody &body = m_body_list[m_body_spherical_list[i]]; - - mat33 T; - - T = transformX(q(body.m_q_index)) * - transformY(q(body.m_q_index + 1)) * - transformZ(q(body.m_q_index + 2)); - body.m_body_T_parent = T * body.m_body_T_parent_ref; - - body.m_parent_pos_parent_body(0)=0; - body.m_parent_pos_parent_body(1)=0; - body.m_parent_pos_parent_body(2)=0; - - 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) - // NOTE: this should be optimized by specializing for different body types - // (e.g., relative rotation is always zero for prismatic joints, etc.) - - // calculations for root body - { - RigidBody &body = m_body_list[0]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) - // not required right now, added here for debugging purposes - body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_body_T_world = body.m_body_T_parent; - - 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++) - { - RigidBody &body = m_body_list[i]; - RigidBody &parent = m_body_list[m_parent_index[i]]; - // 2.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) not required right now added here for debugging purposes - body.m_body_pos = - body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); - body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; - - 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; -} - -#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; - 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; -} -#endif - -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; - Jac_JR(1) = 0; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 1: - Jac_JR(0) = 0; - Jac_JR(1) = 1; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 2: - Jac_JR(0) = 0; - Jac_JR(1) = 0; - Jac_JR(2) = 1; - setZero(Jac_JT); - break; - // translational part - case 3: - setZero(Jac_JR); - Jac_JT(0) = 1; - Jac_JT(1) = 0; - Jac_JT(2) = 0; - break; - case 4: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 1; - Jac_JT(2) = 0; - break; - case 5: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 0; - Jac_JT(2) = 1; - break; - } -} - -static inline int jointNumDoFs(const JointType &type) -{ - switch (type) - { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - case SPHERICAL: - return 3; - } - // this should never happen - bt_id_error_message("invalid joint type\n"); - // TODO add configurable abort/crash function - abort(); - return 0; -} - -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. - - 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())); - 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 (update_kinematics) - { - // 1. update relative kinematics - // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // from reference orientation (q=0) of body-fixed frame to current orientation - mat33 body_T_body_ref; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); - body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; - } - // 1.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) - { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * - transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - } - - for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) - { - //todo: review - RigidBody &body = m_body_list[m_body_spherical_list[i]]; - - mat33 T; - - T = transformX(q(body.m_q_index)) * - transformY(q(body.m_q_index + 1)) * - transformZ(q(body.m_q_index + 2)); - body.m_body_T_parent = T * body.m_body_T_parent_ref; - - body.m_parent_pos_parent_body(0)=0; - body.m_parent_pos_parent_body(1)=0; - body.m_parent_pos_parent_body(2)=0; - - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - } - } - for (int i = m_body_list.size() - 1; i >= 0; i--) - { - RigidBody &body = m_body_list[i]; - // calculate mass, center of mass and inertia of "composite rigid body", - // ie, sub-tree starting at current body - body.m_subtree_mass = body.m_mass; - body.m_body_subtree_mass_com = body.m_body_mass_com; - body.m_body_subtree_I_body = body.m_body_I_body; - - for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) - { - RigidBody &child = m_body_list[m_child_indices[i][c]]; - mat33 body_T_child = child.m_body_T_parent.transpose(); - - body.m_subtree_mass += child.m_subtree_mass; - body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + - child.m_parent_pos_parent_body * child.m_subtree_mass; - body.m_body_subtree_I_body += - body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; - - if (child.m_subtree_mass > 0) - { - // Shift the reference point for the child subtree inertia using the - // Huygens-Steiner ("parallel axis") theorem. - // (First shift from child origin to child com, then from there to this body's - // origin) - vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; - mat33 tilde_r_child_com = tildeOperator(r_com); - mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); - body.m_body_subtree_I_body += - child.m_subtree_mass * - (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); - } - } - } - - for (int i = m_body_list.size() - 1; i >= 0; i--) - { - const RigidBody &body = m_body_list[i]; - - // determine DoF-range for body - const int q_index_min = body.m_q_index; - const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; - // loop over the DoFs used by this body - // local joint jacobians (ok as is for 1-DoF joints) - vec3 Jac_JR = body.m_Jac_JR; - vec3 Jac_JT = body.m_Jac_JT; - for (int col = q_index_max; col >= q_index_min; col--) - { - // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) - { - setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); - } - 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); - vec3 body_eom_trans = - body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); - setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix); - - // 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 (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); - } - } - // 2. ancestor dofs - int child_idx = i; - int parent_idx = m_parent_index[i]; - while (parent_idx >= 0) - { - const RigidBody &child_body = m_body_list[child_idx]; - const RigidBody &parent_body = m_body_list[parent_idx]; - - const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); - body_eom_rot = parent_T_child * body_eom_rot; - body_eom_trans = parent_T_child * body_eom_trans; - body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); - - const int parent_body_q_index_min = parent_body.m_q_index; - const int parent_body_q_index_max = - parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; - vec3 Jac_JR = parent_body.m_Jac_JR; - vec3 Jac_JT = parent_body.m_Jac_JT; - for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) - { - 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) - { - 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); - setMatxxElem(col, row, Mrc, mass_matrix); - } - - child_idx = parent_idx; - parent_idx = m_parent_index[child_idx]; - } - } - } - } - - if (set_lower_triangular_matrix) - { - for (int col = 0; col < m_num_dofs; col++) - { - for (int row = 0; row < col; row++) - { - setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); - } - } - } - return 0; -} - -// 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; \ - } \ - } while (0) - -int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *p = m_parent_index[body_index]; - return 0; -} - -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 -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_ptr = m_user_ptr[body_index]; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_int[body_index] = user_int; - return 0; -} - -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 -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - if (body.m_mass > 0) - { - *world_com = body.m_body_T_world.transpose() * - (body.m_body_pos + body.m_body_mass_com / body.m_mass); - } - else - { - *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); - } - return 0; -} - -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 -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, - vec3 *world_velocity) const -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - vec3 com; - if (body.m_mass > 0) - { - com = body.m_body_mass_com / body.m_mass; - } - else - { - com(0) = 0; - com(1) = 0; - com(2) = 0; - } - - *world_velocity = - body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); - return 0; -} - -int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, - vec3 *world_acceleration) const -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; - return 0; -} - -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 -{ - 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::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::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) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_mass = mass; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3 &first_mass_moment) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_mass_com = first_mass_moment; - return 0; -} -int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, - const mat33 &second_mass_moment) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_I_body = second_mass_moment; - return 0; -} -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 -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *first_mass_moment = m_body_list[body_index].m_body_mass_com; - return 0; -} -int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, - mat33 *second_mass_moment) const -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *second_mass_moment = m_body_list[body_index].m_body_I_body; - return 0; -} - -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) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_force_user += body_force; - return 0; -} - -int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) -{ - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_moment_user += body_moment; - return 0; -} - -#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::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::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 deleted file mode 100644 index eabdbe161b..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ /dev/null @@ -1,288 +0,0 @@ -// The structs and classes defined here provide a basic inverse fynamics implementation used -// by MultiBodyTree -// User interaction should be through MultiBodyTree - -#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_ -#define MULTI_BODY_REFERENCE_IMPL_HPP_ - -#include "../IDConfig.hpp" -#include "../MultiBodyTree.hpp" - -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 -{ - ID_DECLARE_ALIGNED_ALLOCATOR(); - // 1 Inertial properties - /// Mass - idScalar m_mass; - /// Mass times center of gravity in body-fixed frame - vec3 m_body_mass_com; - /// Moment of inertia w.r.t. body-fixed frame - mat33 m_body_I_body; - - // 2 dynamic properties - /// Left-hand side of the body equation of motion, translational part - vec3 m_eom_lhs_translational; - /// Left-hand side of the body equation of motion, rotational part - vec3 m_eom_lhs_rotational; - /// Force acting at the joint when the body is cut from its parent; - /// includes impressed joint force in J_JT direction, - /// as well as constraint force, - /// in body-fixed frame - vec3 m_force_at_joint; - /// Moment acting at the joint when the body is cut from its parent; - /// includes impressed joint moment in J_JR direction, and constraint moment - /// in body-fixed frame - vec3 m_moment_at_joint; - /// external (user provided) force acting at the body-fixed frame's origin, written in that - /// frame - vec3 m_body_force_user; - /// external (user provided) moment acting at the body-fixed frame's origin, written in that - /// frame - vec3 m_body_moment_user; - // 3 absolute kinematic properties - /// Position of body-fixed frame relative to world frame - /// this is currently only for debugging purposes - vec3 m_body_pos; - /// Absolute velocity of body-fixed frame - vec3 m_body_vel; - /// Absolute acceleration of body-fixed frame - /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational - /// acceleration! - vec3 m_body_acc; - /// Absolute angular velocity - vec3 m_body_ang_vel; - /// Absolute angular acceleration - /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational - /// acceleration! - vec3 m_body_ang_acc; - - // 4 relative kinematic properties. - // these are in the parent body frame - /// Transform from world to body-fixed frame; - /// this is currently only for debugging purposes - mat33 m_body_T_world; - /// Transform from parent to body-fixed frame - mat33 m_body_T_parent; - /// Vector from parent to child frame in parent frame - vec3 m_parent_pos_parent_body; - /// Relative angular velocity - vec3 m_body_ang_vel_rel; - /// Relative linear velocity - vec3 m_parent_vel_rel; - /// Relative angular acceleration - vec3 m_body_ang_acc_rel; - /// Relative linear acceleration - vec3 m_parent_acc_rel; - - // 5 Data describing the joint type and geometry - /// Type of joint - JointType m_joint_type; - /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame - /// Components are in body-fixed frame of the parent - vec3 m_parent_pos_parent_body_ref; - /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame - mat33 m_body_T_parent_ref; - /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute - /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. - /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. - /// (NOTE: dimensions will have to be dynamic for additional joint types!) - vec3 m_Jac_JR; - /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute - /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. - /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. - /// (NOTE: dimensions might have to be dynamic for additional joint types!) - vec3 m_Jac_JT; - /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT - vec3 m_parent_Jac_JT; - /// Start of index range for the position degree(s) of freedom describing this body's motion - /// relative to - /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. - int m_q_index; - - // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" - /// mass of the subtree rooted in this body - idScalar m_subtree_mass; - /// center of mass * mass for subtree rooted in this body, in body-fixed frame - vec3 m_body_subtree_mass_com; - /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame - mat33 m_body_subtree_I_body; - -#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; -#endif -}; - -/// The MBS implements a tree structured multibody system -class MultiBodyTree::MultiBodyImpl -{ - friend class MultiBodyTree; - -public: - ID_DECLARE_ALIGNED_ALLOCATOR(); - - enum KinUpdateType - { - POSITION_ONLY, - POSITION_VELOCITY, - POSITION_VELOCITY_ACCELERATION - }; - - /// constructor - /// @param num_bodies the number of bodies in the system - /// @param num_dofs number of degrees of freedom in the system - MultiBodyImpl(int num_bodies_, int num_dofs_); - - /// \copydoc MultiBodyTree::calculateInverseDynamics - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - ///\copydoc MultiBodyTree::calculateMassMatrix - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); - /// 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); -#endif - /// generate additional index sets from the parent_index array - /// @return -1 on error, 0 on success - int generateIndexSets(); - /// set gravity acceleration in world frame - /// @param gravity gravity vector in the world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// pretty print tree - void printTree(); - /// print tree data - void printTreeData(); - /// initialize fixed data - void calculateStaticData(); - /// \copydoc MultiBodyTree::getBodyFrame - int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; - /// \copydoc MultiBodyTree::getParentIndex - int getParentIndex(const int body_index, int* m_parent_index); - /// \copydoc MultiBodyTree::getJointType - int getJointType(const int body_index, JointType* joint_type) const; - /// \copydoc MultiBodyTree::getJointTypeStr - int getJointTypeStr(const int body_index, const char** joint_type) const; - /// \copydoc MultiBodyTree::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 - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// \copydoc MultiBodyTree::getBodyCoM - int getBodyCoM(const int body_index, vec3* world_com) const; - /// \copydoc MultiBodyTree::getBodyTransform - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// \copydoc MultiBodyTree::getBodyAngularVelocity - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// \copydoc MultiBodyTree::getBodyLinearVelocity - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// \copydoc MultiBodyTree::getBodyAngularAcceleration - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// \copydoc MultiBodyTree::getBodyLinearAcceleration - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - /// \copydoc MultiBodyTree::getUserInt - int getUserInt(const int body_index, int* user_int) const; - /// \copydoc MultiBodyTree::getUserPtr - int getUserPtr(const int body_index, void** user_ptr) const; - /// \copydoc MultiBodyTree::setUserInt - int setUserInt(const int body_index, const int user_int); - /// \copydoc MultiBodyTree::setUserPtr - int setUserPtr(const int body_index, void* const user_ptr); - ///\copydoc MultiBodytTree::setBodyMass - int setBodyMass(const int body_index, const idScalar mass); - ///\copydoc MultiBodytTree::setBodyFirstMassMoment - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - ///\copydoc MultiBodytTree::setBodySecondMassMoment - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - ///\copydoc MultiBodytTree::getBodyMass - int getBodyMass(const int body_index, idScalar* mass) const; - ///\copydoc MultiBodytTree::getBodyFirstMassMoment - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - ///\copydoc MultiBodytTree::getBodySecondMassMoment - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments - void clearAllUserForcesAndMoments(); - /// \copydoc MultiBodyTree::addUserForce - int addUserForce(const int body_index, const vec3& body_force); - /// \copydoc MultiBodyTree::addUserMoment - int addUserMoment(const int body_index, const vec3& body_moment); - -private: - // debug function. print tree structure to stdout - void printTree(int index, int indentation); - // get string representation of JointType (for debugging) - const char* jointTypeToString(const JointType& type) const; - // get number of degrees of freedom from joint type - int bodyNumDoFs(const JointType& type) const; - // number of bodies in the system - int m_num_bodies; - // number of degrees of freedom - int m_num_dofs; - // Gravitational acceleration (in world frame) - vec3 m_world_gravity; - // vector of bodies in the system - // body 0 is used as an environment body and is allways fixed. - // The bodies are ordered such that a parent body always has an index - // smaller than its child. - idArray<RigidBody>::type m_body_list; - // Parent_index[i] is the index for i's parent body in body_list. - // This fully describes the tree. - idArray<int>::type m_parent_index; - // child_indices[i] contains a vector of indices of - // all children of the i-th body - idArray<idArray<int>::type>::type m_child_indices; - // Indices of rotary joints - idArray<int>::type m_body_revolute_list; - // Indices of prismatic joints - 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; -#endif -}; -} // namespace btInverseDynamics -#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp deleted file mode 100644 index a718db051e..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "MultiBodyTreeInitCache.hpp" - -namespace btInverseDynamics -{ -MultiBodyTree::InitCache::InitCache() -{ - m_inertias.resize(0); - m_joints.resize(0); - m_num_dofs = 0; - m_root_index = -1; -} - -int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, - const JointType joint_type, - const vec3& parent_r_parent_body_ref, - const mat33& body_T_parent_ref, - const vec3& body_axis_of_motion, const idScalar mass, - const vec3& body_r_body_com, const mat33& body_I_body, - const int user_int, void* user_ptr) -{ - switch (joint_type) - { - case REVOLUTE: - case PRISMATIC: - m_num_dofs += 1; - break; - case FIXED: - // does not add a degree of freedom - // m_num_dofs+=0; - break; - case SPHERICAL: - m_num_dofs += 3; - break; - case FLOATING: - m_num_dofs += 6; - break; - default: - bt_id_error_message("unknown joint type %d\n", joint_type); - return -1; - } - - 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); - return -1; - } - m_root_index = body_index; - } - - JointData joint; - joint.m_child = body_index; - joint.m_parent = parent_index; - joint.m_type = joint_type; - joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; - joint.m_child_T_parent_ref = body_T_parent_ref; - joint.m_child_axis_of_motion = body_axis_of_motion; - - InertiaData body; - body.m_mass = mass; - body.m_body_pos_body_com = body_r_body_com; - body.m_body_I_body = body_I_body; - - m_inertias.push_back(body); - m_joints.push_back(joint); - m_user_int.push_back(user_int); - m_user_ptr.push_back(user_ptr); - return 0; -} -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; - } - - *inertia = m_inertias[index]; - return 0; -} - -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; - } - *user_int = m_user_int[index]; - return 0; -} - -int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const -{ - if (index < 0 || index > static_cast<int>(m_user_ptr.size())) - { - bt_id_error_message("index out of range\n"); - return -1; - } - *user_ptr = m_user_ptr[index]; - return 0; -} - -int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const -{ - if (index < 0 || index > static_cast<int>(m_joints.size())) - { - bt_id_error_message("index out of range\n"); - return -1; - } - *joint = m_joints[index]; - return 0; -} - -int MultiBodyTree::InitCache::buildIndexSets() -{ - // NOTE: This function assumes that proper indices were provided - // User2InternalIndex from utils can be used to facilitate this. - - m_parent_index.resize(numBodies()); - for (idArrayIdx j = 0; j < m_joints.size(); j++) - { - const JointData& joint = m_joints[j]; - m_parent_index[joint.m_child] = joint.m_parent; - } - - return 0; -} -} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp deleted file mode 100644 index dbdb3ff604..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp +++ /dev/null @@ -1,113 +0,0 @@ -#ifndef MULTIBODYTREEINITCACHE_HPP_ -#define MULTIBODYTREEINITCACHE_HPP_ - -#include "../IDConfig.hpp" -#include "../IDMath.hpp" -#include "../MultiBodyTree.hpp" - -namespace btInverseDynamics -{ -/// Mass properties of a rigid body -struct InertiaData -{ - ID_DECLARE_ALIGNED_ALLOCATOR(); - - /// mass - idScalar m_mass; - /// vector from body-fixed frame to center of mass, - /// in body-fixed frame, multiplied by the mass - vec3 m_body_pos_body_com; - /// moment of inertia w.r.t. the origin of the body-fixed - /// frame, represented in that frame - mat33 m_body_I_body; -}; - -/// Joint properties -struct JointData -{ - ID_DECLARE_ALIGNED_ALLOCATOR(); - - /// type of joint - JointType m_type; - /// index of parent body - int m_parent; - /// index of child body - int m_child; - /// vector from parent's body-fixed frame to child's body-fixed - /// frame for q=0, written in the parent's body fixed frame - vec3 m_parent_pos_parent_child_ref; - /// Transform matrix converting vectors written in the parent's frame - /// into vectors written in the child's frame for q=0 - /// ie, child_vector = child_T_parent_ref * parent_vector; - mat33 m_child_T_parent_ref; - /// Axis of motion for 1 degree-of-freedom joints, - /// written in the child's frame - /// For revolute joints, the q-value is positive for a positive - /// rotation about this axis. - /// For prismatic joints, the q-value is positive for a positive - /// translation is this direction. - vec3 m_child_axis_of_motion; -}; - -/// Data structure to store data passed by the user. -/// This is used in MultiBodyTree::finalize to build internal data structures. -class MultiBodyTree::InitCache -{ -public: - ID_DECLARE_ALIGNED_ALLOCATOR(); - /// constructor - InitCache(); - ///\copydoc MultiBodyTree::addBody - int addBody(const int body_index, const int parent_index, const JointType joint_type, - const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, - const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com, - const mat33 &body_I_body, const int user_int, void *user_ptr); - /// build index arrays - /// @return 0 on success, -1 on failure - int buildIndexSets(); - /// @return number of degrees of freedom - int numDoFs() const { return m_num_dofs; } - /// @return number of bodies - int numBodies() const { return m_inertias.size(); } - /// get inertia data for index - /// @param index of the body - /// @param inertia pointer for return data - /// @return 0 on success, -1 on failure - int getInertiaData(const int index, InertiaData *inertia) const; - /// get joint data for index - /// @param index of the body - /// @param joint pointer for return data - /// @return 0 on success, -1 on failure - int getJointData(const int index, JointData *joint) const; - /// get parent index array (paren_index[i] is the index of the parent of i) - /// @param parent_index pointer for return data - void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; } - /// get user integer - /// @param index body index - /// @param user_int user integer - /// @return 0 on success, -1 on failure - int getUserInt(const int index, int *user_int) const; - /// get user pointer - /// @param index body index - /// @param user_int user pointer - /// @return 0 on success, -1 on failure - int getUserPtr(const int index, void **user_ptr) const; - -private: - // vector of bodies - idArray<InertiaData>::type m_inertias; - // vector of joints - idArray<JointData>::type m_joints; - // number of mechanical degrees of freedom - int m_num_dofs; - // parent index array - idArray<int>::type m_parent_index; - // user integers - idArray<int>::type m_user_int; - // user pointers - idArray<void *>::type m_user_ptr; - // index of root body (or -1 if not set) - int m_root_index; -}; -} // namespace btInverseDynamics -#endif // MULTIBODYTREEINITCACHE_HPP_ |