mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
Merge pull request #529 from erwincoumans/master
[InverseDynamics] 4-spaces->tabs for src/BulletInverseDynamics, Add custom namespaces per configuration
This commit is contained in:
commit
4605282b11
@ -1,5 +1,5 @@
|
|||||||
///@file Configuration for Inverse Dynamics Library,
|
///@file Configuration for Inverse Dynamics Library,
|
||||||
/// such as choice of linear algebra library and underlying scalar type
|
/// such as choice of linear algebra library and underlying scalar type
|
||||||
#ifndef IDCONFIG_HPP_
|
#ifndef IDCONFIG_HPP_
|
||||||
#define IDCONFIG_HPP_
|
#define IDCONFIG_HPP_
|
||||||
// If we have a custom configuration, compile without using other parts of bullet.
|
// If we have a custom configuration, compile without using other parts of bullet.
|
||||||
@ -8,6 +8,7 @@
|
|||||||
#define BT_ID_POW(a,b) std::pow(a,b)
|
#define BT_ID_POW(a,b) std::pow(a,b)
|
||||||
#define BT_ID_SNPRINTF snprintf
|
#define BT_ID_SNPRINTF snprintf
|
||||||
#define BT_ID_PI M_PI
|
#define BT_ID_PI M_PI
|
||||||
|
#define BT_ID_USE_DOUBLE_PRECISION
|
||||||
#else
|
#else
|
||||||
#define BT_ID_POW(a,b) btPow(a,b)
|
#define BT_ID_POW(a,b) btPow(a,b)
|
||||||
#define BT_ID_PI SIMD_PI
|
#define BT_ID_PI SIMD_PI
|
||||||
@ -24,11 +25,18 @@
|
|||||||
#define INVDYN_INCLUDE_HELPER_2(x) #x
|
#define INVDYN_INCLUDE_HELPER_2(x) #x
|
||||||
#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
|
#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
|
||||||
#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
|
#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
|
||||||
|
#ifndef btInverseDynamics
|
||||||
|
#error "custom inverse dynamics config, but no custom namespace defined"
|
||||||
|
#endif
|
||||||
#else
|
#else
|
||||||
|
#define btInverseDynamics btInverseDynamicsBullet3
|
||||||
// Use default configuration with bullet's types
|
// Use default configuration with bullet's types
|
||||||
// Use the same scalar type as rest of bullet library
|
// Use the same scalar type as rest of bullet library
|
||||||
#include "LinearMath/btScalar.h"
|
#include "LinearMath/btScalar.h"
|
||||||
typedef btScalar idScalar;
|
typedef btScalar idScalar;
|
||||||
|
#ifdef BT_USE_DOUBLE_PRECISION
|
||||||
|
#define BT_ID_USE_DOUBLE_PRECISION
|
||||||
|
#endif
|
||||||
// use bullet types for arrays and array indices
|
// use bullet types for arrays and array indices
|
||||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||||
// this is to make it work with C++2003, otherwise we could do this:
|
// this is to make it work with C++2003, otherwise we could do this:
|
||||||
@ -36,7 +44,7 @@ typedef btScalar idScalar;
|
|||||||
// using idArray = b3AlignedObjectArray<T>;
|
// using idArray = b3AlignedObjectArray<T>;
|
||||||
template <typename T>
|
template <typename T>
|
||||||
struct idArray {
|
struct idArray {
|
||||||
typedef b3AlignedObjectArray<T> type;
|
typedef b3AlignedObjectArray<T> type;
|
||||||
};
|
};
|
||||||
typedef int idArrayIdx;
|
typedef int idArrayIdx;
|
||||||
#define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR
|
#define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR
|
||||||
|
@ -1,6 +1,7 @@
|
|||||||
///@file Configuration for Inverse Dynamics Library without external dependencies
|
///@file Configuration for Inverse Dynamics Library without external dependencies
|
||||||
#ifndef INVDYNCONFIG_BUILTIN_HPP_
|
#ifndef INVDYNCONFIG_BUILTIN_HPP_
|
||||||
#define INVDYNCONFIG_BUILTIN_HPP_
|
#define INVDYNCONFIG_BUILTIN_HPP_
|
||||||
|
#define btInverseDynamics btInverseDynamicsBuiltin
|
||||||
#ifdef BT_USE_DOUBLE_PRECISION
|
#ifdef BT_USE_DOUBLE_PRECISION
|
||||||
// choose double/single precision version
|
// choose double/single precision version
|
||||||
typedef double idScalar;
|
typedef double idScalar;
|
||||||
@ -14,7 +15,7 @@ typedef float idScalar;
|
|||||||
// using idArray = std::vector<T>;
|
// using idArray = std::vector<T>;
|
||||||
template <typename T>
|
template <typename T>
|
||||||
struct idArray {
|
struct idArray {
|
||||||
typedef std::vector<T> type;
|
typedef std::vector<T> type;
|
||||||
};
|
};
|
||||||
typedef std::vector<int>::size_type idArrayIdx;
|
typedef std::vector<int>::size_type idArrayIdx;
|
||||||
// default to standard malloc/free
|
// default to standard malloc/free
|
||||||
@ -22,15 +23,15 @@ typedef std::vector<int>::size_type idArrayIdx;
|
|||||||
#define idMalloc ::malloc
|
#define idMalloc ::malloc
|
||||||
#define idFree ::free
|
#define idFree ::free
|
||||||
// currently not aligned at all...
|
// currently not aligned at all...
|
||||||
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
|
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
|
||||||
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
||||||
inline void operator delete(void* ptr) { idFree(ptr); } \
|
inline void operator delete(void* ptr) { idFree(ptr); } \
|
||||||
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
|
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
|
||||||
inline void operator delete(void*, void*) {} \
|
inline void operator delete(void*, void*) {} \
|
||||||
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
||||||
inline void operator delete[](void* ptr) { idFree(ptr); } \
|
inline void operator delete[](void* ptr) { idFree(ptr); } \
|
||||||
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
|
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
|
||||||
inline void operator delete[](void*, void*) {}
|
inline void operator delete[](void*, void*) {}
|
||||||
|
|
||||||
#include "details/IDMatVec.hpp"
|
#include "details/IDMatVec.hpp"
|
||||||
#endif
|
#endif
|
||||||
|
@ -1,6 +1,7 @@
|
|||||||
///@file Configuration for Inverse Dynamics Library with Eigen
|
///@file Configuration for Inverse Dynamics Library with Eigen
|
||||||
#ifndef INVDYNCONFIG_EIGEN_HPP_
|
#ifndef INVDYNCONFIG_EIGEN_HPP_
|
||||||
#define INVDYNCONFIG_EIGEN_HPP_
|
#define INVDYNCONFIG_EIGEN_HPP_
|
||||||
|
#define btInverseDynamics btInverseDynamicsEigen
|
||||||
#ifdef BT_USE_DOUBLE_PRECISION
|
#ifdef BT_USE_DOUBLE_PRECISION
|
||||||
// choose double/single precision version
|
// choose double/single precision version
|
||||||
typedef double idScalar;
|
typedef double idScalar;
|
||||||
@ -15,7 +16,7 @@ typedef float idScalar;
|
|||||||
// using idArray = std::vector<T>;
|
// using idArray = std::vector<T>;
|
||||||
template <typename T>
|
template <typename T>
|
||||||
struct idArray {
|
struct idArray {
|
||||||
typedef std::vector<T> type;
|
typedef std::vector<T> type;
|
||||||
};
|
};
|
||||||
typedef std::vector<int>::size_type idArrayIdx;
|
typedef std::vector<int>::size_type idArrayIdx;
|
||||||
// default to standard malloc/free
|
// default to standard malloc/free
|
||||||
@ -24,15 +25,15 @@ typedef std::vector<int>::size_type idArrayIdx;
|
|||||||
|
|
||||||
#define idFree ::free
|
#define idFree ::free
|
||||||
// currently not aligned at all...
|
// currently not aligned at all...
|
||||||
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
|
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
|
||||||
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
||||||
inline void operator delete(void* ptr) { idFree(ptr); } \
|
inline void operator delete(void* ptr) { idFree(ptr); } \
|
||||||
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
|
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
|
||||||
inline void operator delete(void*, void*) {} \
|
inline void operator delete(void*, void*) {} \
|
||||||
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
|
||||||
inline void operator delete[](void* ptr) { idFree(ptr); } \
|
inline void operator delete[](void* ptr) { idFree(ptr); } \
|
||||||
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
|
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
|
||||||
inline void operator delete[](void*, void*) {}
|
inline void operator delete[](void*, void*) {}
|
||||||
|
|
||||||
// Note on interfaces:
|
// Note on interfaces:
|
||||||
// Eigen::Matrix has data(), to get c-array storage
|
// Eigen::Matrix has data(), to get c-array storage
|
||||||
|
@ -13,22 +13,22 @@
|
|||||||
#else // BT_ID_WO_BULLET
|
#else // BT_ID_WO_BULLET
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
/// print error message with file/line information
|
/// print error message with file/line information
|
||||||
#define error_message(...) \
|
#define error_message(...) \
|
||||||
do { \
|
do { \
|
||||||
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
|
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
|
||||||
fprintf(stderr, __VA_ARGS__); \
|
fprintf(stderr, __VA_ARGS__); \
|
||||||
} while (0)
|
} while (0)
|
||||||
/// print warning message with file/line information
|
/// print warning message with file/line information
|
||||||
#define warning_message(...) \
|
#define warning_message(...) \
|
||||||
do { \
|
do { \
|
||||||
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
|
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
|
||||||
fprintf(stderr, __VA_ARGS__); \
|
fprintf(stderr, __VA_ARGS__); \
|
||||||
} while (0)
|
} while (0)
|
||||||
#define warning_message(...) \
|
#define warning_message(...) \
|
||||||
do { \
|
do { \
|
||||||
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
|
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
|
||||||
fprintf(stderr, __VA_ARGS__); \
|
fprintf(stderr, __VA_ARGS__); \
|
||||||
} while (0)
|
} while (0)
|
||||||
#define id_printf(...) printf(__VA_ARGS__)
|
#define id_printf(...) printf(__VA_ARGS__)
|
||||||
#endif // BT_ID_WO_BULLET
|
#endif // BT_ID_WO_BULLET
|
||||||
#endif
|
#endif
|
||||||
|
@ -10,362 +10,362 @@ static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
|
|||||||
static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
|
static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
|
||||||
|
|
||||||
void setZero(vec3 &v) {
|
void setZero(vec3 &v) {
|
||||||
v(0) = 0;
|
v(0) = 0;
|
||||||
v(1) = 0;
|
v(1) = 0;
|
||||||
v(2) = 0;
|
v(2) = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setZero(vecx &v) {
|
void setZero(vecx &v) {
|
||||||
for (int i = 0; i < v.size(); i++) {
|
for (int i = 0; i < v.size(); i++) {
|
||||||
v(i) = 0;
|
v(i) = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void setZero(mat33 &m) {
|
void setZero(mat33 &m) {
|
||||||
m(0, 0) = 0;
|
m(0, 0) = 0;
|
||||||
m(0, 1) = 0;
|
m(0, 1) = 0;
|
||||||
m(0, 2) = 0;
|
m(0, 2) = 0;
|
||||||
m(1, 0) = 0;
|
m(1, 0) = 0;
|
||||||
m(1, 1) = 0;
|
m(1, 1) = 0;
|
||||||
m(1, 2) = 0;
|
m(1, 2) = 0;
|
||||||
m(2, 0) = 0;
|
m(2, 0) = 0;
|
||||||
m(2, 1) = 0;
|
m(2, 1) = 0;
|
||||||
m(2, 2) = 0;
|
m(2, 2) = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
idScalar maxAbs(const vecx &v) {
|
idScalar maxAbs(const vecx &v) {
|
||||||
idScalar result = 0.0;
|
idScalar result = 0.0;
|
||||||
for (int i = 0; i < v.size(); i++) {
|
for (int i = 0; i < v.size(); i++) {
|
||||||
const idScalar tmp = std::fabs(v(i));
|
const idScalar tmp = std::fabs(v(i));
|
||||||
if (tmp > result) {
|
if (tmp > result) {
|
||||||
result = tmp;
|
result = tmp;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
idScalar maxAbs(const vec3 &v) {
|
idScalar maxAbs(const vec3 &v) {
|
||||||
idScalar result = 0.0;
|
idScalar result = 0.0;
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
const idScalar tmp = std::fabs(v(i));
|
const idScalar tmp = std::fabs(v(i));
|
||||||
if (tmp > result) {
|
if (tmp > result) {
|
||||||
result = tmp;
|
result = tmp;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
mat33 transformX(const idScalar &alpha) {
|
mat33 transformX(const idScalar &alpha) {
|
||||||
mat33 T;
|
mat33 T;
|
||||||
const idScalar cos_alpha = std::cos(alpha);
|
const idScalar cos_alpha = std::cos(alpha);
|
||||||
const idScalar sin_alpha = std::sin(alpha);
|
const idScalar sin_alpha = std::sin(alpha);
|
||||||
// [1 0 0]
|
// [1 0 0]
|
||||||
// [0 c s]
|
// [0 c s]
|
||||||
// [0 -s c]
|
// [0 -s c]
|
||||||
T(0, 0) = 1.0;
|
T(0, 0) = 1.0;
|
||||||
T(0, 1) = 0.0;
|
T(0, 1) = 0.0;
|
||||||
T(0, 2) = 0.0;
|
T(0, 2) = 0.0;
|
||||||
|
|
||||||
T(1, 0) = 0.0;
|
T(1, 0) = 0.0;
|
||||||
T(1, 1) = cos_alpha;
|
T(1, 1) = cos_alpha;
|
||||||
T(1, 2) = sin_alpha;
|
T(1, 2) = sin_alpha;
|
||||||
|
|
||||||
T(2, 0) = 0.0;
|
T(2, 0) = 0.0;
|
||||||
T(2, 1) = -sin_alpha;
|
T(2, 1) = -sin_alpha;
|
||||||
T(2, 2) = cos_alpha;
|
T(2, 2) = cos_alpha;
|
||||||
|
|
||||||
return T;
|
return T;
|
||||||
}
|
}
|
||||||
|
|
||||||
mat33 transformY(const idScalar &beta) {
|
mat33 transformY(const idScalar &beta) {
|
||||||
mat33 T;
|
mat33 T;
|
||||||
const idScalar cos_beta = std::cos(beta);
|
const idScalar cos_beta = std::cos(beta);
|
||||||
const idScalar sin_beta = std::sin(beta);
|
const idScalar sin_beta = std::sin(beta);
|
||||||
// [c 0 -s]
|
// [c 0 -s]
|
||||||
// [0 1 0]
|
// [0 1 0]
|
||||||
// [s 0 c]
|
// [s 0 c]
|
||||||
T(0, 0) = cos_beta;
|
T(0, 0) = cos_beta;
|
||||||
T(0, 1) = 0.0;
|
T(0, 1) = 0.0;
|
||||||
T(0, 2) = -sin_beta;
|
T(0, 2) = -sin_beta;
|
||||||
|
|
||||||
T(1, 0) = 0.0;
|
T(1, 0) = 0.0;
|
||||||
T(1, 1) = 1.0;
|
T(1, 1) = 1.0;
|
||||||
T(1, 2) = 0.0;
|
T(1, 2) = 0.0;
|
||||||
|
|
||||||
T(2, 0) = sin_beta;
|
T(2, 0) = sin_beta;
|
||||||
T(2, 1) = 0.0;
|
T(2, 1) = 0.0;
|
||||||
T(2, 2) = cos_beta;
|
T(2, 2) = cos_beta;
|
||||||
|
|
||||||
return T;
|
return T;
|
||||||
}
|
}
|
||||||
|
|
||||||
mat33 transformZ(const idScalar &gamma) {
|
mat33 transformZ(const idScalar &gamma) {
|
||||||
mat33 T;
|
mat33 T;
|
||||||
const idScalar cos_gamma = std::cos(gamma);
|
const idScalar cos_gamma = std::cos(gamma);
|
||||||
const idScalar sin_gamma = std::sin(gamma);
|
const idScalar sin_gamma = std::sin(gamma);
|
||||||
// [ c s 0]
|
// [ c s 0]
|
||||||
// [-s c 0]
|
// [-s c 0]
|
||||||
// [ 0 0 1]
|
// [ 0 0 1]
|
||||||
T(0, 0) = cos_gamma;
|
T(0, 0) = cos_gamma;
|
||||||
T(0, 1) = sin_gamma;
|
T(0, 1) = sin_gamma;
|
||||||
T(0, 2) = 0.0;
|
T(0, 2) = 0.0;
|
||||||
|
|
||||||
T(1, 0) = -sin_gamma;
|
T(1, 0) = -sin_gamma;
|
||||||
T(1, 1) = cos_gamma;
|
T(1, 1) = cos_gamma;
|
||||||
T(1, 2) = 0.0;
|
T(1, 2) = 0.0;
|
||||||
|
|
||||||
T(2, 0) = 0.0;
|
T(2, 0) = 0.0;
|
||||||
T(2, 1) = 0.0;
|
T(2, 1) = 0.0;
|
||||||
T(2, 2) = 1.0;
|
T(2, 2) = 1.0;
|
||||||
|
|
||||||
return T;
|
return T;
|
||||||
}
|
}
|
||||||
|
|
||||||
mat33 tildeOperator(const vec3 &v) {
|
mat33 tildeOperator(const vec3 &v) {
|
||||||
mat33 m;
|
mat33 m;
|
||||||
m(0, 0) = 0.0;
|
m(0, 0) = 0.0;
|
||||||
m(0, 1) = -v(2);
|
m(0, 1) = -v(2);
|
||||||
m(0, 2) = v(1);
|
m(0, 2) = v(1);
|
||||||
m(1, 0) = v(2);
|
m(1, 0) = v(2);
|
||||||
m(1, 1) = 0.0;
|
m(1, 1) = 0.0;
|
||||||
m(1, 2) = -v(0);
|
m(1, 2) = -v(0);
|
||||||
m(2, 0) = -v(1);
|
m(2, 0) = -v(1);
|
||||||
m(2, 1) = v(0);
|
m(2, 1) = v(0);
|
||||||
m(2, 2) = 0.0;
|
m(2, 2) = 0.0;
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) {
|
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) {
|
||||||
const idScalar sa = std::sin(alpha);
|
const idScalar sa = std::sin(alpha);
|
||||||
const idScalar ca = std::cos(alpha);
|
const idScalar ca = std::cos(alpha);
|
||||||
const idScalar st = std::sin(theta);
|
const idScalar st = std::sin(theta);
|
||||||
const idScalar ct = std::cos(theta);
|
const idScalar ct = std::cos(theta);
|
||||||
|
|
||||||
(*r)(0) = a;
|
(*r)(0) = a;
|
||||||
(*r)(1) = -sa * d;
|
(*r)(1) = -sa * d;
|
||||||
(*r)(2) = ca * d;
|
(*r)(2) = ca * d;
|
||||||
|
|
||||||
(*T)(0, 0) = ct;
|
(*T)(0, 0) = ct;
|
||||||
(*T)(0, 1) = -st;
|
(*T)(0, 1) = -st;
|
||||||
(*T)(0, 2) = 0.0;
|
(*T)(0, 2) = 0.0;
|
||||||
|
|
||||||
(*T)(1, 0) = st * ca;
|
(*T)(1, 0) = st * ca;
|
||||||
(*T)(1, 1) = ct * ca;
|
(*T)(1, 1) = ct * ca;
|
||||||
(*T)(1, 2) = -sa;
|
(*T)(1, 2) = -sa;
|
||||||
|
|
||||||
(*T)(2, 0) = st * sa;
|
(*T)(2, 0) = st * sa;
|
||||||
(*T)(2, 1) = ct * sa;
|
(*T)(2, 1) = ct * sa;
|
||||||
(*T)(2, 2) = ca;
|
(*T)(2, 2) = ca;
|
||||||
}
|
}
|
||||||
|
|
||||||
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
|
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
|
||||||
const idScalar c = cos(angle);
|
const idScalar c = cos(angle);
|
||||||
const idScalar s = -sin(angle);
|
const idScalar s = -sin(angle);
|
||||||
const idScalar one_m_c = 1.0 - c;
|
const idScalar one_m_c = 1.0 - c;
|
||||||
|
|
||||||
const idScalar &x = axis(0);
|
const idScalar &x = axis(0);
|
||||||
const idScalar &y = axis(1);
|
const idScalar &y = axis(1);
|
||||||
const idScalar &z = axis(2);
|
const idScalar &z = axis(2);
|
||||||
|
|
||||||
(*T)(0, 0) = x * x * one_m_c + c;
|
(*T)(0, 0) = x * x * one_m_c + c;
|
||||||
(*T)(0, 1) = x * y * one_m_c - z * s;
|
(*T)(0, 1) = x * y * one_m_c - z * s;
|
||||||
(*T)(0, 2) = x * z * one_m_c + y * s;
|
(*T)(0, 2) = x * z * one_m_c + y * s;
|
||||||
|
|
||||||
(*T)(1, 0) = x * y * one_m_c + z * s;
|
(*T)(1, 0) = x * y * one_m_c + z * s;
|
||||||
(*T)(1, 1) = y * y * one_m_c + c;
|
(*T)(1, 1) = y * y * one_m_c + c;
|
||||||
(*T)(1, 2) = y * z * one_m_c - x * s;
|
(*T)(1, 2) = y * z * one_m_c - x * s;
|
||||||
|
|
||||||
(*T)(2, 0) = x * z * one_m_c - y * s;
|
(*T)(2, 0) = x * z * one_m_c - y * s;
|
||||||
(*T)(2, 1) = y * z * one_m_c + x * s;
|
(*T)(2, 1) = y * z * one_m_c + x * s;
|
||||||
(*T)(2, 2) = z * z * one_m_c + c;
|
(*T)(2, 2) = z * z * one_m_c + c;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isPositiveDefinite(const mat33 &m) {
|
bool isPositiveDefinite(const mat33 &m) {
|
||||||
// test if all upper left determinants are positive
|
// test if all upper left determinants are positive
|
||||||
if (m(0, 0) <= 0) { // upper 1x1
|
if (m(0, 0) <= 0) { // upper 1x1
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2
|
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
|
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, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
|
||||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
|
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isPositiveSemiDefinite(const mat33 &m) {
|
bool isPositiveSemiDefinite(const mat33 &m) {
|
||||||
// test if all upper left determinants are positive
|
// test if all upper left determinants are positive
|
||||||
if (m(0, 0) < 0) { // upper 1x1
|
if (m(0, 0) < 0) { // upper 1x1
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2
|
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
|
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, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
|
||||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
|
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isPositiveSemiDefiniteFuzzy(const mat33 &m) {
|
bool isPositiveSemiDefiniteFuzzy(const mat33 &m) {
|
||||||
// test if all upper left determinants are positive
|
// test if all upper left determinants are positive
|
||||||
if (m(0, 0) < -kIsZero) { // upper 1x1
|
if (m(0, 0) < -kIsZero) { // upper 1x1
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2
|
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
|
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, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
|
||||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) {
|
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
idScalar determinant(const mat33 &m) {
|
idScalar determinant(const mat33 &m) {
|
||||||
return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) -
|
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);
|
m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) {
|
bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) {
|
||||||
// TODO(Thomas) do we really want this?
|
// TODO(Thomas) do we really want this?
|
||||||
// in cases where the inertia tensor about the center of mass is zero,
|
// 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
|
// the determinant of the inertia tensor about the joint axis is almost
|
||||||
// zero and can have a very small negative value.
|
// zero and can have a very small negative value.
|
||||||
if (!isPositiveSemiDefiniteFuzzy(I)) {
|
if (!isPositiveSemiDefiniteFuzzy(I)) {
|
||||||
error_message("invalid inertia matrix for body %d, not positive definite "
|
error_message("invalid inertia matrix for body %d, not positive definite "
|
||||||
"(fixed joint)\n",
|
"(fixed joint)\n",
|
||||||
index);
|
index);
|
||||||
error_message("matrix is:\n"
|
error_message("matrix is:\n"
|
||||||
"[%.20e %.20e %.20e;\n"
|
"[%.20e %.20e %.20e;\n"
|
||||||
"%.20e %.20e %.20e;\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(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));
|
I(2, 2));
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
|
// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
|
||||||
if (!has_fixed_joint) {
|
if (!has_fixed_joint) {
|
||||||
if (I(0, 0) + I(1, 1) < I(2, 2)) {
|
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("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
|
||||||
error_message("matrix is:\n"
|
error_message("matrix is:\n"
|
||||||
"[%.20e %.20e %.20e;\n"
|
"[%.20e %.20e %.20e;\n"
|
||||||
"%.20e %.20e %.20e;\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(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));
|
I(2, 2));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (I(0, 0) + I(1, 1) < I(2, 2)) {
|
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("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
|
||||||
error_message("matrix is:\n"
|
error_message("matrix is:\n"
|
||||||
"[%.20e %.20e %.20e;\n"
|
"[%.20e %.20e %.20e;\n"
|
||||||
"%.20e %.20e %.20e;\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(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));
|
I(2, 2));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (I(1, 1) + I(2, 2) < I(0, 0)) {
|
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("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
|
||||||
error_message("matrix is:\n"
|
error_message("matrix is:\n"
|
||||||
"[%.20e %.20e %.20e;\n"
|
"[%.20e %.20e %.20e;\n"
|
||||||
"%.20e %.20e %.20e;\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(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));
|
I(2, 2));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// check positive/zero diagonal elements
|
// check positive/zero diagonal elements
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
if (I(i, i) < 0) { // accept zero
|
if (I(i, i) < 0) { // accept zero
|
||||||
error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
|
error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// check symmetry
|
// check symmetry
|
||||||
if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) {
|
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)= "
|
error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
|
||||||
"%e\n",
|
"%e\n",
|
||||||
index, I(1, 0) - I(0, 1));
|
index, I(1, 0) - I(0, 1));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) {
|
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)= "
|
error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
|
||||||
"%e\n",
|
"%e\n",
|
||||||
index, I(2, 0) - I(0, 2));
|
index, I(2, 0) - I(0, 2));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) {
|
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,
|
error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
|
||||||
I(1, 2) - I(2, 1));
|
I(1, 2) - I(2, 1));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isValidTransformMatrix(const mat33 &m) {
|
bool isValidTransformMatrix(const mat33 &m) {
|
||||||
#define print_mat(x) \
|
#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), \
|
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))
|
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
|
||||||
|
|
||||||
// check for unit length column vectors
|
// check for unit length column vectors
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
const idScalar length_minus_1 =
|
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);
|
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) {
|
if (length_minus_1 > kAxisLengthEpsilon) {
|
||||||
error_message("Not a valid rotation matrix (column %d not unit length)\n"
|
error_message("Not a valid rotation matrix (column %d not unit length)\n"
|
||||||
"column = [%.18e %.18e %.18e]\n"
|
"column = [%.18e %.18e %.18e]\n"
|
||||||
"length-1.0= %.18e\n",
|
"length-1.0= %.18e\n",
|
||||||
i, m(0, i), m(1, i), m(2, i), length_minus_1);
|
i, m(0, i), m(1, i), m(2, i), length_minus_1);
|
||||||
print_mat(m);
|
print_mat(m);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// check for orthogonal column vectors
|
// 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) {
|
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");
|
error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
|
||||||
print_mat(m);
|
print_mat(m);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
|
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");
|
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
|
||||||
print_mat(m);
|
print_mat(m);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
|
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");
|
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
|
||||||
print_mat(m);
|
print_mat(m);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// check determinant (rotation not reflection)
|
// check determinant (rotation not reflection)
|
||||||
if (determinant(m) <= 0) {
|
if (determinant(m) <= 0) {
|
||||||
error_message("Not a valid rotation matrix (determinant <=0)\n");
|
error_message("Not a valid rotation matrix (determinant <=0)\n");
|
||||||
print_mat(m);
|
print_mat(m);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isUnitVector(const vec3 &vector) {
|
bool isUnitVector(const vec3 &vector) {
|
||||||
return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
|
return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
|
||||||
kIsZero;
|
kIsZero;
|
||||||
}
|
}
|
||||||
|
|
||||||
vec3 rpyFromMatrix(const mat33 &rot) {
|
vec3 rpyFromMatrix(const mat33 &rot) {
|
||||||
vec3 rpy;
|
vec3 rpy;
|
||||||
rpy(2) = std::atan2(-rot(1, 0), rot(0, 0));
|
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(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));
|
rpy(0) = std::atan2(-rot(2, 0), rot(2, 2));
|
||||||
return rpy;
|
return rpy;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1,5 +1,5 @@
|
|||||||
/// @file Math utility functions used in inverse dynamics library.
|
/// @file Math utility functions used in inverse dynamics library.
|
||||||
/// Defined here as they may not be provided by the math library.
|
/// Defined here as they may not be provided by the math library.
|
||||||
|
|
||||||
#ifndef IDMATH_HPP_
|
#ifndef IDMATH_HPP_
|
||||||
#define IDMATH_HPP_
|
#define IDMATH_HPP_
|
||||||
@ -39,7 +39,7 @@ bool isPositiveSemiDefiniteFuzzy(const mat33& m);
|
|||||||
|
|
||||||
/// Determinant of 3x3 matrix
|
/// Determinant of 3x3 matrix
|
||||||
/// NOTE: implemented here for portability, as determinant operation
|
/// NOTE: implemented here for portability, as determinant operation
|
||||||
/// will be implemented differently for various matrix/vector libraries
|
/// will be implemented differently for various matrix/vector libraries
|
||||||
/// @param m a 3x3 matrix
|
/// @param m a 3x3 matrix
|
||||||
/// @return det(m)
|
/// @return det(m)
|
||||||
idScalar determinant(const mat33& m);
|
idScalar determinant(const mat33& m);
|
||||||
|
@ -11,54 +11,54 @@
|
|||||||
namespace btInverseDynamics {
|
namespace btInverseDynamics {
|
||||||
|
|
||||||
MultiBodyTree::MultiBodyTree()
|
MultiBodyTree::MultiBodyTree()
|
||||||
: m_is_finalized(false),
|
: m_is_finalized(false),
|
||||||
m_mass_parameters_are_valid(true),
|
m_mass_parameters_are_valid(true),
|
||||||
m_accept_invalid_mass_parameters(false),
|
m_accept_invalid_mass_parameters(false),
|
||||||
m_impl(0x0),
|
m_impl(0x0),
|
||||||
m_init_cache(0x0) {
|
m_init_cache(0x0) {
|
||||||
m_init_cache = new InitCache();
|
m_init_cache = new InitCache();
|
||||||
}
|
}
|
||||||
|
|
||||||
MultiBodyTree::~MultiBodyTree() {
|
MultiBodyTree::~MultiBodyTree() {
|
||||||
delete m_impl;
|
delete m_impl;
|
||||||
delete m_init_cache;
|
delete m_init_cache;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
|
void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
|
||||||
m_accept_invalid_mass_parameters = flag;
|
m_accept_invalid_mass_parameters = flag;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MultiBodyTree::getAcceptInvalidMassProperties() const {
|
bool MultiBodyTree::getAcceptInvalidMassProperties() const {
|
||||||
return m_accept_invalid_mass_parameters;
|
return m_accept_invalid_mass_parameters;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const {
|
int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const {
|
||||||
return m_impl->getBodyOrigin(body_index, world_origin);
|
return m_impl->getBodyOrigin(body_index, world_origin);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const {
|
int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const {
|
||||||
return m_impl->getBodyCoM(body_index, world_com);
|
return m_impl->getBodyCoM(body_index, world_com);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const {
|
int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const {
|
||||||
return m_impl->getBodyTransform(body_index, world_T_body);
|
return m_impl->getBodyTransform(body_index, world_T_body);
|
||||||
}
|
}
|
||||||
int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const {
|
int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const {
|
||||||
return m_impl->getBodyAngularVelocity(body_index, world_omega);
|
return m_impl->getBodyAngularVelocity(body_index, world_omega);
|
||||||
}
|
}
|
||||||
int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const {
|
int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const {
|
||||||
return m_impl->getBodyLinearVelocity(body_index, world_velocity);
|
return m_impl->getBodyLinearVelocity(body_index, world_velocity);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const {
|
int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const {
|
||||||
return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
|
return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const {
|
int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const {
|
||||||
return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
|
return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
|
||||||
}
|
}
|
||||||
int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const {
|
int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const {
|
||||||
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
|
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MultiBodyTree::printTree() { m_impl->printTree(); }
|
void MultiBodyTree::printTree() { m_impl->printTree(); }
|
||||||
@ -69,263 +69,263 @@ int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
|
|||||||
int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
|
int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
|
||||||
|
|
||||||
int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
|
int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
|
||||||
vecx *joint_forces) {
|
vecx *joint_forces) {
|
||||||
if (false == m_is_finalized) {
|
if (false == m_is_finalized) {
|
||||||
error_message("system has not been initialized\n");
|
error_message("system has not been initialized\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
|
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
|
||||||
error_message("error in inverse dynamics calculation\n");
|
error_message("error in inverse dynamics calculation\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
|
int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
|
||||||
const bool initialize_matrix,
|
const bool initialize_matrix,
|
||||||
const bool set_lower_triangular_matrix, matxx *mass_matrix) {
|
const bool set_lower_triangular_matrix, matxx *mass_matrix) {
|
||||||
if (false == m_is_finalized) {
|
if (false == m_is_finalized) {
|
||||||
error_message("system has not been initialized\n");
|
error_message("system has not been initialized\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
if (-1 ==
|
if (-1 ==
|
||||||
m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
|
m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
|
||||||
set_lower_triangular_matrix, mass_matrix)) {
|
set_lower_triangular_matrix, mass_matrix)) {
|
||||||
error_message("error in mass matrix calculation\n");
|
error_message("error in mass matrix calculation\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
|
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
|
||||||
return calculateMassMatrix(q, true, true, true, mass_matrix);
|
return calculateMassMatrix(q, true, true, true, mass_matrix);
|
||||||
}
|
}
|
||||||
int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
|
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 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
|
||||||
const vec3 &body_axis_of_motion_, idScalar mass,
|
const vec3 &body_axis_of_motion_, idScalar mass,
|
||||||
const vec3 &body_r_body_com, const mat33 &body_I_body,
|
const vec3 &body_r_body_com, const mat33 &body_I_body,
|
||||||
const int user_int, void *user_ptr) {
|
const int user_int, void *user_ptr) {
|
||||||
if (body_index < 0) {
|
if (body_index < 0) {
|
||||||
error_message("body index must be positive (got %d)\n", body_index);
|
error_message("body index must be positive (got %d)\n", body_index);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
vec3 body_axis_of_motion(body_axis_of_motion_);
|
vec3 body_axis_of_motion(body_axis_of_motion_);
|
||||||
switch (joint_type) {
|
switch (joint_type) {
|
||||||
case REVOLUTE:
|
case REVOLUTE:
|
||||||
case PRISMATIC:
|
case PRISMATIC:
|
||||||
// check if axis is unit vector
|
// check if axis is unit vector
|
||||||
if (!isUnitVector(body_axis_of_motion)) {
|
if (!isUnitVector(body_axis_of_motion)) {
|
||||||
warning_message(
|
warning_message(
|
||||||
"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
|
"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));
|
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) +
|
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(1), 2) +
|
||||||
std::pow(body_axis_of_motion(2), 2));
|
std::pow(body_axis_of_motion(2), 2));
|
||||||
if (length < std::sqrt(std::numeric_limits<idScalar>::min())) {
|
if (length < std::sqrt(std::numeric_limits<idScalar>::min())) {
|
||||||
error_message("axis of motion vector too short (%e)\n", length);
|
error_message("axis of motion vector too short (%e)\n", length);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
|
body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case FIXED:
|
case FIXED:
|
||||||
break;
|
break;
|
||||||
case FLOATING:
|
case FLOATING:
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
error_message("unknown joint type %d\n", joint_type);
|
error_message("unknown joint type %d\n", joint_type);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// sanity check for mass properties. Zero mass is OK.
|
// sanity check for mass properties. Zero mass is OK.
|
||||||
if (mass < 0) {
|
if (mass < 0) {
|
||||||
m_mass_parameters_are_valid = false;
|
m_mass_parameters_are_valid = false;
|
||||||
error_message("Body %d has invalid mass %e\n", body_index, mass);
|
error_message("Body %d has invalid mass %e\n", body_index, mass);
|
||||||
if (!m_accept_invalid_mass_parameters) {
|
if (!m_accept_invalid_mass_parameters) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) {
|
if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) {
|
||||||
m_mass_parameters_are_valid = false;
|
m_mass_parameters_are_valid = false;
|
||||||
// error message printed in function call
|
// error message printed in function call
|
||||||
if (!m_accept_invalid_mass_parameters) {
|
if (!m_accept_invalid_mass_parameters) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!isValidTransformMatrix(body_T_parent_ref)) {
|
if (!isValidTransformMatrix(body_T_parent_ref)) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref,
|
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_T_parent_ref, body_axis_of_motion, mass, body_r_body_com,
|
||||||
body_I_body, user_int, user_ptr);
|
body_I_body, user_int, user_ptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const {
|
int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const {
|
||||||
return m_impl->getParentIndex(body_index, parent_index);
|
return m_impl->getParentIndex(body_index, parent_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getUserInt(const int body_index, int *user_int) const {
|
int MultiBodyTree::getUserInt(const int body_index, int *user_int) const {
|
||||||
return m_impl->getUserInt(body_index, user_int);
|
return m_impl->getUserInt(body_index, user_int);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const {
|
int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const {
|
||||||
return m_impl->getUserPtr(body_index, user_ptr);
|
return m_impl->getUserPtr(body_index, user_ptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::setUserInt(const int body_index, const int user_int) {
|
int MultiBodyTree::setUserInt(const int body_index, const int user_int) {
|
||||||
return m_impl->setUserInt(body_index, user_int);
|
return m_impl->setUserInt(body_index, user_int);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) {
|
int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) {
|
||||||
return m_impl->setUserPtr(body_index, user_ptr);
|
return m_impl->setUserPtr(body_index, user_ptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::finalize() {
|
int MultiBodyTree::finalize() {
|
||||||
const int &num_bodies = m_init_cache->numBodies();
|
const int &num_bodies = m_init_cache->numBodies();
|
||||||
const int &num_dofs = m_init_cache->numDoFs();
|
const int &num_dofs = m_init_cache->numDoFs();
|
||||||
|
|
||||||
// 1 allocate internal MultiBody structure
|
// 1 allocate internal MultiBody structure
|
||||||
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
|
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
|
||||||
|
|
||||||
// 2 build new index set assuring index(parent) < index(child)
|
// 2 build new index set assuring index(parent) < index(child)
|
||||||
if (-1 == m_init_cache->buildIndexSets()) {
|
if (-1 == m_init_cache->buildIndexSets()) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
|
m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
|
||||||
|
|
||||||
// 3 setup internal kinematic and dynamic data
|
// 3 setup internal kinematic and dynamic data
|
||||||
for (int index = 0; index < num_bodies; index++) {
|
for (int index = 0; index < num_bodies; index++) {
|
||||||
InertiaData inertia;
|
InertiaData inertia;
|
||||||
JointData joint;
|
JointData joint;
|
||||||
if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
|
if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
if (-1 == m_init_cache->getJointData(index, &joint)) {
|
if (-1 == m_init_cache->getJointData(index, &joint)) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
RigidBody &rigid_body = m_impl->m_body_list[index];
|
RigidBody &rigid_body = m_impl->m_body_list[index];
|
||||||
|
|
||||||
rigid_body.m_mass = inertia.m_mass;
|
rigid_body.m_mass = inertia.m_mass;
|
||||||
rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com;
|
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_body_I_body = inertia.m_body_I_body;
|
||||||
rigid_body.m_joint_type = joint.m_type;
|
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_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_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_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
|
||||||
rigid_body.m_joint_type = joint.m_type;
|
rigid_body.m_joint_type = joint.m_type;
|
||||||
|
|
||||||
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
|
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
|
||||||
// matrices.
|
// matrices.
|
||||||
switch (rigid_body.m_joint_type) {
|
switch (rigid_body.m_joint_type) {
|
||||||
case REVOLUTE:
|
case REVOLUTE:
|
||||||
rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
|
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(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_JR(2) = joint.m_child_axis_of_motion(2);
|
||||||
rigid_body.m_Jac_JT(0) = 0.0;
|
rigid_body.m_Jac_JT(0) = 0.0;
|
||||||
rigid_body.m_Jac_JT(1) = 0.0;
|
rigid_body.m_Jac_JT(1) = 0.0;
|
||||||
rigid_body.m_Jac_JT(2) = 0.0;
|
rigid_body.m_Jac_JT(2) = 0.0;
|
||||||
break;
|
break;
|
||||||
case PRISMATIC:
|
case PRISMATIC:
|
||||||
rigid_body.m_Jac_JR(0) = 0.0;
|
rigid_body.m_Jac_JR(0) = 0.0;
|
||||||
rigid_body.m_Jac_JR(1) = 0.0;
|
rigid_body.m_Jac_JR(1) = 0.0;
|
||||||
rigid_body.m_Jac_JR(2) = 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(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(1) = joint.m_child_axis_of_motion(1);
|
||||||
rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
|
rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
|
||||||
break;
|
break;
|
||||||
case FIXED:
|
case FIXED:
|
||||||
// NOTE/TODO: dimension really should be zero ..
|
// NOTE/TODO: dimension really should be zero ..
|
||||||
rigid_body.m_Jac_JR(0) = 0.0;
|
rigid_body.m_Jac_JR(0) = 0.0;
|
||||||
rigid_body.m_Jac_JR(1) = 0.0;
|
rigid_body.m_Jac_JR(1) = 0.0;
|
||||||
rigid_body.m_Jac_JR(2) = 0.0;
|
rigid_body.m_Jac_JR(2) = 0.0;
|
||||||
rigid_body.m_Jac_JT(0) = 0.0;
|
rigid_body.m_Jac_JT(0) = 0.0;
|
||||||
rigid_body.m_Jac_JT(1) = 0.0;
|
rigid_body.m_Jac_JT(1) = 0.0;
|
||||||
rigid_body.m_Jac_JT(2) = 0.0;
|
rigid_body.m_Jac_JT(2) = 0.0;
|
||||||
break;
|
break;
|
||||||
case FLOATING:
|
case FLOATING:
|
||||||
// NOTE/TODO: this is not really correct.
|
// NOTE/TODO: this is not really correct.
|
||||||
// the Jacobians should be 3x3 matrices here !
|
// the Jacobians should be 3x3 matrices here !
|
||||||
rigid_body.m_Jac_JR(0) = 0.0;
|
rigid_body.m_Jac_JR(0) = 0.0;
|
||||||
rigid_body.m_Jac_JR(1) = 0.0;
|
rigid_body.m_Jac_JR(1) = 0.0;
|
||||||
rigid_body.m_Jac_JR(2) = 0.0;
|
rigid_body.m_Jac_JR(2) = 0.0;
|
||||||
rigid_body.m_Jac_JT(0) = 0.0;
|
rigid_body.m_Jac_JT(0) = 0.0;
|
||||||
rigid_body.m_Jac_JT(1) = 0.0;
|
rigid_body.m_Jac_JT(1) = 0.0;
|
||||||
rigid_body.m_Jac_JT(2) = 0.0;
|
rigid_body.m_Jac_JT(2) = 0.0;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
|
error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 4 assign degree of freedom indices & build per-joint-type index arrays
|
// 4 assign degree of freedom indices & build per-joint-type index arrays
|
||||||
if (-1 == m_impl->generateIndexSets()) {
|
if (-1 == m_impl->generateIndexSets()) {
|
||||||
error_message("generating index sets\n");
|
error_message("generating index sets\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 5 do some pre-computations ..
|
// 5 do some pre-computations ..
|
||||||
m_impl->calculateStaticData();
|
m_impl->calculateStaticData();
|
||||||
|
|
||||||
// 6. make sure all user forces are set to zero, as this might not happen
|
// 6. make sure all user forces are set to zero, as this might not happen
|
||||||
// in the vector ctors.
|
// in the vector ctors.
|
||||||
m_impl->clearAllUserForcesAndMoments();
|
m_impl->clearAllUserForcesAndMoments();
|
||||||
|
|
||||||
m_is_finalized = true;
|
m_is_finalized = true;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
|
int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
|
||||||
return m_impl->setGravityInWorldFrame(gravity);
|
return m_impl->setGravityInWorldFrame(gravity);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const {
|
int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const {
|
||||||
return m_impl->getJointType(body_index, joint_type);
|
return m_impl->getJointType(body_index, joint_type);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const {
|
int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const {
|
||||||
return m_impl->getJointTypeStr(body_index, joint_type);
|
return m_impl->getJointTypeStr(body_index, joint_type);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const {
|
int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const {
|
||||||
return m_impl->getDoFOffset(body_index, q_offset);
|
return m_impl->getDoFOffset(body_index, q_offset);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
|
int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
|
||||||
return m_impl->setBodyMass(body_index, mass);
|
return m_impl->setBodyMass(body_index, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) {
|
int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) {
|
||||||
return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
|
return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) {
|
int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) {
|
||||||
return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
|
return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const {
|
int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const {
|
||||||
return m_impl->getBodyMass(body_index, mass);
|
return m_impl->getBodyMass(body_index, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const {
|
int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const {
|
||||||
return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
|
return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const {
|
int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const {
|
||||||
return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
|
return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
|
void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
|
||||||
|
|
||||||
int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) {
|
int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) {
|
||||||
return m_impl->addUserForce(body_index, body_force);
|
return m_impl->addUserForce(body_index, body_force);
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) {
|
int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) {
|
||||||
return m_impl->addUserMoment(body_index, body_moment);
|
return m_impl->addUserMoment(body_index, body_moment);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -1,20 +1,21 @@
|
|||||||
#ifndef MULTIBODYTREE_HPP_
|
#ifndef MULTIBODYTREE_HPP_
|
||||||
#define MULTIBODYTREE_HPP_
|
#define MULTIBODYTREE_HPP_
|
||||||
|
|
||||||
|
#include "IDConfig.hpp"
|
||||||
#include "IDMath.hpp"
|
#include "IDMath.hpp"
|
||||||
|
|
||||||
namespace btInverseDynamics {
|
namespace btInverseDynamics {
|
||||||
|
|
||||||
/// Enumeration of supported joint types
|
/// Enumeration of supported joint types
|
||||||
enum JointType {
|
enum JointType {
|
||||||
/// no degree of freedom, moves with parent
|
/// no degree of freedom, moves with parent
|
||||||
FIXED = 0,
|
FIXED = 0,
|
||||||
/// one rotational degree of freedom relative to parent
|
/// one rotational degree of freedom relative to parent
|
||||||
REVOLUTE,
|
REVOLUTE,
|
||||||
/// one translational degree of freedom relative to parent
|
/// one translational degree of freedom relative to parent
|
||||||
PRISMATIC,
|
PRISMATIC,
|
||||||
/// six degrees of freedom relative to parent
|
/// six degrees of freedom relative to parent
|
||||||
FLOATING
|
FLOATING
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Interface class for calculating inverse dynamics for tree structured
|
/// Interface class for calculating inverse dynamics for tree structured
|
||||||
@ -24,285 +25,284 @@ enum JointType {
|
|||||||
/// The q vector contains the generalized coordinate set defining the tree's configuration.
|
/// 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
|
/// Every joint adds elements that define the corresponding link's frame pose relative to
|
||||||
/// its parent. For the joint types that is:
|
/// its parent. For the joint types that is:
|
||||||
/// - FIXED: none
|
/// - FIXED: none
|
||||||
/// - REVOLUTE: angle of rotation [rad]
|
/// - REVOLUTE: angle of rotation [rad]
|
||||||
/// - PRISMATIC: displacement [m]
|
/// - PRISMATIC: displacement [m]
|
||||||
/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
|
/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
|
||||||
/// (in that order)
|
/// (in that order)
|
||||||
/// The u vector contains the generalized speeds, which are
|
/// The u vector contains the generalized speeds, which are
|
||||||
/// - FIXED: none
|
/// - FIXED: none
|
||||||
/// - REVOLUTE: time derivative of angle of rotation [rad/s]
|
/// - REVOLUTE: time derivative of angle of rotation [rad/s]
|
||||||
/// - PRISMATIC: time derivative of displacement [m/s]
|
/// - PRISMATIC: time derivative of displacement [m/s]
|
||||||
/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles)
|
/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles)
|
||||||
/// and time derivative of displacement in parent frame [m/s]
|
/// 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
|
/// The q and u vectors are obtained by stacking contributions of all bodies in one
|
||||||
/// vector in the order of body indices.
|
/// vector in the order of body indices.
|
||||||
///
|
///
|
||||||
/// Note on generalized forces: analogous to u, i.e.,
|
/// Note on generalized forces: analogous to u, i.e.,
|
||||||
/// - FIXED: none
|
/// - FIXED: none
|
||||||
/// - REVOLUTE: moment [Nm], about joint axis
|
/// - REVOLUTE: moment [Nm], about joint axis
|
||||||
/// - PRISMATIC: force [N], along joint axis
|
/// - PRISMATIC: force [N], along joint axis
|
||||||
/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame
|
/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame
|
||||||
/// (in that order)
|
/// (in that order)
|
||||||
///
|
///
|
||||||
/// TODO - force element interface (friction, springs, dampers, etc)
|
/// TODO - force element interface (friction, springs, dampers, etc)
|
||||||
/// - gears and motor inertia
|
/// - gears and motor inertia
|
||||||
class MultiBodyTree {
|
class MultiBodyTree {
|
||||||
public:
|
public:
|
||||||
/// The contructor.
|
/// The contructor.
|
||||||
/// Initialization & allocation is via addBody and buildSystem calls.
|
/// Initialization & allocation is via addBody and buildSystem calls.
|
||||||
MultiBodyTree();
|
MultiBodyTree();
|
||||||
/// the destructor. This also deallocates all memory
|
/// the destructor. This also deallocates all memory
|
||||||
~MultiBodyTree();
|
~MultiBodyTree();
|
||||||
|
|
||||||
/// Add body to the system. this allocates memory and not real-time safe.
|
/// 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
|
/// This only adds the data to an initial cache. After all bodies have been
|
||||||
/// added,
|
/// added,
|
||||||
/// the system is setup using the buildSystem call
|
/// the system is setup using the buildSystem call
|
||||||
/// @param body_index index of the body to be added. Must >=0, <number of bodies,
|
/// @param body_index index of the body to be added. Must >=0, <number of bodies,
|
||||||
/// and index of parent must be < index of body
|
/// and index of parent must be < index of body
|
||||||
/// @param parent_index index of the parent body
|
/// @param parent_index index of the parent body
|
||||||
/// The root of the tree has index 0 and its parent (the world frame)
|
/// The root of the tree has index 0 and its parent (the world frame)
|
||||||
/// is assigned index -1
|
/// is assigned index -1
|
||||||
/// the rotation and translation relative to the parent are taken as
|
/// the rotation and translation relative to the parent are taken as
|
||||||
/// pose of the root body relative to the world frame. Other parameters
|
/// pose of the root body relative to the world frame. Other parameters
|
||||||
/// are ignored
|
/// are ignored
|
||||||
/// @param JointType type of joint connecting the body to the parent
|
/// @param JointType type of joint connecting the body to the parent
|
||||||
/// @param mass the mass of the body
|
/// @param mass the mass of the body
|
||||||
/// @param body_r_body_com the center of mass of the body relative to and
|
/// @param body_r_body_com the center of mass of the body relative to and
|
||||||
/// described in
|
/// described in
|
||||||
/// the body fixed frame, which is located in the joint axis connecting
|
/// the body fixed frame, which is located in the joint axis connecting
|
||||||
/// the body to its parent
|
/// the body to its parent
|
||||||
/// @param body_I_body the moment of inertia of the body w.r.t the body-fixed
|
/// @param body_I_body the moment of inertia of the body w.r.t the body-fixed
|
||||||
/// frame
|
/// frame
|
||||||
/// (ie, the reference point is the origin of the body-fixed frame and
|
/// (ie, the reference point is the origin of the body-fixed frame and
|
||||||
/// the matrix is written
|
/// the matrix is written
|
||||||
/// w.r.t. those unit vectors)
|
/// w.r.t. those unit vectors)
|
||||||
/// @param parent_r_parent_body_ref position of joint relative to the parent
|
/// @param parent_r_parent_body_ref position of joint relative to the parent
|
||||||
/// body's reference frame
|
/// body's reference frame
|
||||||
/// for q=0, written in the parent bodies reference frame
|
/// for q=0, written in the parent bodies reference frame
|
||||||
/// @param body_axis_of_motion translation/rotation axis in body-fixed frame.
|
/// @param body_axis_of_motion translation/rotation axis in body-fixed frame.
|
||||||
/// Ignored for joints that are not revolute or prismatic.
|
/// Ignored for joints that are not revolute or prismatic.
|
||||||
/// must be a unit vector.
|
/// must be a unit vector.
|
||||||
/// @param body_T_parent_ref transform matrix from parent to body reference
|
/// @param body_T_parent_ref transform matrix from parent to body reference
|
||||||
/// frame for q=0.
|
/// frame for q=0.
|
||||||
/// This is the matrix transforming a vector represented in the
|
/// This is the matrix transforming a vector represented in the
|
||||||
/// parent's reference frame into one represented
|
/// parent's reference frame into one represented
|
||||||
/// in this body's reference frame.
|
/// in this body's reference frame.
|
||||||
/// ie, if parent_vec is a vector in R^3 whose components are w.r.t to
|
/// ie, if parent_vec is a vector in R^3 whose components are w.r.t to
|
||||||
/// the parent's reference frame,
|
/// the parent's reference frame,
|
||||||
/// then the same vector written w.r.t. this body's frame (for q=0) is
|
/// then the same vector written w.r.t. this body's frame (for q=0) is
|
||||||
/// given by
|
/// given by
|
||||||
/// body_vec = parent_R_body_ref * parent_vec
|
/// body_vec = parent_R_body_ref * parent_vec
|
||||||
/// @param user_ptr pointer to user data
|
/// @param user_ptr pointer to user data
|
||||||
/// @param user_int pointer to user integer
|
/// @param user_int pointer to user integer
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int addBody(int body_index, int parent_index, JointType joint_type,
|
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& 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 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);
|
const mat33& body_I_body, const int user_int, void* user_ptr);
|
||||||
/// set policy for invalid mass properties
|
/// set policy for invalid mass properties
|
||||||
/// @param flag if true, invalid mass properties are accepted,
|
/// @param flag if true, invalid mass properties are accepted,
|
||||||
/// the default is false
|
/// the default is false
|
||||||
void setAcceptInvalidMassParameters(bool flag);
|
void setAcceptInvalidMassParameters(bool flag);
|
||||||
/// @return the mass properties policy flag
|
/// @return the mass properties policy flag
|
||||||
bool getAcceptInvalidMassProperties() const;
|
bool getAcceptInvalidMassProperties() const;
|
||||||
/// build internal data structures
|
/// build internal data structures
|
||||||
/// call this after all bodies have been added via addBody
|
/// call this after all bodies have been added via addBody
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int finalize();
|
int finalize();
|
||||||
/// pretty print ascii description of tree to stdout
|
/// pretty print ascii description of tree to stdout
|
||||||
void printTree();
|
void printTree();
|
||||||
/// print tree data to stdout
|
/// print tree data to stdout
|
||||||
void printTreeData();
|
void printTreeData();
|
||||||
/// calculate joint forces for given generalized state & derivatives
|
/// calculate joint forces for given generalized state & derivatives
|
||||||
/// @param q generalized coordinates
|
/// @param q generalized coordinates
|
||||||
/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
|
/// @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 dot_u time derivative of u
|
||||||
/// @param joint_forces this is where the resulting joint forces will be
|
/// @param joint_forces this is where the resulting joint forces will be
|
||||||
/// stored. dim(joint_forces) = dim(u)
|
/// stored. dim(joint_forces) = dim(u)
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
|
int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
|
||||||
vecx* joint_forces);
|
vecx* joint_forces);
|
||||||
/// Calculate joint space mass matrix
|
/// Calculate joint space mass matrix
|
||||||
/// @param q generalized coordinates
|
/// @param q generalized coordinates
|
||||||
/// @param initialize_matrix if true, initialize mass matrix with zero.
|
/// @param initialize_matrix if true, initialize mass matrix with zero.
|
||||||
/// If mass_matrix is initialized to zero externally and only used
|
/// If mass_matrix is initialized to zero externally and only used
|
||||||
/// for mass matrix computations for the same system, it is safe to
|
/// for mass matrix computations for the same system, it is safe to
|
||||||
/// set this to false.
|
/// set this to false.
|
||||||
/// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix
|
/// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix
|
||||||
/// is also populated, otherwise not.
|
/// is also populated, otherwise not.
|
||||||
/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
|
/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
|
||||||
/// @return -1 on error, 0 on success
|
/// @return -1 on error, 0 on success
|
||||||
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
|
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
|
||||||
const bool initialize_matrix, const bool set_lower_triangular_matrix,
|
const bool initialize_matrix, const bool set_lower_triangular_matrix,
|
||||||
matxx* mass_matrix);
|
matxx* mass_matrix);
|
||||||
|
|
||||||
/// Calculate joint space mass matrix.
|
/// Calculate joint space mass matrix.
|
||||||
/// This version will update kinematics, initialize all mass_matrix elements to zero and
|
/// This version will update kinematics, initialize all mass_matrix elements to zero and
|
||||||
/// populate all mass matrix entries.
|
/// populate all mass matrix entries.
|
||||||
/// @param q generalized coordinates
|
/// @param q generalized coordinates
|
||||||
/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
|
/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
|
||||||
/// @return -1 on error, 0 on success
|
/// @return -1 on error, 0 on success
|
||||||
int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
|
int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
|
||||||
|
|
||||||
/// set gravitational acceleration
|
/// set gravitational acceleration
|
||||||
/// the default is [0;0;-9.8] in the world frame
|
/// the default is [0;0;-9.8] in the world frame
|
||||||
/// @param gravity the gravitational acceleration in world frame
|
/// @param gravity the gravitational acceleration in world frame
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int setGravityInWorldFrame(const vec3& gravity);
|
int setGravityInWorldFrame(const vec3& gravity);
|
||||||
/// returns number of bodies in tree
|
/// returns number of bodies in tree
|
||||||
int numBodies() const;
|
int numBodies() const;
|
||||||
/// returns number of mechanical degrees of freedom (dimension of q-vector)
|
/// returns number of mechanical degrees of freedom (dimension of q-vector)
|
||||||
int numDoFs() const;
|
int numDoFs() const;
|
||||||
/// get origin of a body-fixed frame, represented in world frame
|
/// get origin of a body-fixed frame, represented in world frame
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_origin pointer for return data
|
/// @param world_origin pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyOrigin(const int body_index, vec3* world_origin) const;
|
int getBodyOrigin(const int body_index, vec3* world_origin) const;
|
||||||
/// get center of mass of a body, represented in world frame
|
/// get center of mass of a body, represented in world frame
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_com pointer for return data
|
/// @param world_com pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyCoM(const int body_index, vec3* world_com) const;
|
int getBodyCoM(const int body_index, vec3* world_com) const;
|
||||||
/// get transform from of a body-fixed frame to the world frame
|
/// get transform from of a body-fixed frame to the world frame
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_T_body pointer for return data
|
/// @param world_T_body pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyTransform(const int body_index, mat33* world_T_body) const;
|
int getBodyTransform(const int body_index, mat33* world_T_body) const;
|
||||||
/// get absolute angular velocity for a body, represented in the world frame
|
/// get absolute angular velocity for a body, represented in the world frame
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_omega pointer for return data
|
/// @param world_omega pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
|
int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
|
||||||
/// get linear velocity of a body, represented in world frame
|
/// get linear velocity of a body, represented in world frame
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_velocity pointer for return data
|
/// @param world_velocity pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
|
int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
|
||||||
/// get linear velocity of a body's CoM, represented in world frame
|
/// get linear velocity of a body's CoM, represented in world frame
|
||||||
/// (not required for inverse dynamics, provided for convenience)
|
/// (not required for inverse dynamics, provided for convenience)
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_vel_com pointer for return data
|
/// @param world_vel_com pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
|
int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
|
||||||
/// get origin of a body-fixed frame, represented in world frame
|
/// get origin of a body-fixed frame, represented in world frame
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_origin pointer for return data
|
/// @param world_origin pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
|
int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
|
||||||
/// get origin of a body-fixed frame, represented in world frame
|
/// get origin of a body-fixed frame, represented in world frame
|
||||||
/// NOTE: this will include the gravitational acceleration, so the actual acceleration is
|
/// NOTE: this will include the gravitational acceleration, so the actual acceleration is
|
||||||
/// obtainened by setting gravitational acceleration to zero, or subtracting it.
|
/// obtainened by setting gravitational acceleration to zero, or subtracting it.
|
||||||
/// @param body_index index for frame/body
|
/// @param body_index index for frame/body
|
||||||
/// @param world_origin pointer for return data
|
/// @param world_origin pointer for return data
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
|
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
|
||||||
/// returns the (internal) index of body
|
/// returns the (internal) index of body
|
||||||
/// @param body_index is the index of a body (internal: TODO: fix/clarify
|
/// @param body_index is the index of a body (internal: TODO: fix/clarify
|
||||||
/// indexing!)
|
/// indexing!)
|
||||||
/// @param parent_index pointer to where parent index will be stored
|
/// @param parent_index pointer to where parent index will be stored
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getParentIndex(const int body_index, int* parent_index) const;
|
int getParentIndex(const int body_index, int* parent_index) const;
|
||||||
/// get joint type
|
/// get joint type
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param joint_type the corresponding joint type
|
/// @param joint_type the corresponding joint type
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getJointType(const int body_index, JointType* joint_type) const;
|
int getJointType(const int body_index, JointType* joint_type) const;
|
||||||
/// get joint type as string
|
/// get joint type as string
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param joint_type string naming the corresponding joint type
|
/// @param joint_type string naming the corresponding joint type
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getJointTypeStr(const int body_index, const char** joint_type) const;
|
int getJointTypeStr(const int body_index, const char** joint_type) const;
|
||||||
/// get offset for degrees of freedom of this body into the q-vector
|
/// get offset for degrees of freedom of this body into the q-vector
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param q_offset offset the q vector
|
/// @param q_offset offset the q vector
|
||||||
/// @return -1 on error, 0 on success
|
/// @return -1 on error, 0 on success
|
||||||
int getDoFOffset(const int body_index, int* q_offset) const;
|
int getDoFOffset(const int body_index, int* q_offset) const;
|
||||||
/// get user integer. not used by the library.
|
/// get user integer. not used by the library.
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param user_int the user integer
|
/// @param user_int the user integer
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getUserInt(const int body_index, int* user_int) const;
|
int getUserInt(const int body_index, int* user_int) const;
|
||||||
/// get user pointer. not used by the library.
|
/// get user pointer. not used by the library.
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param user_ptr the user pointer
|
/// @param user_ptr the user pointer
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int getUserPtr(const int body_index, void** user_ptr) const;
|
int getUserPtr(const int body_index, void** user_ptr) const;
|
||||||
/// set user integer. not used by the library.
|
/// set user integer. not used by the library.
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param user_int the user integer
|
/// @param user_int the user integer
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int setUserInt(const int body_index, const int user_int);
|
int setUserInt(const int body_index, const int user_int);
|
||||||
/// set user pointer. not used by the library.
|
/// set user pointer. not used by the library.
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param user_ptr the user pointer
|
/// @param user_ptr the user pointer
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int setUserPtr(const int body_index, void* const user_ptr);
|
int setUserPtr(const int body_index, void* const user_ptr);
|
||||||
/// set mass for a body
|
/// set mass for a body
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param mass the mass to set
|
/// @param mass the mass to set
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int setBodyMass(const int body_index, const idScalar mass);
|
int setBodyMass(const int body_index, const idScalar mass);
|
||||||
/// set first moment of mass for a body
|
/// set first moment of mass for a body
|
||||||
/// (mass * center of mass, in body fixed frame, relative to joint)
|
/// (mass * center of mass, in body fixed frame, relative to joint)
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param first_mass_moment the vector to set
|
/// @param first_mass_moment the vector to set
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
|
int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
|
||||||
/// set second moment of mass for a body
|
/// set second moment of mass for a body
|
||||||
/// (moment of inertia, in body fixed frame, relative to joint)
|
/// (moment of inertia, in body fixed frame, relative to joint)
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param second_mass_moment the inertia matrix
|
/// @param second_mass_moment the inertia matrix
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
|
int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
|
||||||
/// get mass for a body
|
/// get mass for a body
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param mass the mass
|
/// @param mass the mass
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getBodyMass(const int body_index, idScalar* mass) const;
|
int getBodyMass(const int body_index, idScalar* mass) const;
|
||||||
/// get first moment of mass for a body
|
/// get first moment of mass for a body
|
||||||
/// (mass * center of mass, in body fixed frame, relative to joint)
|
/// (mass * center of mass, in body fixed frame, relative to joint)
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param first_moment the vector
|
/// @param first_moment the vector
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
|
int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
|
||||||
/// get second moment of mass for a body
|
/// get second moment of mass for a body
|
||||||
/// (moment of inertia, in body fixed frame, relative to joint)
|
/// (moment of inertia, in body fixed frame, relative to joint)
|
||||||
/// @param body_index index of the body
|
/// @param body_index index of the body
|
||||||
/// @param second_mass_moment the inertia matrix
|
/// @param second_mass_moment the inertia matrix
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
|
int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
|
||||||
/// set all user forces and moments to zero
|
/// set all user forces and moments to zero
|
||||||
void clearAllUserForcesAndMoments();
|
void clearAllUserForcesAndMoments();
|
||||||
/// Add an external force to a body, acting at the origin of the body-fixed frame.
|
/// 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
|
/// Calls to addUserForce are cumulative. Set the user force and moment to zero
|
||||||
/// via clearAllUserForcesAndMoments()
|
/// via clearAllUserForcesAndMoments()
|
||||||
/// @param body_force the force represented in the body-fixed frame of reference
|
/// @param body_force the force represented in the body-fixed frame of reference
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int addUserForce(const int body_index, const vec3& body_force);
|
int addUserForce(const int body_index, const vec3& body_force);
|
||||||
/// Add an external moment to a body.
|
/// Add an external moment to a body.
|
||||||
/// Calls to addUserMoment are cumulative. Set the user force and moment to zero
|
/// Calls to addUserMoment are cumulative. Set the user force and moment to zero
|
||||||
/// via clearAllUserForcesAndMoments()
|
/// via clearAllUserForcesAndMoments()
|
||||||
/// @param body_moment the moment represented in the body-fixed frame of reference
|
/// @param body_moment the moment represented in the body-fixed frame of reference
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int addUserMoment(const int body_index, const vec3& body_moment);
|
int addUserMoment(const int body_index, const vec3& body_moment);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// flag indicating if system has been initialized
|
// flag indicating if system has been initialized
|
||||||
bool m_is_finalized;
|
bool m_is_finalized;
|
||||||
// flag indicating if mass properties are physically valid
|
// flag indicating if mass properties are physically valid
|
||||||
bool m_mass_parameters_are_valid;
|
bool m_mass_parameters_are_valid;
|
||||||
// flag defining if unphysical mass parameters are accepted
|
// flag defining if unphysical mass parameters are accepted
|
||||||
bool m_accept_invalid_mass_parameters;
|
bool m_accept_invalid_mass_parameters;
|
||||||
// This struct implements the inverse dynamics calculations
|
// This struct implements the inverse dynamics calculations
|
||||||
class MultiBodyImpl;
|
class MultiBodyImpl;
|
||||||
MultiBodyImpl* m_impl;
|
MultiBodyImpl* m_impl;
|
||||||
// cache data structure for initialization
|
// cache data structure for initialization
|
||||||
class InitCache;
|
class InitCache;
|
||||||
InitCache* m_init_cache;
|
InitCache* m_init_cache;
|
||||||
};
|
};
|
||||||
} // namespace btInverseDynamics
|
} // namespace btInverseDynamics
|
||||||
|
|
||||||
#endif // MULTIBODYTREE_HPP_
|
#endif // MULTIBODYTREE_HPP_
|
||||||
|
@ -17,29 +17,29 @@ typedef btMatrixX<idScalar> matxx;
|
|||||||
|
|
||||||
class vec3 : public btVector3 {
|
class vec3 : public btVector3 {
|
||||||
public:
|
public:
|
||||||
vec3() : btVector3() {}
|
vec3() : btVector3() {}
|
||||||
vec3(const btVector3& btv) { *this = btv; }
|
vec3(const btVector3& btv) { *this = btv; }
|
||||||
idScalar& operator()(int i) { return (*this)[i]; }
|
idScalar& operator()(int i) { return (*this)[i]; }
|
||||||
const idScalar& operator()(int i) const { return (*this)[i]; }
|
const idScalar& operator()(int i) const { return (*this)[i]; }
|
||||||
const int size() const { return 3; }
|
const int size() const { return 3; }
|
||||||
const vec3& operator=(const btVector3& rhs) {
|
const vec3& operator=(const btVector3& rhs) {
|
||||||
*static_cast<btVector3*>(this) = rhs;
|
*static_cast<btVector3*>(this) = rhs;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
class mat33 : public btMatrix3x3 {
|
class mat33 : public btMatrix3x3 {
|
||||||
public:
|
public:
|
||||||
mat33() : btMatrix3x3() {}
|
mat33() : btMatrix3x3() {}
|
||||||
mat33(const btMatrix3x3& btm) { *this = btm; }
|
mat33(const btMatrix3x3& btm) { *this = btm; }
|
||||||
idScalar& operator()(int i, int j) { return (*this)[i][j]; }
|
idScalar& operator()(int i, int j) { return (*this)[i][j]; }
|
||||||
const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
|
const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
|
||||||
const mat33& operator=(const btMatrix3x3& rhs) {
|
const mat33& operator=(const btMatrix3x3& rhs) {
|
||||||
*static_cast<btMatrix3x3*>(this) = rhs;
|
*static_cast<btMatrix3x3*>(this) = rhs;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
friend mat33 operator*(const idScalar& s, const mat33& a);
|
friend mat33 operator*(const idScalar& s, const mat33& a);
|
||||||
friend mat33 operator/(const mat33& a, const idScalar& s);
|
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 mat33& a, const idScalar& s) { return a * (1.0 / s); }
|
||||||
@ -48,64 +48,64 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
|
|||||||
|
|
||||||
class vecx : public btVectorX<idScalar> {
|
class vecx : public btVectorX<idScalar> {
|
||||||
public:
|
public:
|
||||||
vecx(int size) : btVectorX(size) {}
|
vecx(int size) : btVectorX(size) {}
|
||||||
const vecx& operator=(const btVectorX<idScalar>& rhs) {
|
const vecx& operator=(const btVectorX<idScalar>& rhs) {
|
||||||
*static_cast<btVectorX*>(this) = rhs;
|
*static_cast<btVectorX*>(this) = rhs;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
idScalar& operator()(int i) { return (*this)[i]; }
|
idScalar& operator()(int i) { return (*this)[i]; }
|
||||||
const idScalar& operator()(int i) const { 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 vecx& a, const idScalar& s);
|
||||||
friend vecx operator*(const idScalar& s, const vecx& a);
|
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 vecx& b);
|
friend vecx operator-(const vecx& a, const vecx& b);
|
||||||
friend vecx operator/(const vecx& a, const idScalar& s);
|
friend vecx operator/(const vecx& a, const idScalar& s);
|
||||||
};
|
};
|
||||||
|
|
||||||
inline vecx operator*(const vecx& a, const idScalar& s) {
|
inline vecx operator*(const vecx& a, const idScalar& s) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
for (int i = 0; i < result.size(); i++) {
|
for (int i = 0; i < result.size(); i++) {
|
||||||
result(i) = a(i) * s;
|
result(i) = a(i) * s;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
|
inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
|
||||||
inline vecx operator+(const vecx& a, const vecx& b) {
|
inline vecx operator+(const vecx& a, const vecx& b) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
// TODO: error handling for a.size() != b.size()??
|
// TODO: error handling for a.size() != b.size()??
|
||||||
if (a.size() != b.size()) {
|
if (a.size() != b.size()) {
|
||||||
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
||||||
abort();
|
abort();
|
||||||
}
|
}
|
||||||
for (int i = 0; i < a.size(); i++) {
|
for (int i = 0; i < a.size(); i++) {
|
||||||
result(i) = a(i) + b(i);
|
result(i) = a(i) + b(i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline vecx operator-(const vecx& a, const vecx& b) {
|
inline vecx operator-(const vecx& a, const vecx& b) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
// TODO: error handling for a.size() != b.size()??
|
// TODO: error handling for a.size() != b.size()??
|
||||||
if (a.size() != b.size()) {
|
if (a.size() != b.size()) {
|
||||||
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
||||||
abort();
|
abort();
|
||||||
}
|
}
|
||||||
for (int i = 0; i < a.size(); i++) {
|
for (int i = 0; i < a.size(); i++) {
|
||||||
result(i) = a(i) - b(i);
|
result(i) = a(i) - b(i);
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vecx operator/(const vecx& a, const idScalar& s) {
|
inline vecx operator/(const vecx& a, const idScalar& s) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
for (int i = 0; i < result.size(); i++) {
|
for (int i = 0; i < result.size(); i++) {
|
||||||
result(i) = a(i) / s;
|
result(i) = a(i) / s;
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -4,6 +4,8 @@
|
|||||||
|
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
|
|
||||||
|
#include "../IDConfig.hpp"
|
||||||
|
|
||||||
namespace btInverseDynamics {
|
namespace btInverseDynamics {
|
||||||
class vec3;
|
class vec3;
|
||||||
class vecx;
|
class vecx;
|
||||||
@ -15,312 +17,312 @@ class matxx;
|
|||||||
/// want from a "fully featured" linear math library.
|
/// want from a "fully featured" linear math library.
|
||||||
class vec3 {
|
class vec3 {
|
||||||
public:
|
public:
|
||||||
idScalar& operator()(int i) { return m_data[i]; }
|
idScalar& operator()(int i) { return m_data[i]; }
|
||||||
const idScalar& operator()(int i) const { return m_data[i]; }
|
const idScalar& operator()(int i) const { return m_data[i]; }
|
||||||
const int size() const { return 3; }
|
const int size() const { return 3; }
|
||||||
const vec3& operator=(const vec3& rhs);
|
const vec3& operator=(const vec3& rhs);
|
||||||
const vec3& operator+=(const vec3& b);
|
const vec3& operator+=(const vec3& b);
|
||||||
const vec3& operator-=(const vec3& b);
|
const vec3& operator-=(const vec3& b);
|
||||||
vec3 cross(const vec3& b) const;
|
vec3 cross(const vec3& b) const;
|
||||||
idScalar dot(const vec3& b) const;
|
idScalar dot(const vec3& b) const;
|
||||||
|
|
||||||
friend vec3 operator*(const mat33& a, const vec3& b);
|
friend vec3 operator*(const mat33& a, const vec3& b);
|
||||||
friend vec3 operator*(const vec3& a, const idScalar& s);
|
friend vec3 operator*(const vec3& a, const idScalar& s);
|
||||||
friend vec3 operator*(const idScalar& s, const vec3& a);
|
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 vec3& b);
|
friend vec3 operator-(const vec3& a, const vec3& b);
|
||||||
friend vec3 operator/(const vec3& a, const idScalar& s);
|
friend vec3 operator/(const vec3& a, const idScalar& s);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
idScalar m_data[3];
|
idScalar m_data[3];
|
||||||
};
|
};
|
||||||
|
|
||||||
class mat33 {
|
class mat33 {
|
||||||
public:
|
public:
|
||||||
idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
|
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 idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
|
||||||
const mat33& operator=(const mat33& rhs);
|
const mat33& operator=(const mat33& rhs);
|
||||||
mat33 transpose() const;
|
mat33 transpose() const;
|
||||||
const mat33& operator+=(const mat33& b);
|
const mat33& operator+=(const mat33& b);
|
||||||
const mat33& operator-=(const mat33& b);
|
const mat33& operator-=(const mat33& b);
|
||||||
|
|
||||||
friend mat33 operator*(const mat33& a, const mat33& b);
|
friend mat33 operator*(const mat33& a, const mat33& b);
|
||||||
friend vec3 operator*(const mat33& a, const vec3& b);
|
friend vec3 operator*(const mat33& a, const vec3& b);
|
||||||
friend mat33 operator*(const mat33& a, const idScalar& s);
|
friend mat33 operator*(const mat33& a, const idScalar& s);
|
||||||
friend mat33 operator*(const idScalar& s, const mat33& a);
|
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 mat33& b);
|
friend mat33 operator-(const mat33& a, const mat33& b);
|
||||||
friend mat33 operator/(const mat33& a, const idScalar& s);
|
friend mat33 operator/(const mat33& a, const idScalar& s);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// layout is [0,1,2;3,4,5;6,7,8]
|
// layout is [0,1,2;3,4,5;6,7,8]
|
||||||
idScalar m_data[9];
|
idScalar m_data[9];
|
||||||
};
|
};
|
||||||
|
|
||||||
class vecx {
|
class vecx {
|
||||||
public:
|
public:
|
||||||
vecx(int size) : m_size(size) {
|
vecx(int size) : m_size(size) {
|
||||||
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
|
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
|
||||||
}
|
}
|
||||||
~vecx() { idFree(m_data); }
|
~vecx() { idFree(m_data); }
|
||||||
const vecx& operator=(const vecx& rhs);
|
const vecx& operator=(const vecx& rhs);
|
||||||
idScalar& operator()(int i) { return m_data[i]; }
|
idScalar& operator()(int i) { return m_data[i]; }
|
||||||
const idScalar& operator()(int i) const { return m_data[i]; }
|
const idScalar& operator()(int i) const { return m_data[i]; }
|
||||||
const int& size() const { return m_size; }
|
const int& size() const { return m_size; }
|
||||||
|
|
||||||
friend vecx operator*(const vecx& a, const idScalar& s);
|
friend vecx operator*(const vecx& a, const idScalar& s);
|
||||||
friend vecx operator*(const idScalar& s, const vecx& a);
|
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 vecx& b);
|
friend vecx operator-(const vecx& a, const vecx& b);
|
||||||
friend vecx operator/(const vecx& a, const idScalar& s);
|
friend vecx operator/(const vecx& a, const idScalar& s);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int m_size;
|
int m_size;
|
||||||
idScalar* m_data;
|
idScalar* m_data;
|
||||||
};
|
};
|
||||||
|
|
||||||
class matxx {
|
class matxx {
|
||||||
public:
|
public:
|
||||||
matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
|
matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
|
||||||
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
|
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
|
||||||
}
|
}
|
||||||
~matxx() { idFree(m_data); }
|
~matxx() { idFree(m_data); }
|
||||||
const matxx& operator=(const matxx& rhs);
|
const matxx& operator=(const matxx& rhs);
|
||||||
idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; }
|
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 idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; }
|
||||||
const int& rows() const { return m_rows; }
|
const int& rows() const { return m_rows; }
|
||||||
const int& cols() const { return m_cols; }
|
const int& cols() const { return m_cols; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int m_rows;
|
int m_rows;
|
||||||
int m_cols;
|
int m_cols;
|
||||||
idScalar* m_data;
|
idScalar* m_data;
|
||||||
};
|
};
|
||||||
|
|
||||||
inline const vec3& vec3::operator=(const vec3& rhs) {
|
inline const vec3& vec3::operator=(const vec3& rhs) {
|
||||||
if (&rhs != this) {
|
if (&rhs != this) {
|
||||||
memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
|
memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline vec3 vec3::cross(const vec3& b) const {
|
inline vec3 vec3::cross(const vec3& b) const {
|
||||||
vec3 result;
|
vec3 result;
|
||||||
result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
|
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[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];
|
result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0];
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline idScalar vec3::dot(const vec3& b) const {
|
inline idScalar vec3::dot(const vec3& b) const {
|
||||||
return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
|
return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const mat33& mat33::operator=(const mat33& rhs) {
|
inline const mat33& mat33::operator=(const mat33& rhs) {
|
||||||
if (&rhs != this) {
|
if (&rhs != this) {
|
||||||
memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
|
memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
inline mat33 mat33::transpose() const {
|
inline mat33 mat33::transpose() const {
|
||||||
mat33 result;
|
mat33 result;
|
||||||
result.m_data[0] = m_data[0];
|
result.m_data[0] = m_data[0];
|
||||||
result.m_data[1] = m_data[3];
|
result.m_data[1] = m_data[3];
|
||||||
result.m_data[2] = m_data[6];
|
result.m_data[2] = m_data[6];
|
||||||
result.m_data[3] = m_data[1];
|
result.m_data[3] = m_data[1];
|
||||||
result.m_data[4] = m_data[4];
|
result.m_data[4] = m_data[4];
|
||||||
result.m_data[5] = m_data[7];
|
result.m_data[5] = m_data[7];
|
||||||
result.m_data[6] = m_data[2];
|
result.m_data[6] = m_data[2];
|
||||||
result.m_data[7] = m_data[5];
|
result.m_data[7] = m_data[5];
|
||||||
result.m_data[8] = m_data[8];
|
result.m_data[8] = m_data[8];
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline mat33 operator*(const mat33& a, const mat33& b) {
|
inline mat33 operator*(const mat33& a, const mat33& b) {
|
||||||
mat33 result;
|
mat33 result;
|
||||||
result.m_data[0] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8];
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const mat33& mat33::operator+=(const mat33& b) {
|
inline const mat33& mat33::operator+=(const mat33& b) {
|
||||||
for (int i = 0; i < 9; i++) {
|
for (int i = 0; i < 9; i++) {
|
||||||
m_data[i] += b.m_data[i];
|
m_data[i] += b.m_data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const mat33& mat33::operator-=(const mat33& b) {
|
inline const mat33& mat33::operator-=(const mat33& b) {
|
||||||
for (int i = 0; i < 9; i++) {
|
for (int i = 0; i < 9; i++) {
|
||||||
m_data[i] -= b.m_data[i];
|
m_data[i] -= b.m_data[i];
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline vec3 operator*(const mat33& a, const vec3& b) {
|
inline vec3 operator*(const mat33& a, const vec3& b) {
|
||||||
vec3 result;
|
vec3 result;
|
||||||
|
|
||||||
result.m_data[0] =
|
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];
|
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] =
|
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];
|
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] =
|
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];
|
a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2];
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const vec3& vec3::operator+=(const vec3& b) {
|
inline const vec3& vec3::operator+=(const vec3& b) {
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
m_data[i] += b.m_data[i];
|
m_data[i] += b.m_data[i];
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const vec3& vec3::operator-=(const vec3& b) {
|
inline const vec3& vec3::operator-=(const vec3& b) {
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
m_data[i] -= b.m_data[i];
|
m_data[i] -= b.m_data[i];
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline mat33 operator*(const mat33& a, const idScalar& s) {
|
inline mat33 operator*(const mat33& a, const idScalar& s) {
|
||||||
mat33 result;
|
mat33 result;
|
||||||
for (int i = 0; i < 9; i++) {
|
for (int i = 0; i < 9; i++) {
|
||||||
result.m_data[i] = a.m_data[i] * s;
|
result.m_data[i] = a.m_data[i] * s;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
|
inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
|
||||||
|
|
||||||
inline vec3 operator*(const vec3& a, const idScalar& s) {
|
inline vec3 operator*(const vec3& a, const idScalar& s) {
|
||||||
vec3 result;
|
vec3 result;
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
result.m_data[i] = a.m_data[i] * s;
|
result.m_data[i] = a.m_data[i] * s;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
|
inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
|
||||||
|
|
||||||
inline mat33 operator+(const mat33& a, const mat33& b) {
|
inline mat33 operator+(const mat33& a, const mat33& b) {
|
||||||
mat33 result;
|
mat33 result;
|
||||||
for (int i = 0; i < 9; i++) {
|
for (int i = 0; i < 9; i++) {
|
||||||
result.m_data[i] = a.m_data[i] + b.m_data[i];
|
result.m_data[i] = a.m_data[i] + b.m_data[i];
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vec3 operator+(const vec3& a, const vec3& b) {
|
inline vec3 operator+(const vec3& a, const vec3& b) {
|
||||||
vec3 result;
|
vec3 result;
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
result.m_data[i] = a.m_data[i] + b.m_data[i];
|
result.m_data[i] = a.m_data[i] + b.m_data[i];
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline mat33 operator-(const mat33& a, const mat33& b) {
|
inline mat33 operator-(const mat33& a, const mat33& b) {
|
||||||
mat33 result;
|
mat33 result;
|
||||||
for (int i = 0; i < 9; i++) {
|
for (int i = 0; i < 9; i++) {
|
||||||
result.m_data[i] = a.m_data[i] - b.m_data[i];
|
result.m_data[i] = a.m_data[i] - b.m_data[i];
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vec3 operator-(const vec3& a, const vec3& b) {
|
inline vec3 operator-(const vec3& a, const vec3& b) {
|
||||||
vec3 result;
|
vec3 result;
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
result.m_data[i] = a.m_data[i] - b.m_data[i];
|
result.m_data[i] = a.m_data[i] - b.m_data[i];
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline mat33 operator/(const mat33& a, const idScalar& s) {
|
inline mat33 operator/(const mat33& a, const idScalar& s) {
|
||||||
mat33 result;
|
mat33 result;
|
||||||
for (int i = 0; i < 9; i++) {
|
for (int i = 0; i < 9; i++) {
|
||||||
result.m_data[i] = a.m_data[i] / s;
|
result.m_data[i] = a.m_data[i] / s;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline vec3 operator/(const vec3& a, const idScalar& s) {
|
inline vec3 operator/(const vec3& a, const idScalar& s) {
|
||||||
vec3 result;
|
vec3 result;
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
result.m_data[i] = a.m_data[i] / s;
|
result.m_data[i] = a.m_data[i] / s;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const vecx& vecx::operator=(const vecx& rhs) {
|
inline const vecx& vecx::operator=(const vecx& rhs) {
|
||||||
if (size() != rhs.size()) {
|
if (size() != rhs.size()) {
|
||||||
error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
|
error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
|
||||||
abort();
|
abort();
|
||||||
}
|
}
|
||||||
if (&rhs != this) {
|
if (&rhs != this) {
|
||||||
memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
|
memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
inline vecx operator*(const vecx& a, const idScalar& s) {
|
inline vecx operator*(const vecx& a, const idScalar& s) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
for (int i = 0; i < result.size(); i++) {
|
for (int i = 0; i < result.size(); i++) {
|
||||||
result.m_data[i] = a.m_data[i] * s;
|
result.m_data[i] = a.m_data[i] * s;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
|
inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
|
||||||
inline vecx operator+(const vecx& a, const vecx& b) {
|
inline vecx operator+(const vecx& a, const vecx& b) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
// TODO: error handling for a.size() != b.size()??
|
// TODO: error handling for a.size() != b.size()??
|
||||||
if (a.size() != b.size()) {
|
if (a.size() != b.size()) {
|
||||||
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
||||||
abort();
|
abort();
|
||||||
}
|
}
|
||||||
for (int i = 0; i < a.size(); i++) {
|
for (int i = 0; i < a.size(); i++) {
|
||||||
result.m_data[i] = a.m_data[i] + b.m_data[i];
|
result.m_data[i] = a.m_data[i] + b.m_data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vecx operator-(const vecx& a, const vecx& b) {
|
inline vecx operator-(const vecx& a, const vecx& b) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
// TODO: error handling for a.size() != b.size()??
|
// TODO: error handling for a.size() != b.size()??
|
||||||
if (a.size() != b.size()) {
|
if (a.size() != b.size()) {
|
||||||
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
|
||||||
abort();
|
abort();
|
||||||
}
|
}
|
||||||
for (int i = 0; i < a.size(); i++) {
|
for (int i = 0; i < a.size(); i++) {
|
||||||
result.m_data[i] = a.m_data[i] - b.m_data[i];
|
result.m_data[i] = a.m_data[i] - b.m_data[i];
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
inline vecx operator/(const vecx& a, const idScalar& s) {
|
inline vecx operator/(const vecx& a, const idScalar& s) {
|
||||||
vecx result(a.size());
|
vecx result(a.size());
|
||||||
for (int i = 0; i < result.size(); i++) {
|
for (int i = 0; i < result.size(); i++) {
|
||||||
result.m_data[i] = a.m_data[i] / s;
|
result.m_data[i] = a.m_data[i] / s;
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
File diff suppressed because it is too large
Load Diff
@ -14,223 +14,223 @@ namespace btInverseDynamics {
|
|||||||
/// all vectors and matrices are in body-fixed frame, if not indicated otherwise.
|
/// 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.
|
/// The body-fixed frame is located in the joint connecting the body to its parent.
|
||||||
struct RigidBody {
|
struct RigidBody {
|
||||||
ID_DECLARE_ALIGNED_ALLOCATOR();
|
ID_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
// 1 Inertial properties
|
// 1 Inertial properties
|
||||||
/// Mass
|
/// Mass
|
||||||
idScalar m_mass;
|
idScalar m_mass;
|
||||||
/// Mass times center of gravity in body-fixed frame
|
/// Mass times center of gravity in body-fixed frame
|
||||||
vec3 m_body_mass_com;
|
vec3 m_body_mass_com;
|
||||||
/// Moment of inertia w.r.t. body-fixed frame
|
/// Moment of inertia w.r.t. body-fixed frame
|
||||||
mat33 m_body_I_body;
|
mat33 m_body_I_body;
|
||||||
|
|
||||||
// 2 dynamic properties
|
// 2 dynamic properties
|
||||||
/// Left-hand side of the body equation of motion, translational part
|
/// Left-hand side of the body equation of motion, translational part
|
||||||
vec3 m_eom_lhs_translational;
|
vec3 m_eom_lhs_translational;
|
||||||
/// Left-hand side of the body equation of motion, rotational part
|
/// Left-hand side of the body equation of motion, rotational part
|
||||||
vec3 m_eom_lhs_rotational;
|
vec3 m_eom_lhs_rotational;
|
||||||
/// Force acting at the joint when the body is cut from its parent;
|
/// Force acting at the joint when the body is cut from its parent;
|
||||||
/// includes impressed joint force in J_JT direction,
|
/// includes impressed joint force in J_JT direction,
|
||||||
/// as well as constraint force,
|
/// as well as constraint force,
|
||||||
/// in body-fixed frame
|
/// in body-fixed frame
|
||||||
vec3 m_force_at_joint;
|
vec3 m_force_at_joint;
|
||||||
/// Moment acting at the joint when the body is cut from its parent;
|
/// Moment acting at the joint when the body is cut from its parent;
|
||||||
/// includes impressed joint moment in J_JR direction, and constraint moment
|
/// includes impressed joint moment in J_JR direction, and constraint moment
|
||||||
/// in body-fixed frame
|
/// in body-fixed frame
|
||||||
vec3 m_moment_at_joint;
|
vec3 m_moment_at_joint;
|
||||||
/// external (user provided) force acting at the body-fixed frame's origin, written in that
|
/// external (user provided) force acting at the body-fixed frame's origin, written in that
|
||||||
/// frame
|
/// frame
|
||||||
vec3 m_body_force_user;
|
vec3 m_body_force_user;
|
||||||
/// external (user provided) moment acting at the body-fixed frame's origin, written in that
|
/// external (user provided) moment acting at the body-fixed frame's origin, written in that
|
||||||
/// frame
|
/// frame
|
||||||
vec3 m_body_moment_user;
|
vec3 m_body_moment_user;
|
||||||
// 3 absolute kinematic properties
|
// 3 absolute kinematic properties
|
||||||
/// Position of body-fixed frame relative to world frame
|
/// Position of body-fixed frame relative to world frame
|
||||||
/// this is currently only for debugging purposes
|
/// this is currently only for debugging purposes
|
||||||
vec3 m_body_pos;
|
vec3 m_body_pos;
|
||||||
/// Absolute velocity of body-fixed frame
|
/// Absolute velocity of body-fixed frame
|
||||||
vec3 m_body_vel;
|
vec3 m_body_vel;
|
||||||
/// Absolute acceleration of body-fixed frame
|
/// Absolute acceleration of body-fixed frame
|
||||||
/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
|
/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
|
||||||
/// acceleration!
|
/// acceleration!
|
||||||
vec3 m_body_acc;
|
vec3 m_body_acc;
|
||||||
/// Absolute angular velocity
|
/// Absolute angular velocity
|
||||||
vec3 m_body_ang_vel;
|
vec3 m_body_ang_vel;
|
||||||
/// Absolute angular acceleration
|
/// Absolute angular acceleration
|
||||||
/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
|
/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
|
||||||
/// acceleration!
|
/// acceleration!
|
||||||
vec3 m_body_ang_acc;
|
vec3 m_body_ang_acc;
|
||||||
|
|
||||||
// 4 relative kinematic properties.
|
// 4 relative kinematic properties.
|
||||||
// these are in the parent body frame
|
// these are in the parent body frame
|
||||||
/// Transform from world to body-fixed frame;
|
/// Transform from world to body-fixed frame;
|
||||||
/// this is currently only for debugging purposes
|
/// this is currently only for debugging purposes
|
||||||
mat33 m_body_T_world;
|
mat33 m_body_T_world;
|
||||||
/// Transform from parent to body-fixed frame
|
/// Transform from parent to body-fixed frame
|
||||||
mat33 m_body_T_parent;
|
mat33 m_body_T_parent;
|
||||||
/// Vector from parent to child frame in parent frame
|
/// Vector from parent to child frame in parent frame
|
||||||
vec3 m_parent_pos_parent_body;
|
vec3 m_parent_pos_parent_body;
|
||||||
/// Relative angular velocity
|
/// Relative angular velocity
|
||||||
vec3 m_body_ang_vel_rel;
|
vec3 m_body_ang_vel_rel;
|
||||||
/// Relative linear velocity
|
/// Relative linear velocity
|
||||||
vec3 m_parent_vel_rel;
|
vec3 m_parent_vel_rel;
|
||||||
/// Relative angular acceleration
|
/// Relative angular acceleration
|
||||||
vec3 m_body_ang_acc_rel;
|
vec3 m_body_ang_acc_rel;
|
||||||
/// Relative linear acceleration
|
/// Relative linear acceleration
|
||||||
vec3 m_parent_acc_rel;
|
vec3 m_parent_acc_rel;
|
||||||
|
|
||||||
// 5 Data describing the joint type and geometry
|
// 5 Data describing the joint type and geometry
|
||||||
/// Type of joint
|
/// Type of joint
|
||||||
JointType m_joint_type;
|
JointType m_joint_type;
|
||||||
/// Position of joint frame (body-fixed frame at q=0) relative to the parent frame
|
/// Position of joint frame (body-fixed frame at q=0) relative to the parent frame
|
||||||
/// Components are in body-fixed frame of the parent
|
/// Components are in body-fixed frame of the parent
|
||||||
vec3 m_parent_pos_parent_body_ref;
|
vec3 m_parent_pos_parent_body_ref;
|
||||||
/// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame
|
/// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame
|
||||||
mat33 m_body_T_parent_ref;
|
mat33 m_body_T_parent_ref;
|
||||||
/// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute
|
/// 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.
|
/// 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.
|
/// 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!)
|
/// (NOTE: dimensions will have to be dynamic for additional joint types!)
|
||||||
vec3 m_Jac_JR;
|
vec3 m_Jac_JR;
|
||||||
/// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute
|
/// 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.
|
/// 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.
|
/// 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!)
|
/// (NOTE: dimensions might have to be dynamic for additional joint types!)
|
||||||
vec3 m_Jac_JT;
|
vec3 m_Jac_JT;
|
||||||
/// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*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;
|
vec3 m_parent_Jac_JT;
|
||||||
/// Start of index range for the position degree(s) of freedom describing this body's motion
|
/// Start of index range for the position degree(s) of freedom describing this body's motion
|
||||||
/// relative to
|
/// relative to
|
||||||
/// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates.
|
/// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates.
|
||||||
int m_q_index;
|
int m_q_index;
|
||||||
|
|
||||||
// 6 Scratch data for mass matrix computation using "composite rigid body algorithm"
|
// 6 Scratch data for mass matrix computation using "composite rigid body algorithm"
|
||||||
/// mass of the subtree rooted in this body
|
/// mass of the subtree rooted in this body
|
||||||
idScalar m_subtree_mass;
|
idScalar m_subtree_mass;
|
||||||
/// center of mass * mass for subtree rooted in this body, in body-fixed frame
|
/// center of mass * mass for subtree rooted in this body, in body-fixed frame
|
||||||
vec3 m_body_subtree_mass_com;
|
vec3 m_body_subtree_mass_com;
|
||||||
/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
|
/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
|
||||||
mat33 m_body_subtree_I_body;
|
mat33 m_body_subtree_I_body;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// The MBS implements a tree structured multibody system
|
/// The MBS implements a tree structured multibody system
|
||||||
class MultiBodyTree::MultiBodyImpl {
|
class MultiBodyTree::MultiBodyImpl {
|
||||||
friend class MultiBodyTree;
|
friend class MultiBodyTree;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ID_DECLARE_ALIGNED_ALLOCATOR();
|
ID_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
|
||||||
/// constructor
|
/// constructor
|
||||||
/// @param num_bodies the number of bodies in the system
|
/// @param num_bodies the number of bodies in the system
|
||||||
/// @param num_dofs number of degrees of freedom in the system
|
/// @param num_dofs number of degrees of freedom in the system
|
||||||
MultiBodyImpl(int num_bodies_, int num_dofs_);
|
MultiBodyImpl(int num_bodies_, int num_dofs_);
|
||||||
|
|
||||||
/// \copydoc MultiBodyTree::calculateInverseDynamics
|
/// \copydoc MultiBodyTree::calculateInverseDynamics
|
||||||
int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
|
int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
|
||||||
vecx* joint_forces);
|
vecx* joint_forces);
|
||||||
///\copydoc MultiBodyTree::calculateMassMatrix
|
///\copydoc MultiBodyTree::calculateMassMatrix
|
||||||
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
|
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
|
||||||
const bool initialize_matrix, const bool set_lower_triangular_matrix,
|
const bool initialize_matrix, const bool set_lower_triangular_matrix,
|
||||||
matxx* mass_matrix);
|
matxx* mass_matrix);
|
||||||
/// generate additional index sets from the parent_index array
|
/// generate additional index sets from the parent_index array
|
||||||
/// @return -1 on error, 0 on success
|
/// @return -1 on error, 0 on success
|
||||||
int generateIndexSets();
|
int generateIndexSets();
|
||||||
/// set gravity acceleration in world frame
|
/// set gravity acceleration in world frame
|
||||||
/// @param gravity gravity vector in the world frame
|
/// @param gravity gravity vector in the world frame
|
||||||
/// @return 0 on success, -1 on error
|
/// @return 0 on success, -1 on error
|
||||||
int setGravityInWorldFrame(const vec3& gravity);
|
int setGravityInWorldFrame(const vec3& gravity);
|
||||||
/// pretty print tree
|
/// pretty print tree
|
||||||
void printTree();
|
void printTree();
|
||||||
/// print tree data
|
/// print tree data
|
||||||
void printTreeData();
|
void printTreeData();
|
||||||
/// initialize fixed data
|
/// initialize fixed data
|
||||||
void calculateStaticData();
|
void calculateStaticData();
|
||||||
/// \copydoc MultiBodyTree::getBodyFrame
|
/// \copydoc MultiBodyTree::getBodyFrame
|
||||||
int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
|
int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
|
||||||
/// \copydoc MultiBodyTree::getParentIndex
|
/// \copydoc MultiBodyTree::getParentIndex
|
||||||
int getParentIndex(const int body_index, int* m_parent_index);
|
int getParentIndex(const int body_index, int* m_parent_index);
|
||||||
/// \copydoc MultiBodyTree::getJointType
|
/// \copydoc MultiBodyTree::getJointType
|
||||||
int getJointType(const int body_index, JointType* joint_type) const;
|
int getJointType(const int body_index, JointType* joint_type) const;
|
||||||
/// \copydoc MultiBodyTree::getJointTypeStr
|
/// \copydoc MultiBodyTree::getJointTypeStr
|
||||||
int getJointTypeStr(const int body_index, const char** joint_type) const;
|
int getJointTypeStr(const int body_index, const char** joint_type) const;
|
||||||
/// \copydoc MultiBodyTree:getDoFOffset
|
/// \copydoc MultiBodyTree:getDoFOffset
|
||||||
int getDoFOffset(const int body_index, int* q_index) const;
|
int getDoFOffset(const int body_index, int* q_index) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyOrigin
|
/// \copydoc MultiBodyTree::getBodyOrigin
|
||||||
int getBodyOrigin(const int body_index, vec3* world_origin) const;
|
int getBodyOrigin(const int body_index, vec3* world_origin) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyCoM
|
/// \copydoc MultiBodyTree::getBodyCoM
|
||||||
int getBodyCoM(const int body_index, vec3* world_com) const;
|
int getBodyCoM(const int body_index, vec3* world_com) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyTransform
|
/// \copydoc MultiBodyTree::getBodyTransform
|
||||||
int getBodyTransform(const int body_index, mat33* world_T_body) const;
|
int getBodyTransform(const int body_index, mat33* world_T_body) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyAngularVelocity
|
/// \copydoc MultiBodyTree::getBodyAngularVelocity
|
||||||
int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
|
int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyLinearVelocity
|
/// \copydoc MultiBodyTree::getBodyLinearVelocity
|
||||||
int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
|
int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
|
/// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
|
||||||
int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
|
int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyAngularAcceleration
|
/// \copydoc MultiBodyTree::getBodyAngularAcceleration
|
||||||
int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
|
int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
|
||||||
/// \copydoc MultiBodyTree::getBodyLinearAcceleration
|
/// \copydoc MultiBodyTree::getBodyLinearAcceleration
|
||||||
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
|
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
|
||||||
/// \copydoc MultiBodyTree::getUserInt
|
/// \copydoc MultiBodyTree::getUserInt
|
||||||
int getUserInt(const int body_index, int* user_int) const;
|
int getUserInt(const int body_index, int* user_int) const;
|
||||||
/// \copydoc MultiBodyTree::getUserPtr
|
/// \copydoc MultiBodyTree::getUserPtr
|
||||||
int getUserPtr(const int body_index, void** user_ptr) const;
|
int getUserPtr(const int body_index, void** user_ptr) const;
|
||||||
/// \copydoc MultiBodyTree::setUserInt
|
/// \copydoc MultiBodyTree::setUserInt
|
||||||
int setUserInt(const int body_index, const int user_int);
|
int setUserInt(const int body_index, const int user_int);
|
||||||
/// \copydoc MultiBodyTree::setUserPtr
|
/// \copydoc MultiBodyTree::setUserPtr
|
||||||
int setUserPtr(const int body_index, void* const user_ptr);
|
int setUserPtr(const int body_index, void* const user_ptr);
|
||||||
///\copydoc MultiBodytTree::setBodyMass
|
///\copydoc MultiBodytTree::setBodyMass
|
||||||
int setBodyMass(const int body_index, const idScalar mass);
|
int setBodyMass(const int body_index, const idScalar mass);
|
||||||
///\copydoc MultiBodytTree::setBodyFirstMassMoment
|
///\copydoc MultiBodytTree::setBodyFirstMassMoment
|
||||||
int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
|
int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
|
||||||
///\copydoc MultiBodytTree::setBodySecondMassMoment
|
///\copydoc MultiBodytTree::setBodySecondMassMoment
|
||||||
int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
|
int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
|
||||||
///\copydoc MultiBodytTree::getBodyMass
|
///\copydoc MultiBodytTree::getBodyMass
|
||||||
int getBodyMass(const int body_index, idScalar* mass) const;
|
int getBodyMass(const int body_index, idScalar* mass) const;
|
||||||
///\copydoc MultiBodytTree::getBodyFirstMassMoment
|
///\copydoc MultiBodytTree::getBodyFirstMassMoment
|
||||||
int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
|
int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
|
||||||
///\copydoc MultiBodytTree::getBodySecondMassMoment
|
///\copydoc MultiBodytTree::getBodySecondMassMoment
|
||||||
int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
|
int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
|
||||||
/// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
|
/// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
|
||||||
void clearAllUserForcesAndMoments();
|
void clearAllUserForcesAndMoments();
|
||||||
/// \copydoc MultiBodyTree::addUserForce
|
/// \copydoc MultiBodyTree::addUserForce
|
||||||
int addUserForce(const int body_index, const vec3& body_force);
|
int addUserForce(const int body_index, const vec3& body_force);
|
||||||
/// \copydoc MultiBodyTree::addUserMoment
|
/// \copydoc MultiBodyTree::addUserMoment
|
||||||
int addUserMoment(const int body_index, const vec3& body_moment);
|
int addUserMoment(const int body_index, const vec3& body_moment);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// debug function. print tree structure to stdout
|
// debug function. print tree structure to stdout
|
||||||
void printTree(int index, int indentation);
|
void printTree(int index, int indentation);
|
||||||
// get string representation of JointType (for debugging)
|
// get string representation of JointType (for debugging)
|
||||||
const char* jointTypeToString(const JointType& type) const;
|
const char* jointTypeToString(const JointType& type) const;
|
||||||
// get number of degrees of freedom from joint type
|
// get number of degrees of freedom from joint type
|
||||||
int bodyNumDoFs(const JointType& type) const;
|
int bodyNumDoFs(const JointType& type) const;
|
||||||
// number of bodies in the system
|
// number of bodies in the system
|
||||||
int m_num_bodies;
|
int m_num_bodies;
|
||||||
// number of degrees of freedom
|
// number of degrees of freedom
|
||||||
int m_num_dofs;
|
int m_num_dofs;
|
||||||
// Gravitational acceleration (in world frame)
|
// Gravitational acceleration (in world frame)
|
||||||
vec3 m_world_gravity;
|
vec3 m_world_gravity;
|
||||||
// vector of bodies in the system
|
// vector of bodies in the system
|
||||||
// body 0 is used as an environment body and is allways fixed.
|
// 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
|
// The bodies are ordered such that a parent body always has an index
|
||||||
// smaller than its child.
|
// smaller than its child.
|
||||||
idArray<RigidBody>::type m_body_list;
|
idArray<RigidBody>::type m_body_list;
|
||||||
// Parent_index[i] is the index for i's parent body in body_list.
|
// Parent_index[i] is the index for i's parent body in body_list.
|
||||||
// This fully describes the tree.
|
// This fully describes the tree.
|
||||||
idArray<int>::type m_parent_index;
|
idArray<int>::type m_parent_index;
|
||||||
// child_indices[i] contains a vector of indices of
|
// child_indices[i] contains a vector of indices of
|
||||||
// all children of the i-th body
|
// all children of the i-th body
|
||||||
idArray<idArray<int>::type>::type m_child_indices;
|
idArray<idArray<int>::type>::type m_child_indices;
|
||||||
// Indices of rotary joints
|
// Indices of rotary joints
|
||||||
idArray<int>::type m_body_revolute_list;
|
idArray<int>::type m_body_revolute_list;
|
||||||
// Indices of prismatic joints
|
// Indices of prismatic joints
|
||||||
idArray<int>::type m_body_prismatic_list;
|
idArray<int>::type m_body_prismatic_list;
|
||||||
// Indices of floating joints
|
// Indices of floating joints
|
||||||
idArray<int>::type m_body_floating_list;
|
idArray<int>::type m_body_floating_list;
|
||||||
// a user-provided integer
|
// a user-provided integer
|
||||||
idArray<int>::type m_user_int;
|
idArray<int>::type m_user_int;
|
||||||
// a user-provided pointer
|
// a user-provided pointer
|
||||||
idArray<void*>::type m_user_ptr;
|
idArray<void*>::type m_user_ptr;
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -3,111 +3,111 @@
|
|||||||
namespace btInverseDynamics {
|
namespace btInverseDynamics {
|
||||||
|
|
||||||
MultiBodyTree::InitCache::InitCache() {
|
MultiBodyTree::InitCache::InitCache() {
|
||||||
m_inertias.resize(0);
|
m_inertias.resize(0);
|
||||||
m_joints.resize(0);
|
m_joints.resize(0);
|
||||||
m_num_dofs = 0;
|
m_num_dofs = 0;
|
||||||
m_root_index=-1;
|
m_root_index=-1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
|
int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
|
||||||
const JointType joint_type,
|
const JointType joint_type,
|
||||||
const vec3& parent_r_parent_body_ref,
|
const vec3& parent_r_parent_body_ref,
|
||||||
const mat33& body_T_parent_ref,
|
const mat33& body_T_parent_ref,
|
||||||
const vec3& body_axis_of_motion, const idScalar mass,
|
const vec3& body_axis_of_motion, const idScalar mass,
|
||||||
const vec3& body_r_body_com, const mat33& body_I_body,
|
const vec3& body_r_body_com, const mat33& body_I_body,
|
||||||
const int user_int, void* user_ptr) {
|
const int user_int, void* user_ptr) {
|
||||||
switch (joint_type) {
|
switch (joint_type) {
|
||||||
case REVOLUTE:
|
case REVOLUTE:
|
||||||
case PRISMATIC:
|
case PRISMATIC:
|
||||||
m_num_dofs += 1;
|
m_num_dofs += 1;
|
||||||
break;
|
break;
|
||||||
case FIXED:
|
case FIXED:
|
||||||
// does not add a degree of freedom
|
// does not add a degree of freedom
|
||||||
// m_num_dofs+=0;
|
// m_num_dofs+=0;
|
||||||
break;
|
break;
|
||||||
case FLOATING:
|
case FLOATING:
|
||||||
m_num_dofs += 6;
|
m_num_dofs += 6;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
error_message("unknown joint type %d\n", joint_type);
|
error_message("unknown joint type %d\n", joint_type);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(-1 == parent_index) {
|
if(-1 == parent_index) {
|
||||||
if(m_root_index>=0) {
|
if(m_root_index>=0) {
|
||||||
error_message("trying to add body %d as root, but already added %d as root body\n",
|
error_message("trying to add body %d as root, but already added %d as root body\n",
|
||||||
body_index, m_root_index);
|
body_index, m_root_index);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
m_root_index=body_index;
|
m_root_index=body_index;
|
||||||
}
|
}
|
||||||
|
|
||||||
JointData joint;
|
JointData joint;
|
||||||
joint.m_child = body_index;
|
joint.m_child = body_index;
|
||||||
joint.m_parent = parent_index;
|
joint.m_parent = parent_index;
|
||||||
joint.m_type = joint_type;
|
joint.m_type = joint_type;
|
||||||
joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref;
|
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_T_parent_ref = body_T_parent_ref;
|
||||||
joint.m_child_axis_of_motion = body_axis_of_motion;
|
joint.m_child_axis_of_motion = body_axis_of_motion;
|
||||||
|
|
||||||
InertiaData body;
|
InertiaData body;
|
||||||
body.m_mass = mass;
|
body.m_mass = mass;
|
||||||
body.m_body_pos_body_com = body_r_body_com;
|
body.m_body_pos_body_com = body_r_body_com;
|
||||||
body.m_body_I_body = body_I_body;
|
body.m_body_I_body = body_I_body;
|
||||||
|
|
||||||
m_inertias.push_back(body);
|
m_inertias.push_back(body);
|
||||||
m_joints.push_back(joint);
|
m_joints.push_back(joint);
|
||||||
m_user_int.push_back(user_int);
|
m_user_int.push_back(user_int);
|
||||||
m_user_ptr.push_back(user_ptr);
|
m_user_ptr.push_back(user_ptr);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
|
int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
|
||||||
if (index < 0 || index > static_cast<int>(m_inertias.size())) {
|
if (index < 0 || index > static_cast<int>(m_inertias.size())) {
|
||||||
error_message("index out of range\n");
|
error_message("index out of range\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
*inertia = m_inertias[index];
|
*inertia = m_inertias[index];
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
|
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
|
||||||
if (index < 0 || index > static_cast<int>(m_user_int.size())) {
|
if (index < 0 || index > static_cast<int>(m_user_int.size())) {
|
||||||
error_message("index out of range\n");
|
error_message("index out of range\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
*user_int = m_user_int[index];
|
*user_int = m_user_int[index];
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
|
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
|
||||||
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
|
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
|
||||||
error_message("index out of range\n");
|
error_message("index out of range\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
*user_ptr = m_user_ptr[index];
|
*user_ptr = m_user_ptr[index];
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
|
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
|
||||||
if (index < 0 || index > static_cast<int>(m_joints.size())) {
|
if (index < 0 || index > static_cast<int>(m_joints.size())) {
|
||||||
error_message("index out of range\n");
|
error_message("index out of range\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
*joint = m_joints[index];
|
*joint = m_joints[index];
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int MultiBodyTree::InitCache::buildIndexSets() {
|
int MultiBodyTree::InitCache::buildIndexSets() {
|
||||||
// NOTE: This function assumes that proper indices were provided
|
// NOTE: This function assumes that proper indices were provided
|
||||||
// User2InternalIndex from utils can be used to facilitate this.
|
// User2InternalIndex from utils can be used to facilitate this.
|
||||||
|
|
||||||
m_parent_index.resize(numBodies());
|
m_parent_index.resize(numBodies());
|
||||||
for (idArrayIdx j = 0; j < m_joints.size(); j++) {
|
for (idArrayIdx j = 0; j < m_joints.size(); j++) {
|
||||||
const JointData& joint = m_joints[j];
|
const JointData& joint = m_joints[j];
|
||||||
m_parent_index[joint.m_child] = joint.m_parent;
|
m_parent_index[joint.m_child] = joint.m_parent;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -8,102 +8,102 @@
|
|||||||
namespace btInverseDynamics {
|
namespace btInverseDynamics {
|
||||||
/// Mass properties of a rigid body
|
/// Mass properties of a rigid body
|
||||||
struct InertiaData {
|
struct InertiaData {
|
||||||
ID_DECLARE_ALIGNED_ALLOCATOR();
|
ID_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
|
||||||
/// mass
|
/// mass
|
||||||
idScalar m_mass;
|
idScalar m_mass;
|
||||||
/// vector from body-fixed frame to center of mass,
|
/// vector from body-fixed frame to center of mass,
|
||||||
/// in body-fixed frame, multiplied by the mass
|
/// in body-fixed frame, multiplied by the mass
|
||||||
vec3 m_body_pos_body_com;
|
vec3 m_body_pos_body_com;
|
||||||
/// moment of inertia w.r.t. the origin of the body-fixed
|
/// moment of inertia w.r.t. the origin of the body-fixed
|
||||||
/// frame, represented in that frame
|
/// frame, represented in that frame
|
||||||
mat33 m_body_I_body;
|
mat33 m_body_I_body;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Joint properties
|
/// Joint properties
|
||||||
struct JointData {
|
struct JointData {
|
||||||
ID_DECLARE_ALIGNED_ALLOCATOR();
|
ID_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
|
||||||
/// type of joint
|
/// type of joint
|
||||||
JointType m_type;
|
JointType m_type;
|
||||||
/// index of parent body
|
/// index of parent body
|
||||||
int m_parent;
|
int m_parent;
|
||||||
/// index of child body
|
/// index of child body
|
||||||
int m_child;
|
int m_child;
|
||||||
/// vector from parent's body-fixed frame to child's body-fixed
|
/// vector from parent's body-fixed frame to child's body-fixed
|
||||||
/// frame for q=0, written in the parent's body fixed frame
|
/// frame for q=0, written in the parent's body fixed frame
|
||||||
vec3 m_parent_pos_parent_child_ref;
|
vec3 m_parent_pos_parent_child_ref;
|
||||||
/// Transform matrix converting vectors written in the parent's frame
|
/// Transform matrix converting vectors written in the parent's frame
|
||||||
/// into vectors written in the child's frame for q=0
|
/// into vectors written in the child's frame for q=0
|
||||||
/// ie, child_vector = child_T_parent_ref * parent_vector;
|
/// ie, child_vector = child_T_parent_ref * parent_vector;
|
||||||
mat33 m_child_T_parent_ref;
|
mat33 m_child_T_parent_ref;
|
||||||
/// Axis of motion for 1 degree-of-freedom joints,
|
/// Axis of motion for 1 degree-of-freedom joints,
|
||||||
/// written in the child's frame
|
/// written in the child's frame
|
||||||
/// For revolute joints, the q-value is positive for a positive
|
/// For revolute joints, the q-value is positive for a positive
|
||||||
/// rotation about this axis.
|
/// rotation about this axis.
|
||||||
/// For prismatic joints, the q-value is positive for a positive
|
/// For prismatic joints, the q-value is positive for a positive
|
||||||
/// translation is this direction.
|
/// translation is this direction.
|
||||||
vec3 m_child_axis_of_motion;
|
vec3 m_child_axis_of_motion;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Data structure to store data passed by the user.
|
/// Data structure to store data passed by the user.
|
||||||
/// This is used in MultiBodyTree::finalize to build internal data structures.
|
/// This is used in MultiBodyTree::finalize to build internal data structures.
|
||||||
class MultiBodyTree::InitCache {
|
class MultiBodyTree::InitCache {
|
||||||
public:
|
public:
|
||||||
ID_DECLARE_ALIGNED_ALLOCATOR();
|
ID_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
/// constructor
|
/// constructor
|
||||||
InitCache();
|
InitCache();
|
||||||
///\copydoc MultiBodyTree::addBody
|
///\copydoc MultiBodyTree::addBody
|
||||||
int addBody(const int body_index, const int parent_index, const JointType joint_type,
|
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 &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 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);
|
const mat33 &body_I_body, const int user_int, void *user_ptr);
|
||||||
/// build index arrays
|
/// build index arrays
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int buildIndexSets();
|
int buildIndexSets();
|
||||||
/// @return number of degrees of freedom
|
/// @return number of degrees of freedom
|
||||||
int numDoFs() const { return m_num_dofs; }
|
int numDoFs() const { return m_num_dofs; }
|
||||||
/// @return number of bodies
|
/// @return number of bodies
|
||||||
int numBodies() const { return m_inertias.size(); }
|
int numBodies() const { return m_inertias.size(); }
|
||||||
/// get inertia data for index
|
/// get inertia data for index
|
||||||
/// @param index of the body
|
/// @param index of the body
|
||||||
/// @param inertia pointer for return data
|
/// @param inertia pointer for return data
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getInertiaData(const int index, InertiaData *inertia) const;
|
int getInertiaData(const int index, InertiaData *inertia) const;
|
||||||
/// get joint data for index
|
/// get joint data for index
|
||||||
/// @param index of the body
|
/// @param index of the body
|
||||||
/// @param joint pointer for return data
|
/// @param joint pointer for return data
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getJointData(const int index, JointData *joint) const;
|
int getJointData(const int index, JointData *joint) const;
|
||||||
/// get parent index array (paren_index[i] is the index of the parent of i)
|
/// get parent index array (paren_index[i] is the index of the parent of i)
|
||||||
/// @param parent_index pointer for return data
|
/// @param parent_index pointer for return data
|
||||||
void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; }
|
void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; }
|
||||||
/// get user integer
|
/// get user integer
|
||||||
/// @param index body index
|
/// @param index body index
|
||||||
/// @param user_int user integer
|
/// @param user_int user integer
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getUserInt(const int index, int *user_int) const;
|
int getUserInt(const int index, int *user_int) const;
|
||||||
/// get user pointer
|
/// get user pointer
|
||||||
/// @param index body index
|
/// @param index body index
|
||||||
/// @param user_int user pointer
|
/// @param user_int user pointer
|
||||||
/// @return 0 on success, -1 on failure
|
/// @return 0 on success, -1 on failure
|
||||||
int getUserPtr(const int index, void **user_ptr) const;
|
int getUserPtr(const int index, void **user_ptr) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// vector of bodies
|
// vector of bodies
|
||||||
idArray<InertiaData>::type m_inertias;
|
idArray<InertiaData>::type m_inertias;
|
||||||
// vector of joints
|
// vector of joints
|
||||||
idArray<JointData>::type m_joints;
|
idArray<JointData>::type m_joints;
|
||||||
// number of mechanical degrees of freedom
|
// number of mechanical degrees of freedom
|
||||||
int m_num_dofs;
|
int m_num_dofs;
|
||||||
// parent index array
|
// parent index array
|
||||||
idArray<int>::type m_parent_index;
|
idArray<int>::type m_parent_index;
|
||||||
// user integers
|
// user integers
|
||||||
idArray<int>::type m_user_int;
|
idArray<int>::type m_user_int;
|
||||||
// user pointers
|
// user pointers
|
||||||
idArray<void *>::type m_user_ptr;
|
idArray<void *>::type m_user_ptr;
|
||||||
// index of root body (or -1 if not set)
|
// index of root body (or -1 if not set)
|
||||||
int m_root_index;
|
int m_root_index;
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
#endif // MULTIBODYTREEINITCACHE_HPP_
|
#endif // MULTIBODYTREEINITCACHE_HPP_
|
||||||
|
@ -248,7 +248,11 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar
|
|||||||
// first test: absolute difference between numerical and numerial
|
// first test: absolute difference between numerical and numerial
|
||||||
// differentiation should be small
|
// differentiation should be small
|
||||||
TEST(InvDynKinematicsDifferentiation, errorAbsolute) {
|
TEST(InvDynKinematicsDifferentiation, errorAbsolute) {
|
||||||
|
#ifdef BT_ID_USE_DOUBLE_PRECISION
|
||||||
const idScalar kDeltaT = 1e-7;
|
const idScalar kDeltaT = 1e-7;
|
||||||
|
#else
|
||||||
|
const idScalar kDeltaT = 1e-2;
|
||||||
|
#endif
|
||||||
const idScalar kDuration = 1e-2;
|
const idScalar kDuration = 1e-2;
|
||||||
const idScalar kAcceptableError = 1e-4;
|
const idScalar kAcceptableError = 1e-4;
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user