diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics')
15 files changed, 3704 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp new file mode 100644 index 0000000000..ebb10e7a16 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp @@ -0,0 +1,107 @@ +///@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 new file mode 100644 index 0000000000..130c19c6d6 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -0,0 +1,37 @@ +///@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 new file mode 100644 index 0000000000..cbd7e8a9c4 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp @@ -0,0 +1,31 @@ +///@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 new file mode 100644 index 0000000000..1dc22f860a --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp @@ -0,0 +1,29 @@ +///@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 error_message(...) b3Error(__VA_ARGS__) +#define 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 error_message(...) \ + do { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +/// print warning message with file/line information +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define 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 new file mode 100644 index 0000000000..99fe20e492 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp @@ -0,0 +1,437 @@ +#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()) { + 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()) { + 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()) { + 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)) { + error_message("invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + + return false; + } + + // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) + if (!has_fixed_joint) { + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(1, 1) + I(2, 2) < I(0, 0)) { + error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + } + // check positive/zero diagonal elements + for (int i = 0; i < 3; i++) { + if (I(i, i) < 0) { // accept zero + error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); + return false; + } + } + // check symmetry + if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " + "%e\n", + index, I(1, 0) - I(0, 1)); + return false; + } + if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " + "%e\n", + index, I(2, 0) - I(0, 2)); + return false; + } + if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) { + error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, + I(1, 2) - I(2, 1)); + return false; + } + return true; +} + +bool isValidTransformMatrix(const mat33 &m) { +#define print_mat(x) \ + error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) + + // 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) { + 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) { + 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) { + 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) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + // check determinant (rotation not reflection) + if (determinant(m) <= 0) { + error_message("Not a valid rotation matrix (determinant <=0)\n"); + print_mat(m); + return false; + } + return true; +} + +bool isUnitVector(const vec3 &vector) { + return 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; +} +} diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp new file mode 100644 index 0000000000..b355474d44 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp @@ -0,0 +1,99 @@ +/// @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); +} +#endif // IDMATH_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp new file mode 100644 index 0000000000..c67588d49f --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp @@ -0,0 +1,445 @@ +#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) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + error_message("error in inverse dynamics calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, + const bool initialize_matrix, + const bool set_lower_triangular_matrix, matxx *mass_matrix) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == + m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, + set_lower_triangular_matrix, mass_matrix)) { + error_message("error in mass matrix calculation\n"); + return -1; + } + return 0; +} + +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) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, dot_u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) { + 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) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, q, q, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + 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) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + 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) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in jacobian calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateJacobians(const vecx& q){ + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, q, + MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) { + 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) { + error_message("body index must be positive (got %d)\n", body_index); + return -1; + } + vec3 body_axis_of_motion(body_axis_of_motion_); + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + // check if axis is unit vector + if (!isUnitVector(body_axis_of_motion)) { + warning_message( + "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", + body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); + idScalar length = 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())) { + error_message("axis of motion vector too short (%e)\n", length); + return -1; + } + body_axis_of_motion = (1.0 / length) * body_axis_of_motion; + } + break; + case FIXED: + break; + case FLOATING: + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + // sanity check for mass properties. Zero mass is OK. + if (mass < 0) { + m_mass_parameters_are_valid = false; + error_message("Body %d has invalid mass %e\n", body_index, mass); + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + 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) { + 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; + + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized + // matrices. + switch (rigid_body.m_joint_type) { + case REVOLUTE: + rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case PRISMATIC: + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); + break; + case FIXED: + // NOTE/TODO: dimension really should be zero .. + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case FLOATING: + // NOTE/TODO: this is not really correct. + // the Jacobians should be 3x3 matrices here ! + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + default: + error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + return -1; + } + } + + // 4 assign degree of freedom indices & build per-joint-type index arrays + if (-1 == m_impl->generateIndexSets()) { + error_message("generating index sets\n"); + return -1; + } + + // 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); +} + +} diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp new file mode 100644 index 0000000000..d235aa6e76 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp @@ -0,0 +1,363 @@ +#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 +}; + +/// 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) +/// 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] +/// +/// 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) +/// +/// 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 new file mode 100644 index 0000000000..836395cea2 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -0,0 +1,36 @@ +#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; +} + +} +#endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp new file mode 100644 index 0000000000..5bb4a33bdd --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -0,0 +1,172 @@ +#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(size) {} + const vecx& operator=(const btVectorX<idScalar>& rhs) { + *static_cast<btVectorX*>(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()) { + 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()) { + 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) { + 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()) { + 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); +} + +} + +#endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp new file mode 100644 index 0000000000..4d3f6c87e9 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp @@ -0,0 +1,415 @@ +/// @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) { + 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()) { + 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()) { + 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()) { + 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()) { + 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 btInverseDynamcis +#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp new file mode 100644 index 0000000000..b35c55df61 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -0,0 +1,1028 @@ +#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"; + } + 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; + } + 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; + default: + error_message("unsupported joint type %d\n", body.m_joint_type); + return -1; + } + } + // sanity check + if (q_index != m_num_dofs) { + error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); + return -1; + } + + m_child_indices.resize(m_body_list.size()); + + 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 + error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } else { + // should never happen + error_message( + "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", + child, parent, static_cast<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; + } + + // 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) { + 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)) { + 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); + } + + 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 ) { + 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) { + 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; + } + } + + // 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; + } +} + +int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs) { + 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) { + error_message("invalid type %d\n", type); + return -1; + } + + addRelativeJacobianComponent(m_body_list[0]); + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + + mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R); + body.m_body_Jac_T = parent.m_body_Jac_T; + mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x); + sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T); + + addRelativeJacobianComponent(body); + mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T); + + if(type >= POSITION_VELOCITY) { + body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel); + body.m_body_dot_Jac_T_u = body.m_body_T_parent * + (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel)); + } + } + return 0; +} +#endif + +static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { + switch (dof) { + // rotational part + case 0: + Jac_JR(0) = 1; + Jac_JR(1) = 0; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 1: + Jac_JR(0) = 0; + Jac_JR(1) = 1; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 2: + Jac_JR(0) = 0; + Jac_JR(1) = 0; + Jac_JR(2) = 1; + setZero(Jac_JT); + break; + // translational part + case 3: + setZero(Jac_JR); + Jac_JT(0) = 1; + Jac_JT(1) = 0; + Jac_JT(2) = 0; + break; + case 4: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 1; + Jac_JT(2) = 0; + break; + case 5: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 0; + Jac_JT(2) = 1; + break; + } +} + +static inline int jointNumDoFs(const JointType &type) { + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + // this should never happen + error_message("invalid joint type\n"); + // TODO add configurable abort/crash function + abort(); + 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) { + 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 (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); + } + + 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 (FLOATING != body.m_joint_type) { + error_message("??\n"); + return -1; + } + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + 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--) { + // 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) { \ + 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 +} diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp new file mode 100644 index 0000000000..3efe9d0492 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -0,0 +1,283 @@ +// 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; + // 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 +}; +} +#endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp new file mode 100644 index 0000000000..47b4ab3890 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -0,0 +1,113 @@ +#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 FLOATING: + m_num_dofs += 6; + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + if(-1 == parent_index) { + if(m_root_index>=0) { + error_message("trying to add body %d as root, but already added %d as root body\n", + body_index, m_root_index); + return -1; + } + m_root_index=body_index; + } + + 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())) { + 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())) { + 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())) { + 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())) { + 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; +} +} diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp new file mode 100644 index 0000000000..0d2aa4a071 --- /dev/null +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -0,0 +1,109 @@ +#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; +}; +} +#endif // MULTIBODYTREEINITCACHE_HPP_ |