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

Merge pull request #755 from erwincoumans/master

Browse Source 
[InverseDynamics] Support for Jacobians & derivatives
This commit is contained in:
erwincoumans
2016-08-25 20:20:28 -04:00
committed by GitHub
parent d8ee189908 ba8964c4ac
commit 6c961cadf0
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);

8
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
View File

@@ -25,10 +25,10 @@ public:
/// \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;
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:
// 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

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

108
test/InverseDynamics/CMakeLists.txt
View File

@@ -20,14 +20,112 @@ IF (NOT WIN32)
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseDynamics
test_invdyn_kinematics.cpp
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)
LINK_LIBRARIES(
BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
)
IF (NOT WIN32)
LINK_LIBRARIES( pthread )
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseDynamics
test_invdyn_kinematics.cpp
)
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();
}