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

add real-time safe Inverse Dynamics library+test+utils

Browse Source 
an example for the example browser will follow.
thanks to Thomas Buschmann
This commit is contained in:
Erwin Coumans
2015-11-17 08:27:38 -08:00
parent 7d9365319c
commit aa4d119f98
41 changed files with 5233 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
src/BulletInverseDynamics/IDConfig.hpp Normal file
View File

@@ -0,0 +1,40 @@
///@file Configuration for Inverse Dynamics Library,
/// such as choice of linear algebra library and underlying scalar type
#ifndef IDCONFIG_HPP_
#define IDCONFIG_HPP_
// If we have a custom configuration, compile without using other parts of bullet.
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
#define BT_ID_WO_BULLET
#endif
// error messages
#include "IDErrorMessages.hpp"
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
#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)
#else
// Use default configuration with bullet's types
// Use the same scalar type as rest of bullet library
#include "LinearMath/btScalar.h"
typedef btScalar idScalar;
// 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
// use bullet's allocator functions
#define idMalloc btAllocFunc
#define idFree btFreeFunc
#define ID_LINEAR_MATH_USE_BULLET
#include "details/IDLinearMathInterface.hpp"
#endif
#endif

36
src/BulletInverseDynamics/IDConfigBuiltin.hpp Normal file
View File

@@ -0,0 +1,36 @@
///@file Configuration for Inverse Dynamics Library without external dependencies
#ifndef INVDYNCONFIG_BUILTIN_HPP_
#define INVDYNCONFIG_BUILTIN_HPP_
#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

43
src/BulletInverseDynamics/IDConfigEigen.hpp Normal file
View File

@@ -0,0 +1,43 @@
///@file Configuration for Inverse Dynamics Library with Eigen
#ifndef INVDYNCONFIG_EIGEN_HPP_
#define INVDYNCONFIG_EIGEN_HPP_
#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*) {}
// 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

34
src/BulletInverseDynamics/IDErrorMessages.hpp Normal file
View File

@@ -0,0 +1,34 @@
///@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__)
#ifndef BT_ID_WO_BULLET
#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 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

371
src/BulletInverseDynamics/IDMath.cpp Normal file
View File

@@ -0,0 +1,371 @@
#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;
}
idScalar maxAbs(const vecx &v) {
idScalar result = 0.0;
for (int i = 0; i < v.size(); i++) {
const idScalar tmp = std::fabs(v(i));
if (tmp > result) {
result = tmp;
}
}
return result;
}
idScalar maxAbs(const vec3 &v) {
idScalar result = 0.0;
for (int i = 0; i < 3; i++) {
const idScalar tmp = std::fabs(v(i));
if (tmp > result) {
result = tmp;
}
}
return result;
}
mat33 transformX(const idScalar &alpha) {
mat33 T;
const idScalar cos_alpha = std::cos(alpha);
const idScalar sin_alpha = std::sin(alpha);
// [1 0 0]
// [0 c s]
// [0 -s c]
T(0, 0) = 1.0;
T(0, 1) = 0.0;
T(0, 2) = 0.0;
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 = std::cos(beta);
const idScalar sin_beta = std::sin(beta);
// [c 0 -s]
// [0 1 0]
// [s 0 c]
T(0, 0) = cos_beta;
T(0, 1) = 0.0;
T(0, 2) = -sin_beta;
T(1, 0) = 0.0;
T(1, 1) = 1.0;
T(1, 2) = 0.0;
T(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 = std::cos(gamma);
const idScalar sin_gamma = std::sin(gamma);
// [ c s 0]
// [-s c 0]
// [ 0 0 1]
T(0, 0) = cos_gamma;
T(0, 1) = sin_gamma;
T(0, 2) = 0.0;
T(1, 0) = -sin_gamma;
T(1, 1) = cos_gamma;
T(1, 2) = 0.0;
T(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 = std::sin(alpha);
const idScalar ca = std::cos(alpha);
const idScalar st = std::sin(theta);
const idScalar ct = std::cos(theta);
(*r)(0) = a;
(*r)(1) = -sa * d;
(*r)(2) = ca * d;
(*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 = cos(angle);
const idScalar s = -sin(angle);
const idScalar one_m_c = 1.0 - c;
const idScalar &x = axis(0);
const idScalar &y = axis(1);
const idScalar &z = axis(2);
(*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 (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) {
error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
"%e\n",
index, I(1, 0) - I(0, 1));
return false;
}
if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) {
error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
"%e\n",
index, I(2, 0) - I(0, 2));
return false;
}
if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) {
error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
I(1, 2) - I(2, 1));
return false;
}
return true;
}
bool isValidTransformMatrix(const mat33 &m) {
#define print_mat(x) \
error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
// check for unit length column vectors
for (int i = 0; i < 3; i++) {
const idScalar length_minus_1 =
std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
if (length_minus_1 > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (column %d not unit length)\n"
"column = [%.18e %.18e %.18e]\n"
"length-1.0= %.18e\n",
i, m(0, i), m(1, i), m(2, i), length_minus_1);
print_mat(m);
return false;
}
}
// check for orthogonal column vectors
if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
print_mat(m);
return false;
}
if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
// check determinant (rotation not reflection)
if (determinant(m) <= 0) {
error_message("Not a valid rotation matrix (determinant <=0)\n");
print_mat(m);
return false;
}
return true;
}
bool isUnitVector(const vec3 &vector) {
return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
kIsZero;
}
vec3 rpyFromMatrix(const mat33 &rot) {
vec3 rpy;
rpy(2) = std::atan2(-rot(1, 0), rot(0, 0));
rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0));
rpy(0) = std::atan2(-rot(2, 0), rot(2, 2));
return rpy;
}
}

88
src/BulletInverseDynamics/IDMath.hpp Normal file
View File

@@ -0,0 +1,88 @@
/// @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);
/// 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
/// 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_

331
src/BulletInverseDynamics/MultiBodyTree.cpp Normal file
View File

@@ -0,0 +1,331 @@
#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);
}
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::addBody(int body_index, int parent_index, JointType joint_type,
const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
const vec3 &body_axis_of_motion_, idScalar mass,
const vec3 &body_r_body_com, const mat33 &body_I_body,
const int user_int, void *user_ptr) {
if (body_index < 0) {
error_message("body index must be positive (got %d)\n", body_index);
return -1;
}
vec3 body_axis_of_motion(body_axis_of_motion_);
switch (joint_type) {
case REVOLUTE:
case PRISMATIC:
// check if axis is unit vector
if (!isUnitVector(body_axis_of_motion)) {
warning_message(
"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) +
std::pow(body_axis_of_motion(1), 2) +
std::pow(body_axis_of_motion(2), 2));
if (length < std::sqrt(std::numeric_limits<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();
// 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, vec3 first_mass_moment) {
return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
}
int MultiBodyTree::setBodySecondMassMoment(const int body_index, 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);
}
}

308
src/BulletInverseDynamics/MultiBodyTree.hpp Normal file
View File

@@ -0,0 +1,308 @@
#ifndef MULTIBODYTREE_HPP_
#define MULTIBODYTREE_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:
/// 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
/// @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);
/// set gravitational acceleration
/// the default is [0;0;-9.8] in the world frame
/// @param gravity the gravitational acceleration in world frame
/// @return 0 on success, -1 on error
int setGravityInWorldFrame(const vec3& gravity);
/// returns number of bodies in tree
int numBodies() const;
/// returns number of mechanical degrees of freedom (dimension of q-vector)
int numDoFs() const;
/// get origin of a body-fixed frame, represented in world frame
/// @param body_index index for frame/body
/// @param world_origin pointer for return data
/// @return 0 on success, -1 on error
int getBodyOrigin(const int body_index, vec3* world_origin) const;
/// get center of mass of a body, represented in world frame
/// @param body_index index for frame/body
/// @param world_com pointer for return data
/// @return 0 on success, -1 on error
int getBodyCoM(const int body_index, vec3* world_com) const;
/// get transform from of a body-fixed frame to the world frame
/// @param body_index index for frame/body
/// @param world_T_body pointer for return data
/// @return 0 on success, -1 on error
int getBodyTransform(const int body_index, mat33* world_T_body) const;
/// get absolute angular velocity for a body, represented in the world frame
/// @param body_index index for frame/body
/// @param world_omega pointer for return data
/// @return 0 on success, -1 on error
int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
/// get linear velocity of a body, represented in world frame
/// @param body_index index for frame/body
/// @param world_velocity pointer for return data
/// @return 0 on success, -1 on error
int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
/// get linear velocity of a body's CoM, represented in world frame
/// (not required for inverse dynamics, provided for convenience)
/// @param body_index index for frame/body
/// @param world_vel_com pointer for return data
/// @return 0 on success, -1 on error
int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
/// get origin of a body-fixed frame, represented in world frame
/// @param body_index index for frame/body
/// @param world_origin pointer for return data
/// @return 0 on success, -1 on error
int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
/// get origin of a body-fixed frame, represented in world frame
/// NOTE: this will include the gravitational acceleration, so the actual acceleration is
/// obtainened by setting gravitational acceleration to zero, or subtracting it.
/// @param body_index index for frame/body
/// @param world_origin pointer for return data
/// @return 0 on success, -1 on error
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
/// returns the (internal) index of body
/// @param body_index is the index of a body (internal: TODO: fix/clarify
/// indexing!)
/// @param parent_index pointer to where parent index will be stored
/// @return 0 on success, -1 on error
int getParentIndex(const int body_index, int* parent_index) const;
/// get joint type
/// @param body_index index of the body
/// @param joint_type the corresponding joint type
/// @return 0 on success, -1 on failure
int getJointType(const int body_index, JointType* joint_type) const;
/// get joint type as string
/// @param body_index index of the body
/// @param joint_type string naming the corresponding joint type
/// @return 0 on success, -1 on failure
int getJointTypeStr(const int body_index, const char** joint_type) const;
/// get offset for degrees of freedom of this body into the q-vector
/// @param body_index index of the body
/// @param q_offset offset the q vector
/// @return -1 on error, 0 on success
int getDoFOffset(const int body_index, int* q_offset) const;
/// get user integer. not used by the library.
/// @param body_index index of the body
/// @param user_int the user integer
/// @return 0 on success, -1 on error
int getUserInt(const int body_index, int* user_int) const;
/// get user pointer. not used by the library.
/// @param body_index index of the body
/// @param user_ptr the user pointer
/// @return 0 on success, -1 on error
int getUserPtr(const int body_index, void** user_ptr) const;
/// set user integer. not used by the library.
/// @param body_index index of the body
/// @param user_int the user integer
/// @return 0 on success, -1 on error
int setUserInt(const int body_index, const int user_int);
/// set user pointer. not used by the library.
/// @param body_index index of the body
/// @param user_ptr the user pointer
/// @return 0 on success, -1 on error
int setUserPtr(const int body_index, void* const user_ptr);
/// set mass for a body
/// @param body_index index of the body
/// @param mass the mass to set
/// @return 0 on success, -1 on failure
int setBodyMass(const int body_index, const idScalar mass);
/// set first moment of mass for a body
/// (mass * center of mass, in body fixed frame, relative to joint)
/// @param body_index index of the body
/// @param first_mass_moment the vector to set
/// @return 0 on success, -1 on failure
int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment);
/// set second moment of mass for a body
/// (moment of inertia, in body fixed frame, relative to joint)
/// @param body_index index of the body
/// @param second_mass_moment the inertia matrix
/// @return 0 on success, -1 on failure
int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment);
/// get mass for a body
/// @param body_index index of the body
/// @param mass the mass
/// @return 0 on success, -1 on failure
int getBodyMass(const int body_index, idScalar* mass) const;
/// get first moment of mass for a body
/// (mass * center of mass, in body fixed frame, relative to joint)
/// @param body_index index of the body
/// @param first_moment the vector
/// @return 0 on success, -1 on failure
int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
/// get second moment of mass for a body
/// (moment of inertia, in body fixed frame, relative to joint)
/// @param body_index index of the body
/// @param second_mass_moment the inertia matrix
/// @return 0 on success, -1 on failure
int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
/// set all user forces and moments to zero
void clearAllUserForcesAndMoments();
/// Add an external force to a body, acting at the origin of the body-fixed frame.
/// Calls to addUserForce are cumulative. Set the user force and moment to zero
/// via clearAllUserForcesAndMoments()
/// @param body_force the force represented in the body-fixed frame of reference
/// @return 0 on success, -1 on error
int addUserForce(const int body_index, const vec3& body_force);
/// Add an external moment to a body.
/// Calls to addUserMoment are cumulative. Set the user force and moment to zero
/// via clearAllUserForcesAndMoments()
/// @param body_moment the moment represented in the body-fixed frame of reference
/// @return 0 on success, -1 on error
int addUserMoment(const int body_index, const vec3& body_moment);
private:
// flag indicating if system has been initialized
bool m_is_finalized;
// flag indicating if mass properties are physically valid
bool m_mass_parameters_are_valid;
// flag defining if unphysical mass parameters are accepted
bool m_accept_invalid_mass_parameters;
// This struct implements the inverse dynamics calculations
class MultiBodyImpl;
MultiBodyImpl* m_impl;
// cache data structure for initialization
class InitCache;
InitCache* m_init_cache;
};
} // namespace btInverseDynamics
#endif // MULTIBODYTREE_HPP_

17
src/BulletInverseDynamics/details/IDEigenInterface.hpp Normal file
View File

@@ -0,0 +1,17 @@
#ifndef INVDYNEIGENINTERFACE_HPP_
#define INVDYNEIGENINTERFACE_HPP_
#include "../IDConfig.hpp"
namespace btInverseDynamics {
#ifdef BT_USE_DOUBLE_PRECISION
typedef Eigen::VectorXd vecx;
typedef Eigen::Vector3d vec3;
typedef Eigen::Matrix3d mat33;
typedef Eigen::MatrixXd matxx;
#else
typedef Eigen::VectorXf vecx;
typedef Eigen::Vector3f vec3;
typedef Eigen::Matrix3f mat33;
typedef Eigen::MatrixXf matxx;
#endif //
}
#endif // INVDYNEIGENINTERFACE_HPP_

112
src/BulletInverseDynamics/details/IDLinearMathInterface.hpp Normal file
View File

@@ -0,0 +1,112 @@
#ifndef IDLINEARMATHINTERFACE_HPP_
#define IDLINEARMATHINTERFACE_HPP_
#include <cstdlib>
#include "../IDConfig.hpp"
#include "../../LinearMath/btMatrix3x3.h"
#include "../../LinearMath/btVector3.h"
#include "../../LinearMath/btMatrixX.h"
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]; }
const 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;
}
}
#endif // IDLINEARMATHINTERFACE_HPP_

326
src/BulletInverseDynamics/details/IDMatVec.hpp Normal file
View File

@@ -0,0 +1,326 @@
/// @file Built-In Matrix-Vector functions
#ifndef IDMATVEC_HPP_
#define IDMATVEC_HPP_
#include <cstdlib>
namespace btInverseDynamics {
class vec3;
class vecx;
class mat33;
class matxx;
/// 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(int rows, int cols) : m_rows(rows), m_cols(cols) {
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
}
~matxx() { idFree(m_data); }
const matxx& operator=(const matxx& rhs);
idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; }
const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; }
const int& rows() const { return m_rows; }
const int& cols() const { return m_cols; }
private:
int m_rows;
int m_cols;
idScalar* m_data;
};
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;
}
}
#endif

844
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp Normal file
View File

@@ -0,0 +1,844 @@
#include "MultiBodyTreeImpl.hpp"
namespace btInverseDynamics {
MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
m_body_list.resize(num_bodies_);
m_parent_index.resize(num_bodies_);
m_child_indices.resize(num_bodies_);
m_user_int.resize(num_bodies_);
m_user_ptr.resize(num_bodies_);
m_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;
}
}
}
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. update relative kinematics
// 1.1 for revolute
for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
mat33 T;
bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
body.m_body_T_parent = T * body.m_body_T_parent_ref;
// body.m_parent_r_parent_body= fixed
body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
// body.m_parent_dot_r_rel = fixed;
// NOTE: this assumes that Jac_JR is constant, which is true for revolute
// joints, but not in the general case (eg, slider-crank type mechanisms)
body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
// body.m_parent_ddot_r_rel = fixed;
}
// 1.2 for prismatic
for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
// body.m_body_T_parent= fixed
body.m_parent_pos_parent_body =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
// body.m_parent_omega_rel = 0;
body.m_parent_vel_rel =
body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
// body.parent_dot_omega_rel = 0;
// NOTE: this assumes that Jac_JT is constant, which is true for
// prismatic joints, but not in the general case
body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
}
// 3. absolute kinematic quantities & dynamic quantities
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
// calculations for root body
{
RigidBody &body = m_body_list[0];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts)
// not required right now, added here for debugging purposes
body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_body_T_world = body.m_body_T_parent;
// 3.2 update absolute velocities
body.m_body_ang_vel = body.m_body_ang_vel_rel;
body.m_body_vel = body.m_parent_vel_rel;
// 3.3 update absolute accelerations
// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
body.m_body_ang_acc = body.m_body_ang_acc_rel;
body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
// add gravitational acceleration to root body
// this is an efficient way to add gravitational terms,
// but it does mean that the kinematics are no longer
// correct at the acceleration level
// NOTE: To get correct acceleration kinematics, just set world_gravity to zero
body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
}
for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts) not required right now added here for debugging purposes
body.m_body_pos =
body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
// 3.2 update absolute velocities
body.m_body_ang_vel =
body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
body.m_body_vel =
body.m_body_T_parent *
(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
body.m_parent_vel_rel);
// 3.3 update absolute accelerations
// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
body.m_body_ang_acc =
body.m_body_T_parent * parent.m_body_ang_acc -
body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
body.m_body_ang_acc_rel;
body.m_body_acc =
body.m_body_T_parent *
(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
parent.m_body_ang_vel.cross(
parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
}
for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
// 3.4 update dynamic terms (rate of change of angular & linear momentum)
body.m_eom_lhs_rotational =
body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
body.m_body_moment_user;
body.m_eom_lhs_translational =
body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
body.m_body_force_user;
}
// 4. calculate full set of forces at parent joint
// (not directly calculating the joint force along the free direction
// simplifies inclusion of fixed joints.
// An alternative would be to fuse bodies in a pre-processing step,
// but that would make changing masses online harder (eg, payload masses
// added with fixed joints to a gripper)
// Also, this enables adding zero weight bodies as a way to calculate frame poses
// for force elements, etc.
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;
}
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();
}
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.
// Macro for setting matrix elements depending on underlying math library.
#ifdef ID_LINEAR_MATH_USE_BULLET
#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val)
#else
#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val
#endif
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != m_num_dofs) {
error_message("Dimension error. System has %d DOFs,\n"
"but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
m_num_dofs, static_cast<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++) {
setMassMatrixElem(i, j, 0.0);
}
}
}
if (update_kinematics) {
// 1. update relative kinematics
// 1.1 for revolute joints
for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
// from reference orientation (q=0) of body-fixed frame to current orientation
mat33 body_T_body_ref;
bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref);
body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
}
// 1.2 for prismatic joints
for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
// body.m_body_T_parent= fixed
body.m_parent_pos_parent_body =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
RigidBody &body = m_body_list[m_body_floating_list[i]];
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);
setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
// rest of the mass matrix column upwards
{
// 1. for multi-dof joints, rest of the dofs of this body
for (int row = col - 1; row >= q_index_min; row--) {
if (FLOATING != body.m_joint_type) {
error_message("??\n");
return -1;
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMassMatrixElem(col, row, Mrc);
}
// 2. ancestor dofs
int child_idx = i;
int parent_idx = m_parent_index[i];
while (parent_idx >= 0) {
const RigidBody &child_body = m_body_list[child_idx];
const RigidBody &parent_body = m_body_list[parent_idx];
const mat33 parent_T_child = child_body.m_body_T_parent.transpose();
body_eom_rot = parent_T_child * body_eom_rot;
body_eom_trans = parent_T_child * body_eom_trans;
body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans);
const int parent_body_q_index_min = parent_body.m_q_index;
const int parent_body_q_index_max =
parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
vec3 Jac_JR = parent_body.m_Jac_JR;
vec3 Jac_JT = parent_body.m_Jac_JT;
for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) {
// set jacobians for 6-DoF joints
if (FLOATING == parent_body.m_joint_type) {
setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
}
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMassMatrixElem(col, row, Mrc);
}
child_idx = parent_idx;
parent_idx = m_parent_index[child_idx];
}
}
}
}
if (set_lower_triangular_matrix) {
for (int col = 0; col < m_num_dofs; col++) {
for (int row = 0; row < col; row++) {
setMassMatrixElem(row, col, (*mass_matrix)(col, row));
}
}
}
#undef setMassMatrixElem
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::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;
}
}

236
src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp Normal file
View File

@@ -0,0 +1,236 @@
// 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;
};
/// The MBS implements a tree structured multibody system
class MultiBodyTree::MultiBodyImpl {
friend class MultiBodyTree;
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
/// constructor
/// @param num_bodies the number of bodies in the system
/// @param num_dofs number of degrees of freedom in the system
MultiBodyImpl(int num_bodies_, int num_dofs_);
/// \copydoc MultiBodyTree::calculateInverseDynamics
int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
vecx* joint_forces);
///\copydoc MultiBodyTree::calculateMassMatrix
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
const bool initialize_matrix, const bool set_lower_triangular_matrix,
matxx* mass_matrix);
/// generate additional index sets from the parent_index array
/// @return -1 on error, 0 on success
int generateIndexSets();
/// set gravity acceleration in world frame
/// @param gravity gravity vector in the world frame
/// @return 0 on success, -1 on error
int setGravityInWorldFrame(const vec3& gravity);
/// pretty print tree
void printTree();
/// print tree data
void printTreeData();
/// initialize fixed data
void calculateStaticData();
/// \copydoc MultiBodyTree::getBodyFrame
int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
/// \copydoc MultiBodyTree::getParentIndex
int getParentIndex(const int body_index, int* m_parent_index);
/// \copydoc MultiBodyTree::getJointType
int getJointType(const int body_index, JointType* joint_type) const;
/// \copydoc MultiBodyTree::getJointTypeStr
int getJointTypeStr(const int body_index, const char** joint_type) const;
/// \copydoc MultiBodyTree:getDoFOffset
int getDoFOffset(const int body_index, int* q_index) const;
/// \copydoc MultiBodyTree::getBodyOrigin
int getBodyOrigin(const int body_index, vec3* world_origin) const;
/// \copydoc MultiBodyTree::getBodyCoM
int getBodyCoM(const int body_index, vec3* world_com) const;
/// \copydoc MultiBodyTree::getBodyTransform
int getBodyTransform(const int body_index, mat33* world_T_body) const;
/// \copydoc MultiBodyTree::getBodyAngularVelocity
int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
/// \copydoc MultiBodyTree::getBodyLinearVelocity
int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
/// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
/// \copydoc MultiBodyTree::getBodyAngularAcceleration
int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
/// \copydoc MultiBodyTree::getBodyLinearAcceleration
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
/// \copydoc MultiBodyTree::getUserInt
int getUserInt(const int body_index, int* user_int) const;
/// \copydoc MultiBodyTree::getUserPtr
int getUserPtr(const int body_index, void** user_ptr) const;
/// \copydoc MultiBodyTree::setUserInt
int setUserInt(const int body_index, const int user_int);
/// \copydoc MultiBodyTree::setUserPtr
int setUserPtr(const int body_index, void* const user_ptr);
///\copydoc MultiBodytTree::setBodyMass
int setBodyMass(const int body_index, const idScalar mass);
///\copydoc MultiBodytTree::setBodyFirstMassMoment
int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment);
///\copydoc MultiBodytTree::setBodySecondMassMoment
int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment);
///\copydoc MultiBodytTree::getBodyMass
int getBodyMass(const int body_index, idScalar* mass) const;
///\copydoc MultiBodytTree::getBodyFirstMassMoment
int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
///\copydoc MultiBodytTree::getBodySecondMassMoment
int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
/// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
void clearAllUserForcesAndMoments();
/// \copydoc MultiBodyTree::addUserForce
int addUserForce(const int body_index, const vec3& body_force);
/// \copydoc MultiBodyTree::addUserMoment
int addUserMoment(const int body_index, const vec3& body_moment);
private:
// debug function. print tree structure to stdout
void printTree(int index, int indentation);
// get string representation of JointType (for debugging)
const char* jointTypeToString(const JointType& type) const;
// get number of degrees of freedom from joint type
int bodyNumDoFs(const JointType& type) const;
// number of bodies in the system
int m_num_bodies;
// number of degrees of freedom
int m_num_dofs;
// Gravitational acceleration (in world frame)
vec3 m_world_gravity;
// vector of bodies in the system
// body 0 is used as an environment body and is allways fixed.
// The bodies are ordered such that a parent body always has an index
// smaller than its child.
idArray<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;
};
}
#endif

113
src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp Normal file
View File

@@ -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;
}
}

109
src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp Normal file
View File

@@ -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_

12
src/BulletInverseDynamics/premake4.lua Normal file
View File

@@ -0,0 +1,12 @@
project "BulletInverseDynamics"
kind "StaticLib"
includedirs {
"..",
}
files {
"IDMath.cpp",
"MultiBodyTree.cpp",
"details/MultiBodyTreeInitCache.cpp",
"details/MultiBodyTreeImpl.cpp",
}