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

2
Extras/InverseDynamics/CMakeLists.txt
View File

@@ -4,6 +4,7 @@ INCLUDE_DIRECTORIES(
ADD_LIBRARY(
BulletInverseDynamicsUtils
CloneTreeCreator.cpp
CoilCreator.cpp
MultiBodyTreeCreator.cpp
btMultiBodyTreeCreator.cpp
@@ -11,6 +12,7 @@ DillCreator.cpp
MultiBodyTreeDebugGraph.cpp
invdyn_bullet_comparison.cpp
IDRandomUtil.cpp
RandomTreeCreator.cpp
SimpleTreeCreator.cpp
MultiBodyNameMap.cpp
User2InternalIndex.cpp

49
Extras/InverseDynamics/CloneTreeCreator.cpp Normal file
View File

@@ -0,0 +1,49 @@
#include "CloneTreeCreator.hpp"
#include <cstdio>
namespace btInverseDynamics {
#define CHECK_NULLPTR() \
do { \
if (m_reference == 0x0) { \
error_message("m_reference == 0x0\n"); \
return -1; \
} \
} while (0)
#define TRY(x) \
do { \
if (x == -1) { \
error_message("error calling " #x "\n"); \
return -1; \
} \
} while (0)
CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference) { m_reference = reference; }
CloneTreeCreator::~CloneTreeCreator() {}
int CloneTreeCreator::getNumBodies(int* num_bodies) const {
CHECK_NULLPTR();
*num_bodies = m_reference->numBodies();
return 0;
}
int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
CHECK_NULLPTR();
TRY(m_reference->getParentIndex(body_index, parent_index));
TRY(m_reference->getJointType(body_index, joint_type));
TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
TRY(m_reference->getBodyMass(body_index, mass));
TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
TRY(m_reference->getUserInt(body_index, user_int));
TRY(m_reference->getUserPtr(body_index, user_ptr));
return 0;
}
}

27
Extras/InverseDynamics/CloneTreeCreator.hpp Normal file
View File

@@ -0,0 +1,27 @@
#ifndef CLONETREE_CREATOR_HPP_
#define CLONETREE_CREATOR_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
/// Generate an identical multibody tree from a reference system.
class CloneTreeCreator : public MultiBodyTreeCreator {
public:
/// ctor
/// @param reference the MultiBodyTree to clone
CloneTreeCreator(const MultiBodyTree*reference);
~CloneTreeCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
const MultiBodyTree *m_reference;
};
}
#endif // CLONETREE_CREATOR_HPP_

1
Extras/InverseDynamics/IDRandomUtil.cpp
View File

@@ -15,6 +15,7 @@ static const float mass_min = 0.001;
static const float mass_max = 1.0;
void randomInit() { srand(time(NULL)); }
void randomInit(unsigned seed) { srand(seed); }
int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; }

4
Extras/InverseDynamics/IDRandomUtil.hpp
View File

@@ -2,8 +2,10 @@
#define ID_RANDOM_UTIL_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
namespace btInverseDynamics {
/// seed random number generator
/// seed random number generator using time()
void randomInit();
/// seed random number generator with identical value to get repeatable results
void randomInit(unsigned seed);
/// Generate (not quite) uniformly distributed random integers in [low, high]
/// Note: this is a low-quality implementation using only rand(), as
/// C++11 <random> is not supported in bullet.

81
Extras/InverseDynamics/RandomTreeCreator.cpp Normal file
View File

@@ -0,0 +1,81 @@
#include "RandomTreeCreator.hpp"
#include <cstdio>
#include "IDRandomUtil.hpp"
namespace btInverseDynamics {
RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed) {
// seed generator
if(random_seed) {
randomInit(); // seeds with time()
} else {
randomInit(1); // seeds with 1
}
m_num_bodies = randomInt(1, max_bodies);
}
RandomTreeCreator::~RandomTreeCreator() {}
int RandomTreeCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
}
int RandomTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
if(0 == body_index) { //root body
*parent_index = -1;
} else {
*parent_index = randomInt(0, body_index - 1);
}
switch (randomInt(0, 3)) {
case 0:
*joint_type = FIXED;
break;
case 1:
*joint_type = REVOLUTE;
break;
case 2:
*joint_type = PRISMATIC;
break;
case 3:
*joint_type = FLOATING;
break;
default:
error_message("randomInt() result out of range\n");
return -1;
}
(*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0);
(*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0);
bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref);
*body_axis_of_motion = randomAxis();
*mass = randomMass();
(*body_r_body_com)(0) = randomFloat(-1.0, 1.0);
(*body_r_body_com)(1) = randomFloat(-1.0, 1.0);
(*body_r_body_com)(2) = randomFloat(-1.0, 1.0);
const double a = randomFloat(-BT_ID_PI, BT_ID_PI);
const double b = randomFloat(-BT_ID_PI, BT_ID_PI);
const double c = randomFloat(-BT_ID_PI, BT_ID_PI);
vec3 ii = randomInertiaPrincipal();
mat33 ii_diag;
setZero(ii_diag);
ii_diag(0,0)=ii(0);
ii_diag(1,1)=ii(1);
ii_diag(2,2)=ii(2);
*body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag *
transformZ(-c) * transformY(-b) * transformX(-a);
*user_int = 0;
*user_ptr = 0;
return 0;
}
}

31
Extras/InverseDynamics/RandomTreeCreator.hpp Normal file
View File

@@ -0,0 +1,31 @@
#ifndef RANDOMTREE_CREATOR_HPP_
#define RANDOMTREE_CREATOR_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
/// Generate a random MultiBodyTree with fixed or floating base and fixed, prismatic or revolute
/// joints
/// Uses a pseudo random number generator seeded from a random device.
class RandomTreeCreator : public MultiBodyTreeCreator {
public:
/// ctor
/// @param max_bodies maximum number of bodies
/// @param gravity gravitational acceleration
/// @param use_seed if true, seed random number generator
RandomTreeCreator(const int max_bodies, bool use_seed=false);
~RandomTreeCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
private:
int m_num_bodies;
};
}
#endif // RANDOMTREE_CREATOR_HPP_

14
Extras/InverseDynamics/btMultiBodyFromURDF.hpp
View File

@@ -6,12 +6,11 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
/// Create a btMultiBody model from URDF.
/// This is adapted from Bullet URDF loader example
@@ -45,8 +44,7 @@ public:
void init() {
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
BulletURDFImporter urdf_importer(&m_nogfx, 0);
BulletURDFImporter urdf_importer(&m_nogfx,0);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);

6
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
View File

@@ -26,9 +26,9 @@ public:
int getNumBodies(int *num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(const int body_index, int *parent_index, JointType *joint_type,
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion,
idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
void **user_ptr) const;
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com,
mat33 *body_I_body, int *user_int, void **user_ptr) const;
private:
// internal struct holding data extracted from btMultiBody

3
Extras/InverseDynamics/invdyn_bullet_comparison.hpp
View File

@@ -1,12 +1,13 @@
#ifndef INVDYN_BULLET_COMPARISON_HPP
#define INVDYN_BULLET_COMPARISON_HPP
#include "BulletInverseDynamics/IDConfig.hpp"
class btMultiBody;
class btVector3;
namespace btInverseDynamics {
class MultiBodyTree;
class vecx;
/// this function compares the forward dynamics computations implemented in btMultiBody to
/// the inverse dynamics implementation in MultiBodyTree. This is done in three steps

9
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

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

463
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. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
error_message("error in calculateKinematics\n");
return -1;
}
// 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);
}
// 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

104
test/InverseDynamics/CMakeLists.txt
View File

@@ -9,6 +9,103 @@ INCLUDE_DIRECTORIES(
)
#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
LINK_LIBRARIES(
BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
)
IF (NOT WIN32)
LINK_LIBRARIES( pthread )
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseDynamicsJacobian
test_invdyn_jacobian.cpp
)
ADD_TEST(Test_BulletInverseDynamicsJacobian_PASS Test_BulletInverseDynamicsJacobian)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
INCLUDE_DIRECTORIES(
.
../../src
../gtest-1.7.0/include
../../Extras/InverseDynamics
)
#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
LINK_LIBRARIES(
BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
)
IF (NOT WIN32)
LINK_LIBRARIES( pthread )
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseForwardDynamics
test_invdyn_bullet.cpp
../../examples/Utils/b3ResourcePath.cpp
../../examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h
../../examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.h
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.h
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/Utils/b3Clock.cpp
../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp
../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
../../Extras/Serialize/BulletFileLoader/bChunk.cpp
../../Extras/Serialize/BulletFileLoader/bFile.cpp
../../Extras/Serialize/BulletFileLoader/bDNA.cpp
../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp
../../examples/Importers/ImportURDFDemo/URDFImporterInterface.h
../../examples/Importers/ImportURDFDemo/URDFJointTypes.h
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportSTLDemo/ImportSTLSetup.h
../../examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportColladaDemo/ColladaGraphicsInstance.h
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
)
ADD_TEST(Test_BulletInverseForwardDynamics_PASS Test_BulletInverseForwardDynamics)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
INCLUDE_DIRECTORIES(
.
../../src
../gtest-1.7.0/include
../../Extras/InverseDynamics
)
#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
@@ -27,7 +124,8 @@ ENDIF()
ADD_TEST(Test_BulletInverseDynamics_PASS Test_BulletInverseDynamics)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(Test_Collision PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_Collision PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_Collision PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)

35
test/InverseDynamics/premake4.lua
View File

@@ -37,7 +37,8 @@
project "Test_InverseForwardDynamics"
project "Test_InverseDynamicsJacobian"
kind "ConsoleApp"
@@ -45,6 +46,38 @@
includedirs
{
".",
"../../src",
"../../examples/InverseDynamics",
"../../Extras/InverseDynamics",
"../gtest-1.7.0/include"
}
if os.is("Windows") then
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
defines {"_VARIADIC_MAX=10"}
end
links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"}
files {
"test_invdyn_jacobian.cpp",
}
if os.is("Linux") then
links {"pthread"}
end
project "Test_InverseForwardDynamics"
kind "ConsoleApp"
-- defines { }
includedirs
{
".",

10
test/InverseDynamics/test_invdyn_bullet.cpp
View File

@@ -16,12 +16,12 @@
#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
#include <../CommonInterfaces/CommonGUIHelperInterface.h>
#include <gtest/gtest.h>
#include <../Importers/ImportURDFDemo/BulletUrdfImporter.h>
#include <../Importers/ImportURDFDemo/URDF2Bullet.h>
#include <../Importers/ImportURDFDemo/MyMultiBodyCreator.h>
#include <../Importers/ImportURDFDemo/URDF2Bullet.h>
#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Utils/b3ResourcePath.h"
#include <invdyn_bullet_comparison.hpp>
#include <btMultiBodyFromURDF.hpp>

326
test/InverseDynamics/test_invdyn_jacobian.cpp Normal file
View File

@@ -0,0 +1,326 @@
// Test of kinematic consistency: check if finite differences of velocities, accelerations
// match positions
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <gtest/gtest.h>
#include "Bullet3Common/b3Random.h"
#include "CloneTreeCreator.hpp"
#include "CoilCreator.hpp"
#include "DillCreator.hpp"
#include "RandomTreeCreator.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "MultiBodyTreeDebugGraph.hpp"
using namespace btInverseDynamics;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
// minimal smart pointer to make this work for c++2003
template <typename T>
class ptr {
ptr();
ptr(const ptr&);
public:
ptr(T* p) : m_p(p) {};
~ptr() { delete m_p; }
T& operator*() { return *m_p; }
T* operator->() { return m_p; }
T*get() {return m_p;}
const T*get() const {return m_p;}
friend bool operator==(const ptr<T>& lhs, const ptr<T>& rhs) { return rhs.m_p == lhs.m_p; }
friend bool operator!=(const ptr<T>& lhs, const ptr<T>& rhs) { return !(rhs.m_p == lhs.m_p);
}
private:
T* m_p;
};
void calculateDotJacUError(const MultiBodyTreeCreator& creator, const int nloops,
double* max_error) {
// tree1 is used as reference to compute dot(Jacobian)*u from acceleration(dot(u)=0)
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
ASSERT_TRUE(0x0 != tree1);
CloneTreeCreator clone(tree1.get());
// tree2 is used to compute dot(Jacobian)*u using the calculateJacobian function
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
ASSERT_TRUE(0x0 != tree2);
const int ndofs = tree1->numDoFs();
const int nbodies = tree1->numBodies();
if (ndofs <= 0) {
*max_error = 0;
return;
}
vecx q(ndofs);
vecx u(ndofs);
vecx dot_u(ndofs);
vecx zero(ndofs);
setZero(zero);
double max_lin_error = 0;
double max_ang_error = 0;
for (int loop = 0; loop < nloops; loop++) {
for (int i = 0; i < q.size(); i++) {
q(i) = b3RandRange(-B3_PI, B3_PI);
u(i) = b3RandRange(-B3_PI, B3_PI);
}
EXPECT_EQ(0, tree1->calculateKinematics(q, u, zero));
EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u));
EXPECT_EQ(0, tree2->calculateJacobians(q, u));
for (size_t idx = 0; idx < nbodies; idx++) {
vec3 tmp1, tmp2;
vec3 diff;
EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1));
EXPECT_EQ(0, tree2->getBodyDotJacobianTransU(idx, &tmp2));
diff = tmp1 - tmp2;
double lin_error = maxAbs(diff);
if (lin_error > max_lin_error) {
max_lin_error = lin_error;
}
EXPECT_EQ(0, tree1->getBodyAngularAcceleration(idx, &tmp1));
EXPECT_EQ(0, tree2->getBodyDotJacobianRotU(idx, &tmp2));
diff = tmp1 - tmp2;
double ang_error = maxAbs(diff);
if (ang_error > max_ang_error) {
max_ang_error = ang_error;
}
}
}
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
}
void calculateJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
double* max_error) {
// tree1 is used as reference to compute the Jacobian from velocities with unit u vectors.
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
ASSERT_TRUE(0x0 != tree1);
// tree2 is used to compute the Jacobians using the calculateJacobian function
CloneTreeCreator clone(tree1.get());
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
ASSERT_TRUE(0x0 != tree2);
const int ndofs = tree1->numDoFs();
const int nbodies = tree1->numBodies();
if (ndofs <= 0) {
*max_error = 0;
return;
}
vecx q(ndofs);
vecx zero(ndofs);
setZero(zero);
vecx one(ndofs);
double max_lin_error = 0;
double max_ang_error = 0;
for (int loop = 0; loop < nloops; loop++) {
for (int i = 0; i < q.size(); i++) {
q(i) = b3RandRange(-B3_PI, B3_PI);
}
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
EXPECT_EQ(0, tree2->calculateJacobians(q));
for (size_t idx = 0; idx < nbodies; idx++) {
mat3x ref_jac_r(3, ndofs);
mat3x ref_jac_t(3, ndofs);
ref_jac_r.setZero();
ref_jac_t.setZero();
// this re-computes all jacobians for every body ...
// but avoids std::vector<eigen matrix> issues
for (int col = 0; col < ndofs; col++) {
setZero(one);
one(col) = 1.0;
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, one));
vec3 vel, omg;
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel));
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg));
setMat3xElem(0, col, omg(0), &ref_jac_r);
setMat3xElem(1, col, omg(1), &ref_jac_r);
setMat3xElem(2, col, omg(2), &ref_jac_r);
setMat3xElem(0, col, vel(0), &ref_jac_t);
setMat3xElem(1, col, vel(1), &ref_jac_t);
setMat3xElem(2, col, vel(2), &ref_jac_t);
}
mat3x jac_r(3, ndofs);
mat3x jac_t(3, ndofs);
mat3x diff(3, ndofs);
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t));
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r));
sub(ref_jac_t,jac_t,&diff);
double lin_error = maxAbsMat3x(diff);
if (lin_error > max_lin_error) {
max_lin_error = lin_error;
}
sub(ref_jac_r, jac_r,&diff);
double ang_error = maxAbsMat3x(diff);
if (ang_error > max_ang_error) {
max_ang_error = ang_error;
}
}
}
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
}
void calculateVelocityJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
double* max_error) {
// tree1 is used as reference to compute the velocities directly
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
ASSERT_TRUE(0x0 != tree1);
// tree2 is used to compute the velocities via jacobians
CloneTreeCreator clone(tree1.get());
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
ASSERT_TRUE(0x0 != tree2);
const int ndofs = tree1->numDoFs();
const int nbodies = tree1->numBodies();
if (ndofs <= 0) {
*max_error = 0;
return;
}
vecx q(ndofs);
vecx u(ndofs);
double max_lin_error = 0;
double max_ang_error = 0;
for (int loop = 0; loop < nloops; loop++) {
for (int i = 0; i < q.size(); i++) {
q(i) = b3RandRange(-B3_PI, B3_PI);
u(i) = b3RandRange(-B3_PI, B3_PI);
}
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, u));
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
EXPECT_EQ(0, tree2->calculateJacobians(q));
for (size_t idx = 0; idx < nbodies; idx++) {
vec3 vel1;
vec3 omg1;
vec3 vel2;
vec3 omg2;
mat3x jac_r2(3, ndofs);
mat3x jac_t2(3, ndofs);
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel1));
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg1));
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t2));
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r2));
omg2 = jac_r2 * u;
vel2 = jac_t2 * u;
double lin_error = maxAbs(vel1 - vel2);
if (lin_error > max_lin_error) {
max_lin_error = lin_error;
}
double ang_error = maxAbs(omg1 - omg2);
if (ang_error > max_ang_error) {
max_ang_error = ang_error;
}
}
}
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
}
// test nonlinear terms: dot(Jacobian)*u (linear and angular acceleration for dot_u ==0)
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
TEST(InvDynJacobians, JacDotJacU) {
const int kNumLevels = 5;
#ifdef B3_USE_DOUBLE_PRECISION
const double kMaxError = 1e-12;
#else
const double kMaxError = 5e-5;
#endif
const int kNumLoops = 20;
for (int level = 0; level < kNumLevels; level++) {
const int nbodies = BT_ID_POW(2, level);
CoilCreator coil(nbodies);
double error;
calculateDotJacUError(coil, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
DillCreator dill(level);
calculateDotJacUError(dill, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
const int kRandomLoops = 100;
const int kMaxRandomBodies = 128;
for (int loop = 0; loop < kRandomLoops; loop++) {
RandomTreeCreator random(kMaxRandomBodies);
double error;
calculateDotJacUError(random, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
}
// Jacobians: linear and angular acceleration for dot_u ==0
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
TEST(InvDynJacobians, Jacobians) {
const int kNumLevels = 5;
#ifdef B3_USE_DOUBLE_PRECISION
const double kMaxError = 1e-12;
#else
const double kMaxError = 5e-5;
#endif
const int kNumLoops = 20;
for (int level = 0; level < kNumLevels; level++) {
const int nbodies = BT_ID_POW(2, level);
CoilCreator coil(nbodies);
double error;
calculateJacobianError(coil, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
DillCreator dill(level);
calculateDotJacUError(dill, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
const int kRandomLoops = 20;
const int kMaxRandomBodies = 16;
for (int loop = 0; loop < kRandomLoops; loop++) {
RandomTreeCreator random(kMaxRandomBodies);
double error;
calculateJacobianError(random, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
}
// test for jacobian*u == velocity
TEST(InvDynJacobians, VelocitiesFromJacobians) {
const int kRandomLoops = 20;
const int kMaxRandomBodies = 16;
const int kNumLoops = 20;
#ifdef B3_USE_DOUBLE_PRECISION
const double kMaxError = 1e-12;
#else
const double kMaxError = 5e-5;
#endif
for (int loop = 0; loop < kRandomLoops; loop++) {
RandomTreeCreator random(kMaxRandomBodies);
double error;
calculateVelocityJacobianError(random, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
}
#endif
int main(int argc, char** argv) {
b3Srand(1234);
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}