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);

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