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

[InverseDynamics] Support for Jacobians & derivatives

Browse Source 
This change adds support for calculating Jacobians and
dot(Jacobian)*u terms, along with the required support for
the 3xN matrices in the standalone math library.
It also adds functions to compute kinematics only (position, velocity, accel).
To facilitate tests, the Cl also adds a RandomTreeCreator to create
randomized multibody trees.
Thanks to Thomas Buschmann for this contribution!
This commit is contained in:
Erwin Coumans
2016-08-25 16:24:28 -07:00
parent 7db9a020b9
commit ba8964c4ac
25 changed files with 1474 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/BulletInverseDynamics/IDConfig.hpp
View File

@@ -2,8 +2,17 @@
/// such as choice of linear algebra library and underlying scalar type
#ifndef IDCONFIG_HPP_
#define IDCONFIG_HPP_
// If true, enable jacobian calculations.
// This adds a 3xN matrix to every body, + 2 3-Vectors.
// so it is not advised for large systems if it is not absolutely necessary.
// Also, this is not required for standard inverse dynamics calculations.
// Will only work with vector math libraries that support 3xN matrices.
#define BT_ID_WITH_JACOBIANS
// If we have a custom configuration, compile without using other parts of bullet.
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
#include <cmath>
#define BT_ID_WO_BULLET
#define BT_ID_POW(a,b) std::pow(a,b)
#define BT_ID_SNPRINTF snprintf
@@ -25,7 +34,7 @@
#define INVDYN_INCLUDE_HELPER_2(x) #x
#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
#ifndef btInverseDynamics
#ifndef btInverseDynamics
#error "custom inverse dynamics config, but no custom namespace defined"
#endif

15
src/BulletInverseDynamics/IDConfigEigen.hpp
View File

@@ -21,20 +21,7 @@ struct idArray {
typedef std::vector<int>::size_type idArrayIdx;
// default to standard malloc/free
#include <cstdlib>
#define idMalloc ::malloc
#define idFree ::free
// currently not aligned at all...
#define ID_DECLARE_ALIGNED_ALLOCATOR() \
inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
inline void operator delete(void* ptr) { idFree(ptr); } \
inline void* operator new(std::size_t, void* ptr) { return ptr; } \
inline void operator delete(void*, void*) {} \
inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
inline void operator delete[](void* ptr) { idFree(ptr); } \
inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
inline void operator delete[](void*, void*) {}
#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Note on interfaces:
// Eigen::Matrix has data(), to get c-array storage
// HOWEVER: default storage is column-major!

54
src/BulletInverseDynamics/IDMath.cpp
View File

@@ -55,6 +55,60 @@ idScalar maxAbs(const vec3 &v) {
return result;
}
#if (defined BT_ID_HAVE_MAT3X)
idScalar maxAbsMat3x(const mat3x &m) {
// only used for tests -- so just loop here for portability
idScalar result = 0.0;
for (idArrayIdx col = 0; col < m.cols(); col++) {
for (idArrayIdx row = 0; row < 3; row++) {
result = BT_ID_MAX(result, std::fabs(m(row, col)));
}
}
return result;
}
void mul(const mat33 &a, const mat3x &b, mat3x *result) {
if (b.cols() != result->cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++) {
const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col);
const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col);
const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col);
setMat3xElem(0, col, x, result);
setMat3xElem(1, col, y, result);
setMat3xElem(2, col, z, result);
}
}
void add(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++) {
for (idArrayIdx row = 0; row < 3; row++) {
setMat3xElem(row, col, a(row, col) + b(row, col), result);
}
}
}
void sub(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
for (idArrayIdx col = 0; col < b.cols(); col++) {
for (idArrayIdx row = 0; row < 3; row++) {
setMat3xElem(row, col, a(row, col) - b(row, col), result);
}
}
}
#endif
mat33 transformX(const idScalar &alpha) {
mat33 T;
const idScalar cos_alpha = std::cos(alpha);

10
src/BulletInverseDynamics/IDMath.hpp
View File

@@ -19,6 +19,16 @@ idScalar maxAbs(const vecx& v);
/// return maximum absolute value
idScalar maxAbs(const vec3& v);
#endif //ID_LINEAR_MATH_USE_EIGEN
#if (defined BT_ID_HAVE_MAT3X)
idScalar maxAbsMat3x(const mat3x& m);
void setZero(mat3x&m);
// define math functions on mat3x here to avoid allocations in operators.
void mul(const mat33&a, const mat3x&b, mat3x* result);
void add(const mat3x&a, const mat3x&b, mat3x* result);
void sub(const mat3x&a, const mat3x&b, mat3x* result);
#endif
/// get offset vector & transform matrix from DH parameters
/// TODO: add documentation
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);

114
src/BulletInverseDynamics/MultiBodyTree.cpp
View File

@@ -61,6 +61,18 @@ int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_a
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
}
int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const {
return m_impl->getParentRParentBodyRef(body_index, r);
}
int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const {
return m_impl->getBodyTParentRef(body_index, T);
}
int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const {
return m_impl->getBodyAxisOfMotion(body_index, axis);
}
void MultiBodyTree::printTree() { m_impl->printTree(); }
void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
@@ -96,9 +108,106 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati
}
return 0;
}
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
return calculateMassMatrix(q, true, true, true, mass_matrix);
}
int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) {
vec3 world_gravity(m_impl->m_world_gravity);
// temporarily set gravity to zero, to ensure we get the actual accelerations
setZero(m_impl->m_world_gravity);
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, u, dot_u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
error_message("error in kinematics calculation\n");
return -1;
}
m_impl->m_world_gravity=world_gravity;
return 0;
}
int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, q, q,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in kinematics calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, u, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in kinematics calculation\n");
return -1;
}
return 0;
}
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in jacobian calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::calculateJacobians(const vecx& q){
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, q,
MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
error_message("error in jacobian calculation\n");
return -1;
}
return 0;
}
int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u);
}
int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const {
return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u);
}
int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const {
return m_impl->getBodyJacobianTrans(body_index,world_jac_trans);
}
int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const {
return m_impl->getBodyJacobianRot(body_index,world_jac_rot);
}
#endif
int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
const vec3 &body_axis_of_motion_, idScalar mass,
@@ -186,6 +295,11 @@ int MultiBodyTree::finalize() {
const int &num_bodies = m_init_cache->numBodies();
const int &num_dofs = m_init_cache->numDoFs();
if(num_dofs<=0) {
error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
//return -1;
}
// 1 allocate internal MultiBody structure
m_impl = new MultiBodyImpl(num_bodies, num_dofs);

61
src/BulletInverseDynamics/MultiBodyTree.hpp
View File

@@ -51,6 +51,7 @@ enum JointType {
/// - gears and motor inertia
class MultiBodyTree {
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
/// The contructor.
/// Initialization & allocation is via addBody and buildSystem calls.
MultiBodyTree();
@@ -117,7 +118,10 @@ public:
void printTree();
/// print tree data to stdout
void printTreeData();
/// calculate joint forces for given generalized state & derivatives
/// Calculate joint forces for given generalized state & derivatives.
/// This also updates kinematic terms computed in calculateKinematics.
/// If gravity is not set to zero, acceleration terms will contain
/// gravitational acceleration.
/// @param q generalized coordinates
/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
/// @param dot_u time derivative of u
@@ -148,6 +152,31 @@ public:
/// @return -1 on error, 0 on success
int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
/// Calculates kinematics also calculated in calculateInverseDynamics,
/// but not dynamics.
/// This function ensures that correct accelerations are computed that do not
/// contain gravitational acceleration terms.
/// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
/// Calculate position kinematics
int calculatePositionKinematics(const vecx& q);
/// Calculate position and velocity kinematics
int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
/// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
/// d(Jacobian)/dt*u
/// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
/// or calculatePositionAndVelocityKinematics
int calculateJacobians(const vecx& q, const vecx& u);
/// Calculate Jacobians (dvel/du)
/// This function assumes that calculateInverseDynamics was called, or
/// one of the calculateKineamtics functions
int calculateJacobians(const vecx& q);
#endif // BT_ID_HAVE_MAT3X
/// set gravitational acceleration
/// the default is [0;0;-9.8] in the world frame
/// @param gravity the gravitational acceleration in world frame
@@ -200,9 +229,20 @@ public:
/// @param world_origin pointer for return data
/// @return 0 on success, -1 on error
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
// get translational jacobian, in world frame (dworld_velocity/du)
int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
// get rotational jacobian, in world frame (dworld_omega/du)
int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
// get product of translational jacobian derivative * generatlized velocities
int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
// get product of rotational jacobian derivative * generatlized velocities
int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
#endif // BT_ID_HAVE_MAT3X
/// returns the (internal) index of body
/// @param body_index is the index of a body (internal: TODO: fix/clarify
/// indexing!)
/// @param body_index is the index of a body
/// @param parent_index pointer to where parent index will be stored
/// @return 0 on success, -1 on error
int getParentIndex(const int body_index, int* parent_index) const;
@@ -216,6 +256,21 @@ public:
/// @param joint_type string naming the corresponding joint type
/// @return 0 on success, -1 on failure
int getJointTypeStr(const int body_index, const char** joint_type) const;
/// get offset translation to parent body (see addBody)
/// @param body_index index of the body
/// @param r the offset translation (see above)
/// @return 0 on success, -1 on failure
int getParentRParentBodyRef(const int body_index, vec3* r) const;
/// get offset rotation to parent body (see addBody)
/// @param body_index index of the body
/// @param T the transform (see above)
/// @return 0 on success, -1 on failure
int getBodyTParentRef(const int body_index, mat33* T) const;
/// get axis of motion (see addBody)
/// @param body_index index of the body
/// @param axis the axis (see above)
/// @return 0 on success, -1 on failure
int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// get offset for degrees of freedom of this body into the q-vector
/// @param body_index index of the body
/// @param q_offset offset the q vector

37
src/BulletInverseDynamics/details/IDEigenInterface.hpp
View File

@@ -2,16 +2,35 @@
#define INVDYNEIGENINTERFACE_HPP_
#include "../IDConfig.hpp"
namespace btInverseDynamics {
#define BT_ID_HAVE_MAT3X
#ifdef BT_USE_DOUBLE_PRECISION
typedef Eigen::VectorXd vecx;
typedef Eigen::Vector3d vec3;
typedef Eigen::Matrix3d mat33;
typedef Eigen::MatrixXd matxx;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> vec3;
typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> mat33;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
#else
typedef Eigen::VectorXf vecx;
typedef Eigen::Vector3f vec3;
typedef Eigen::Matrix3f mat33;
typedef Eigen::MatrixXf matxx;
#endif //
typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> vec3;
typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> mat33;
typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
#endif
inline void resize(mat3x &m, Eigen::Index size) {
m.resize(3, size);
m.setZero();
}
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
(*m)(row, col) = val;
}
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
(*m)(row, col) = val;
}
}
#endif // INVDYNEIGENINTERFACE_HPP_

69
src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
View File

@@ -8,13 +8,13 @@
#include "../../LinearMath/btMatrix3x3.h"
#include "../../LinearMath/btVector3.h"
#include "../../LinearMath/btMatrixX.h"
#define BT_ID_HAVE_MAT3X
namespace btInverseDynamics {
class vec3;
class vecx;
class mat33;
typedef btMatrixX<idScalar> matxx;
typedef btVectorX<idScalar> vecxx;
class vec3 : public btVector3 {
public:
@@ -47,11 +47,11 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s)
inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
class vecx : public vecxx {
class vecx : public btVectorX<idScalar> {
public:
vecx(int size) : vecxx(size) {}
const vecx& operator=(const vecxx& rhs) {
*static_cast<vecxx*>(this) = rhs;
vecx(int size) : btVectorX(size) {}
const vecx& operator=(const btVectorX<idScalar>& rhs) {
*static_cast<btVectorX*>(this) = rhs;
return *this;
}
@@ -108,6 +108,65 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
return result;
}
// use btMatrixX to implement 3xX matrix
class mat3x : public matxx {
public:
mat3x(){}
mat3x(const mat3x&rhs) {
matxx::resize(rhs.rows(), rhs.cols());
*this = rhs;
}
mat3x(int rows, int cols): matxx(3,cols) {
}
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<rows();i++) {
for(int k=0;k<cols();k++) {
setElem(i,k,rhs(i,k));
}
}
}
void setZero() {
matxx::setZero();
}
};
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;
result(1)=0.0;
result(2)=0.0;
for(int i=0;i<b.size();i++) {
for(int k=0;k<3;k++) {
result(k)+=a(k,i)*b(i);
}
}
return result;
}
inline void resize(mat3x &m, idArrayIdx size) {
m.resize(3, size);
m.setZero();
}
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
m->setElem(row, col, val);
}
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
m->setElem(row, col, val);
}
}
#endif // IDLINEARMATHINTERFACE_HPP_

93
src/BulletInverseDynamics/details/IDMatVec.hpp
View File

@@ -5,12 +5,14 @@
#include <cstdlib>
#include "../IDConfig.hpp"
#define BT_ID_HAVE_MAT3X
namespace btInverseDynamics {
class vec3;
class vecx;
class mat33;
class matxx;
class mat3x;
/// This is a very basic implementation to enable stand-alone use of the library.
/// The implementation is not really optimized and misses many features that you would
@@ -85,13 +87,17 @@ private:
class matxx {
public:
matxx() {
m_data = 0x0;
m_cols=0;
m_rows=0;
}
matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
}
~matxx() { idFree(m_data); }
const matxx& operator=(const matxx& rhs);
idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; }
const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; }
idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
const int& rows() const { return m_rows; }
const int& cols() const { return m_cols; }
@@ -101,6 +107,61 @@ private:
idScalar* m_data;
};
class mat3x {
public:
mat3x() {
m_data = 0x0;
m_cols=0;
}
mat3x(const mat3x&rhs) {
m_cols=rhs.m_cols;
allocate();
*this = rhs;
}
mat3x(int rows, int cols): m_cols(cols) {
allocate();
};
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<3*m_cols;i++) {
m_data[i] = rhs.m_data[i];
}
}
~mat3x() {
free();
}
idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
int rows() const { return m_rows; }
const int& cols() const { return m_cols; }
void resize(int rows, int cols) {
m_cols=cols;
free();
allocate();
}
void setZero() {
memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols);
}
// avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
private:
void allocate(){m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));}
void free() { idFree(m_data);}
enum {m_rows=3};
int m_cols;
idScalar* m_data;
};
inline void resize(mat3x &m, idArrayIdx size) {
m.resize(3, size);
m.setZero();
}
//////////////////////////////////////////////////
// Implementations
inline const vec3& vec3::operator=(const vec3& rhs) {
if (&rhs != this) {
memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
@@ -324,5 +385,31 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
return result;
}
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;
result(1)=0.0;
result(2)=0.0;
for(int i=0;i<b.size();i++) {
for(int k=0;k<3;k++) {
result(k)+=a(k,i)*b(i);
}
}
return result;
}
inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
(*m)(row, col) = val;
}
inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
(*m)(row, col) = val;
}
} // namespace btInverseDynamcis
#endif

465
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
View File

@@ -3,7 +3,15 @@
namespace btInverseDynamics {
MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
,m_m3x(3,m_num_dofs)
#endif
{
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
resize(m_m3x,m_num_dofs);
#endif
m_body_list.resize(num_bodies_);
m_parent_index.resize(num_bodies_);
m_child_indices.resize(num_bodies_);
@@ -205,6 +213,20 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
// no static data
break;
}
// resize & initialize jacobians to zero.
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
body.m_body_dot_Jac_T_u(0) = 0.0;
body.m_body_dot_Jac_T_u(1) = 0.0;
body.m_body_dot_Jac_T_u(2) = 0.0;
body.m_body_dot_Jac_R_u(0) = 0.0;
body.m_body_dot_Jac_R_u(1) = 0.0;
body.m_body_dot_Jac_R_u(2) = 0.0;
resize(body.m_body_Jac_T,m_num_dofs);
resize(body.m_body_Jac_R,m_num_dofs);
body.m_body_Jac_T.setZero();
body.m_body_Jac_R.setZero();
#endif //
}
}
@@ -218,131 +240,12 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
return -1;
}
// 1. update relative kinematics
// 1.1 for revolute
for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
mat33 T;
bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
body.m_body_T_parent = T * body.m_body_T_parent_ref;
// body.m_parent_r_parent_body= fixed
body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
// body.m_parent_dot_r_rel = fixed;
// NOTE: this assumes that Jac_JR is constant, which is true for revolute
// joints, but not in the general case (eg, slider-crank type mechanisms)
body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
// body.m_parent_ddot_r_rel = fixed;
}
// 1.2 for prismatic
for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
// body.m_body_T_parent= fixed
body.m_parent_pos_parent_body =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
// body.m_parent_omega_rel = 0;
body.m_parent_vel_rel =
body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
// body.parent_dot_omega_rel = 0;
// NOTE: this assumes that Jac_JT is constant, which is true for
// prismatic joints, but not in the general case
body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
}
// 3. absolute kinematic quantities & dynamic quantities
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
// calculations for root body
{
RigidBody &body = m_body_list[0];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts)
// not required right now, added here for debugging purposes
body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_body_T_world = body.m_body_T_parent;
// 3.2 update absolute velocities
body.m_body_ang_vel = body.m_body_ang_vel_rel;
body.m_body_vel = body.m_parent_vel_rel;
// 3.3 update absolute accelerations
// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
body.m_body_ang_acc = body.m_body_ang_acc_rel;
body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
// add gravitational acceleration to root body
// this is an efficient way to add gravitational terms,
// but it does mean that the kinematics are no longer
// correct at the acceleration level
// NOTE: To get correct acceleration kinematics, just set world_gravity to zero
body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
}
for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts) not required right now added here for debugging purposes
body.m_body_pos =
body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
// 3.2 update absolute velocities
body.m_body_ang_vel =
body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
body.m_body_vel =
body.m_body_T_parent *
(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
body.m_parent_vel_rel);
// 3.3 update absolute accelerations
// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
body.m_body_ang_acc =
body.m_body_T_parent * parent.m_body_ang_acc -
body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
body.m_body_ang_acc_rel;
body.m_body_acc =
body.m_body_T_parent *
(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
parent.m_body_ang_vel.cross(
parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
}
// 1. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
error_message("error in calculateKinematics\n");
return -1;
}
// 2. update contributions to equations of motion for every body.
for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
// 3.4 update dynamic terms (rate of change of angular & linear momentum)
@@ -356,7 +259,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
body.m_body_force_user;
}
// 4. calculate full set of forces at parent joint
// 3. calculate full set of forces at parent joint
// (not directly calculating the joint force along the free direction
// simplifies inclusion of fixed joints.
// An alternative would be to fuse bodies in a pre-processing step,
@@ -416,6 +319,239 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
return 0;
}
int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
static_cast<int>(dot_u.size()));
return -1;
}
if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
error_message("invalid type %d\n", type);
return -1;
}
// 1. update relative kinematics
// 1.1 for revolute
for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
mat33 T;
bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
body.m_body_T_parent = T * body.m_body_T_parent_ref;
if(type >= POSITION_VELOCITY) {
body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
}
if(type >= POSITION_VELOCITY_ACCELERATION) {
body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
}
}
// 1.2 for prismatic
for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
body.m_parent_pos_parent_body =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
if(type >= POSITION_VELOCITY) {
body.m_parent_vel_rel =
body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
}
if(type >= POSITION_VELOCITY_ACCELERATION) {
body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
}
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
if(type >= POSITION_VELOCITY) {
body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
}
if(type >= POSITION_VELOCITY_ACCELERATION) {
body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
}
}
// 2. absolute kinematic quantities (vector valued)
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
// calculations for root body
{
RigidBody &body = m_body_list[0];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts)
// not required right now, added here for debugging purposes
body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_body_T_world = body.m_body_T_parent;
if(type >= POSITION_VELOCITY) {
// 3.2 update absolute velocities
body.m_body_ang_vel = body.m_body_ang_vel_rel;
body.m_body_vel = body.m_parent_vel_rel;
}
if(type >= POSITION_VELOCITY_ACCELERATION) {
// 3.3 update absolute accelerations
// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
body.m_body_ang_acc = body.m_body_ang_acc_rel;
body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
// add gravitational acceleration to root body
// this is an efficient way to add gravitational terms,
// but it does mean that the kinematics are no longer
// correct at the acceleration level
// NOTE: To get correct acceleration kinematics, just set world_gravity to zero
body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
}
}
for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
// 2.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts) not required right now added here for debugging purposes
body.m_body_pos =
body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
if(type >= POSITION_VELOCITY) {
// 2.2 update absolute velocities
body.m_body_ang_vel =
body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
body.m_body_vel =
body.m_body_T_parent *
(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
body.m_parent_vel_rel);
}
if(type >= POSITION_VELOCITY_ACCELERATION) {
// 2.3 update absolute accelerations
// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
body.m_body_ang_acc =
body.m_body_T_parent * parent.m_body_ang_acc -
body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
body.m_body_ang_acc_rel;
body.m_body_acc =
body.m_body_T_parent *
(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
}
}
return 0;
}
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) {
const int& idx=body.m_q_index;
switch(body.m_joint_type) {
case FIXED:
break;
case REVOLUTE:
setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
break;
case PRISMATIC:
setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0)
+body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1)
+body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2),
&body.m_body_Jac_T);
setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0)
+body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1)
+body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2),
&body.m_body_Jac_T);
setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0)
+body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1)
+body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2),
&body.m_body_Jac_T);
break;
case FLOATING:
setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R);
setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R);
setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R);
// body_Jac_T = body_T_parent.transpose();
setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T);
setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T);
setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T);
setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T);
setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T);
setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T);
setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T);
setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T);
setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T);
break;
}
}
int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
return -1;
}
if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
error_message("invalid type %d\n", type);
return -1;
}
addRelativeJacobianComponent(m_body_list[0]);
for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R);
body.m_body_Jac_T = parent.m_body_Jac_T;
mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x);
sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T);
addRelativeJacobianComponent(body);
mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T);
if(type >= POSITION_VELOCITY) {
body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
body.m_body_dot_Jac_T_u = body.m_body_T_parent *
(parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
}
}
return 0;
}
#endif
static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) {
switch (dof) {
// rotational part
@@ -487,13 +623,6 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// This implementation, however, handles branched systems and uses a formulation centered
// on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
// Macro for setting matrix elements depending on underlying math library.
#ifdef ID_LINEAR_MATH_USE_BULLET
#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val)
#else
#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val
#endif
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != m_num_dofs) {
error_message("Dimension error. System has %d DOFs,\n"
@@ -507,7 +636,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (initialize_matrix) {
for (int i = 0; i < m_num_dofs; i++) {
for (int j = 0; j < m_num_dofs; j++) {
setMassMatrixElem(i, j, 0.0);
setMatxxElem(i, j, 0.0, mass_matrix);
}
}
}
@@ -597,7 +726,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
vec3 body_eom_trans =
body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
// rest of the mass matrix column upwards
{
@@ -609,7 +738,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMassMatrixElem(col, row, Mrc);
setMatxxElem(col, row, Mrc, mass_matrix);
}
// 2. ancestor dofs
int child_idx = i;
@@ -634,7 +763,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
}
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMassMatrixElem(col, row, Mrc);
setMatxxElem(col, row, Mrc, mass_matrix);
}
child_idx = parent_idx;
@@ -647,12 +776,10 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (set_lower_triangular_matrix) {
for (int col = 0; col < m_num_dofs; col++) {
for (int row = 0; row < col; row++) {
setMassMatrixElem(row, col, (*mass_matrix)(col, row));
setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
}
}
}
#undef setMassMatrixElem
return 0;
}
@@ -779,6 +906,32 @@ int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
return 0;
}
int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*r=m_body_list[body_index].m_parent_pos_parent_body_ref;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*T=m_body_list[body_index].m_body_T_parent_ref;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
if(m_body_list[body_index].m_joint_type == REVOLUTE) {
*axis = m_body_list[body_index].m_Jac_JR;
return 0;
}
if(m_body_list[body_index].m_joint_type == PRISMATIC) {
*axis = m_body_list[body_index].m_Jac_JT;
return 0;
}
setZero(*axis);
return 0;
}
int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const {
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*q_index = m_body_list[body_index].m_q_index;
@@ -841,4 +994,34 @@ int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3
return 0;
}
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans);
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot);
return 0;
}
#endif
}

47
src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
View File

@@ -110,6 +110,19 @@ struct RigidBody {
vec3 m_body_subtree_mass_com;
/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
mat33 m_body_subtree_I_body;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
/// translational jacobian in body-fixed frame d(m_body_vel)/du
mat3x m_body_Jac_T;
/// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
mat3x m_body_Jac_R;
/// components of linear acceleration depending on u
/// (same as is d(m_Jac_T)/dt*u)
vec3 m_body_dot_Jac_T_u;
/// components of angular acceleration depending on u
/// (same as is d(m_Jac_T)/dt*u)
vec3 m_body_dot_Jac_R_u;
#endif
};
/// The MBS implements a tree structured multibody system
@@ -119,6 +132,12 @@ class MultiBodyTree::MultiBodyImpl {
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
enum KinUpdateType {
POSITION_ONLY,
POSITION_VELOCITY,
POSITION_VELOCITY_ACCELERATION
};
/// constructor
/// @param num_bodies the number of bodies in the system
/// @param num_dofs number of degrees of freedom in the system
@@ -131,6 +150,25 @@ public:
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
const bool initialize_matrix, const bool set_lower_triangular_matrix,
matxx* mass_matrix);
/// calculate kinematics (vector quantities)
/// Depending on type, update positions only, positions & velocities, or positions, velocities
/// and accelerations.
int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
/// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
/// \copydoc MultiBodyTree::getBodyDotJacobianTransU
int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ;
/// \copydoc MultiBodyTree::getBodyDotJacobianRotU
int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
/// \copydoc MultiBodyTree::getBodyJacobianTrans
int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ;
/// \copydoc MultiBodyTree::getBodyJacobianRot
int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
/// Add relative Jacobian component from motion relative to parent body
/// @param body the body to add the Jacobian component for
void addRelativeJacobianComponent(RigidBody&body);
#endif
/// generate additional index sets from the parent_index array
/// @return -1 on error, 0 on success
int generateIndexSets();
@@ -152,6 +190,12 @@ public:
int getJointType(const int body_index, JointType* joint_type) const;
/// \copydoc MultiBodyTree::getJointTypeStr
int getJointTypeStr(const int body_index, const char** joint_type) const;
/// \copydoc MultiBodyTree::getParentRParentBodyRef
int getParentRParentBodyRef(const int body_index, vec3* r) const;
/// \copydoc MultiBodyTree::getBodyTParentRef
int getBodyTParentRef(const int body_index, mat33* T) const;
/// \copydoc MultiBodyTree::getBodyAxisOfMotion
int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// \copydoc MultiBodyTree:getDoFOffset
int getDoFOffset(const int body_index, int* q_index) const;
/// \copydoc MultiBodyTree::getBodyOrigin
@@ -231,6 +275,9 @@ private:
idArray<int>::type m_user_int;
// a user-provided pointer
idArray<void*>::type m_user_ptr;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
mat3x m_m3x;
#endif
};
}
#endif