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

add real-time safe Inverse Dynamics library+test+utils

Browse Source 
an example for the example browser will follow.
thanks to Thomas Buschmann
This commit is contained in:
Erwin Coumans
2015-11-17 08:27:38 -08:00
parent 7d9365319c
commit aa4d119f98
41 changed files with 5233 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
Extras/InverseDynamics/CoilCreator.cpp Normal file
View File

@@ -0,0 +1,67 @@
#include <cmath>
#include "CoilCreator.hpp"
namespace btInverseDynamics {
CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) {
for (int i = 0; i < m_num_bodies; i++) {
m_parent[i] = i - 1;
}
// DH parameters (that's what's in the paper ...)
const idScalar theta_DH = 0;
const idScalar d_DH = 0.0;
const idScalar a_DH = 1.0 / m_num_bodies;
const idScalar alpha_DH = 5.0 * M_PI / m_num_bodies;
getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref,
&m_body_T_parent_ref);
// always z-axis
m_body_axis_of_motion(0) = 0.0;
m_body_axis_of_motion(1) = 0.0;
m_body_axis_of_motion(2) = 1.0;
m_mass = 1.0 / m_num_bodies;
m_body_r_body_com(0) = 1.0 / (2.0 * m_num_bodies);
m_body_r_body_com(1) = 0.0;
m_body_r_body_com(2) = 0.0;
m_body_I_body(0, 0) = 1e-4 / (2.0 * m_num_bodies);
m_body_I_body(0, 1) = 0.0;
m_body_I_body(0, 2) = 0.0;
m_body_I_body(1, 0) = 0.0;
m_body_I_body(1, 1) = (3e-4 + 4.0 / pow(m_num_bodies, 2)) / (12.0 * m_num_bodies);
m_body_I_body(1, 2) = 0.0;
m_body_I_body(2, 0) = 0.0;
m_body_I_body(2, 1) = 0.0;
m_body_I_body(2, 2) = m_body_I_body(1, 1);
}
CoilCreator::~CoilCreator() {}
int CoilCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
}
int CoilCreator::getBody(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 (body_index < 0 || body_index >= m_num_bodies) {
error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
*joint_type = REVOLUTE;
*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
*body_T_parent_ref = m_body_T_parent_ref;
*body_axis_of_motion = m_body_axis_of_motion;
*mass = m_mass;
*body_r_body_com = m_body_r_body_com;
*body_I_body = m_body_I_body;
*user_int = 0;
*user_ptr = 0;
return 0;
}
}

40
Extras/InverseDynamics/CoilCreator.hpp Normal file
View File

@@ -0,0 +1,40 @@
#ifndef COILCREATOR_HPP_
#define COILCREATOR_HPP_
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
/// Creator class for building a "coil" system as intruduced as benchmark example in
/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n))
/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International
/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628.
///
/// This is a serial chain, with an initial configuration resembling a coil.
class CoilCreator : public MultiBodyTreeCreator {
public:
/// ctor.
/// @param n the number of bodies in the system
CoilCreator(int n);
/// dtor
~CoilCreator();
// \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;
std::vector<int> m_parent;
vec3 m_parent_r_parent_body_ref;
mat33 m_body_T_parent_ref;
vec3 m_body_axis_of_motion;
idScalar m_mass;
vec3 m_body_r_body_com;
mat33 m_body_I_body;
};
}
#endif

122
Extras/InverseDynamics/DillCreator.cpp Normal file
View File

@@ -0,0 +1,122 @@
#include "DillCreator.hpp"
#include <cmath>
namespace btInverseDynamics {
DillCreator::DillCreator(int level)
: m_level(level),
m_num_bodies(std::pow(2, level)),
m_parent(m_num_bodies),
m_parent_r_parent_body_ref(m_num_bodies),
m_body_T_parent_ref(m_num_bodies),
m_body_axis_of_motion(m_num_bodies),
m_mass(m_num_bodies),
m_body_r_body_com(m_num_bodies),
m_body_I_body(m_num_bodies) {
// generate names (for debugging)
for (int i = 0; i < m_num_bodies; i++) {
m_parent[i] = i - 1;
// all z-axis (DH convention)
m_body_axis_of_motion[i](0) = 0.0;
m_body_axis_of_motion[i](1) = 0.0;
m_body_axis_of_motion[i](2) = 1.0;
}
// recursively build data structures
m_current_body = 0;
const int parent = -1;
const idScalar d_DH = 0.0;
const idScalar a_DH = 0.0;
const idScalar alpha_DH = 0.0;
if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) {
error_message("recurseDill failed\n");
abort();
}
}
DillCreator::~DillCreator() {}
int DillCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
}
int DillCreator::getBody(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 (body_index < 0 || body_index >= m_num_bodies) {
error_message("invalid body index %d\n", body_index);
return -1;
}
*parent_index = m_parent[body_index];
*joint_type = REVOLUTE;
*parent_r_parent_body_ref = m_parent_r_parent_body_ref[body_index];
*body_T_parent_ref = m_body_T_parent_ref[body_index];
*body_axis_of_motion = m_body_axis_of_motion[body_index];
*mass = m_mass[body_index];
*body_r_body_com = m_body_r_body_com[body_index];
*body_I_body = m_body_I_body[body_index];
*user_int = 0;
*user_ptr = 0;
return 0;
}
int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in) {
if (level < 0) {
error_message("invalid level parameter (%d)\n", level);
return -1;
}
if (m_current_body >= m_num_bodies || m_current_body < 0) {
error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
m_num_bodies);
return -1;
}
idScalar size = std::max(level, 1);
const int body = m_current_body;
// length = 0.1 * size;
// with = 2 * 0.01 * size;
/// these parameters are from the paper ...
/// TODO: add proper citation
m_parent[body] = parent;
m_mass[body] = 0.1 * std::pow(size, 3);
m_body_r_body_com[body](0) = 0.05 * size;
m_body_r_body_com[body](1) = 0;
m_body_r_body_com[body](2) = 0;
// initialization
for (int i = 0; i < 3; i++) {
m_parent_r_parent_body_ref[body](i) = 0;
for (int j = 0; j < 3; j++) {
m_body_I_body[body](i, j) = 0.0;
m_body_T_parent_ref[body](i, j) = 0.0;
}
}
const idScalar size_5 = pow(size, 5);
m_body_I_body[body](0, 0) = size_5 / 0.2e6;
m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6;
m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1);
getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body],
&m_body_T_parent_ref[body]);
// attach "level" Dill systems of levels 1...level
for (int i = 1; i <= level; i++) {
idScalar d_DH = 0.01 * size;
if (i == level) {
d_DH = 0.0;
}
const idScalar a_DH = i * 0.1;
const idScalar alpha_DH = i * M_PI / 3.0;
m_current_body++;
recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
}
return 0; // ok!
}
}

47
Extras/InverseDynamics/DillCreator.hpp Normal file
View File

@@ -0,0 +1,47 @@
#ifndef DILLCREATOR_HPP_
#define DILLCREATOR_HPP_
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
/// Creator class for building a "Dill" system as intruduced as benchmark example in
/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n))
/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International
/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628.
///
/// This is a self-similar branched tree, somewhat resembling a dill plant
class DillCreator : public MultiBodyTreeCreator {
public:
/// ctor
/// @param levels the number of dill levels
DillCreator(int levels);
/// dtor
~DillCreator();
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(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:
/// recursively generate dill bodies.
/// TODO better documentation
int recurseDill(const int levels, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in);
int m_level;
int m_num_bodies;
std::vector<int> m_parent;
std::vector<vec3> m_parent_r_parent_body_ref;
std::vector<mat33> m_body_T_parent_ref;
std::vector<vec3> m_body_axis_of_motion;
std::vector<idScalar> m_mass;
std::vector<vec3> m_body_r_body_com;
std::vector<mat33> m_body_I_body;
int m_current_body;
};
}
#endif

70
Extras/InverseDynamics/IDRandomUtil.cpp Normal file
View File

@@ -0,0 +1,70 @@
#include <cmath>
#include <cstdlib>
#include <ctime>
#include "BulletInverseDynamics/IDConfig.hpp"
#include "BulletInverseDynamics/IDMath.hpp"
#include "IDRandomUtil.hpp"
namespace btInverseDynamics {
// constants for random mass and inertia generation
// these are arbitrary positive values.
static const float mass_min = 0.001;
static const float mass_max = 1.0;
void randomInit() { srand(time(NULL)); }
int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; }
float randomFloat(float low, float high) {
return low + static_cast<float>(rand()) / RAND_MAX * (high - low);
}
float randomMass() { return randomFloat(mass_min, mass_max); }
vec3 randomInertiaPrincipal() {
vec3 inertia;
do {
inertia(0) = randomFloat(mass_min, mass_max);
inertia(1) = randomFloat(mass_min, mass_max);
inertia(2) = randomFloat(mass_min, mass_max);
} while (inertia(0) + inertia(1) < inertia(2) || inertia(0) + inertia(2) < inertia(1) ||
inertia(1) + inertia(2) < inertia(0));
return inertia;
}
mat33 randomInertiaMatrix() {
// generate random valid inertia matrix by first getting valid components
// along major axes and then rotating by random amount
vec3 principal = randomInertiaPrincipal();
mat33 rot(transformX(randomFloat(-M_PI, M_PI)) * transformY(randomFloat(-M_PI, M_PI)) *
transformZ(randomFloat(-M_PI, M_PI)));
mat33 inertia;
inertia(0, 0) = principal(0);
inertia(0, 1) = 0;
inertia(0, 2) = 0;
inertia(1, 0) = 0;
inertia(1, 1) = principal(1);
inertia(1, 2) = 0;
inertia(2, 0) = 0;
inertia(2, 1) = 0;
inertia(2, 2) = principal(2);
return rot * inertia * rot.transpose();
}
vec3 randomAxis() {
vec3 axis;
idScalar length;
do {
axis(0) = randomFloat(-1.0, 1.0);
axis(1) = randomFloat(-1.0, 1.0);
axis(2) = randomFloat(-1.0, 1.0);
length = std::sqrt(std::pow(axis(0), 2) + std::pow(axis(1), 2) + std::pow(axis(2), 2));
} while (length < 0.01);
return axis / length;
}
}

34
Extras/InverseDynamics/IDRandomUtil.hpp Normal file
View File

@@ -0,0 +1,34 @@
#ifndef ID_RANDOM_UTIL_HPP_
#define ID_RANDOM_UTIL_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
namespace btInverseDynamics {
/// seed random number generator
void randomInit();
/// 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.
/// The results will *not* be perfectly uniform.
/// \param low is the lower bound (inclusive)
/// \param high is the lower bound (inclusive)
/// \return a random number within [\param low, \param high]
int randomInt(int low, int high);
/// Generate a (not quite) uniformly distributed random floats in [low, high]
/// Note: this is a low-quality implementation using only rand(), as
/// C++11 <random> is not supported in bullet.
/// The results will *not* be perfectly uniform.
/// \param low is the lower bound (inclusive)
/// \param high is the lower bound (inclusive)
/// \return a random number within [\param low, \param high]
float randomFloat(float low, float high);
/// generate a random valid mass value
/// \returns random mass
float randomMass();
/// generate a random valid vector of principal moments of inertia
vec3 randomInertiaPrincipal();
/// generate a random valid moment of inertia matrix
mat33 randomInertiaMatrix();
/// generate a random unit vector
vec3 randomAxis();
}
#endif

78
Extras/InverseDynamics/MultiBodyNameMap.cpp Normal file
View File

@@ -0,0 +1,78 @@
#include "MultiBodyNameMap.hpp"
namespace btInverseDynamics {
MultiBodyNameMap::MultiBodyNameMap() {}
int MultiBodyNameMap::addBody(const int index, const std::string& name) {
if (m_index_to_body_name.count(index) > 0) {
error_message("trying to add index %d again\n", index);
return -1;
}
if (m_body_name_to_index.count(name) > 0) {
error_message("trying to add name %s again\n", name.c_str());
return -1;
}
m_index_to_body_name[index] = name;
m_body_name_to_index[name] = index;
return 0;
}
int MultiBodyNameMap::addJoint(const int index, const std::string& name) {
if (m_index_to_joint_name.count(index) > 0) {
error_message("trying to add index %d again\n", index);
return -1;
}
if (m_joint_name_to_index.count(name) > 0) {
error_message("trying to add name %s again\n", name.c_str());
return -1;
}
m_index_to_joint_name[index] = name;
m_joint_name_to_index[name] = index;
return 0;
}
int MultiBodyNameMap::getBodyName(const int index, std::string* name) const {
std::map<int, std::string>::const_iterator it = m_index_to_body_name.find(index);
if (it == m_index_to_body_name.end()) {
error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
return 0;
}
int MultiBodyNameMap::getJointName(const int index, std::string* name) const {
std::map<int, std::string>::const_iterator it = m_index_to_joint_name.find(index);
if (it == m_index_to_joint_name.end()) {
error_message("index %d not known\n", index);
return -1;
}
*name = it->second;
return 0;
}
int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const {
std::map<std::string, int>::const_iterator it = m_body_name_to_index.find(name);
if (it == m_body_name_to_index.end()) {
error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
return 0;
}
int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const {
std::map<std::string, int>::const_iterator it = m_joint_name_to_index.find(name);
if (it == m_joint_name_to_index.end()) {
error_message("name %s not known\n", name.c_str());
return -1;
}
*index = it->second;
return 0;
}
}

54
Extras/InverseDynamics/MultiBodyNameMap.hpp Normal file
View File

@@ -0,0 +1,54 @@
#ifndef MULTIBODYNAMEMAP_HPP_
#define MULTIBODYNAMEMAP_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
#include <string>
#include <map>
namespace btInverseDynamics {
/// \brief The MultiBodyNameMap class
/// Utility class that stores a maps from body/joint indices to/from body and joint names
class MultiBodyNameMap {
public:
MultiBodyNameMap();
/// add a body to the map
/// @param index of the body
/// @param name name of the body
/// @return 0 on success, -1 on failure
int addBody(const int index, const std::string& name);
/// add a joint to the map
/// @param index of the joint
/// @param name name of the joint
/// @return 0 on success, -1 on failure
int addJoint(const int index, const std::string& name);
/// get body name from index
/// @param index of the body
/// @param body_name name of the body
/// @return 0 on success, -1 on failure
int getBodyName(const int index, std::string* name) const;
/// get joint name from index
/// @param index of the joint
/// @param joint_name name of the joint
/// @return 0 on success, -1 on failure
int getJointName(const int index, std::string* name) const;
/// get body index from name
/// @param index of the body
/// @param name name of the body
/// @return 0 on success, -1 on failure
int getBodyIndex(const std::string& name, int* index) const;
/// get joint index from name
/// @param index of the joint
/// @param name name of the joint
/// @return 0 on success, -1 on failure
int getJointIndex(const std::string& name, int* index) const;
private:
std::map<int, std::string> m_index_to_joint_name;
std::map<int, std::string> m_index_to_body_name;
std::map<std::string, int> m_joint_name_to_index;
std::map<std::string, int> m_body_name_to_index;
};
}
#endif // MULTIBODYNAMEMAP_HPP_

64
Extras/InverseDynamics/MultiBodyTreeCreator.cpp Normal file
View File

@@ -0,0 +1,64 @@
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) {
int num_bodies;
int parent_index;
JointType joint_type;
vec3 body_r_parent_body_ref;
mat33 body_R_parent_ref;
vec3 body_axis_of_motion;
idScalar mass;
vec3 body_r_body_com;
mat33 body_I_body;
int user_int;
void* user_ptr;
MultiBodyTree* tree = new MultiBodyTree();
if (0x0 == tree) {
error_message("cannot allocate tree\n");
return 0x0;
}
// TODO: move to some policy argument
tree->setAcceptInvalidMassParameters(false);
// get number of bodies in the system
if (-1 == creator.getNumBodies(&num_bodies)) {
error_message("getting body indices\n");
delete tree;
return 0x0;
}
// get data for all bodies
for (int index = 0; index < num_bodies; index++) {
// get body parameters from user callbacks
if (-1 ==
creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
&body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
&body_I_body, &user_int, &user_ptr)) {
error_message("getting data for body %d\n", index);
delete tree;
return 0x0;
}
// add body to system
if (-1 ==
tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
body_I_body, user_int, user_ptr)) {
error_message("adding body %d\n", index);
delete tree;
return 0x0;
}
}
// finalize initialization
if (-1 == tree->finalize()) {
error_message("building system\n");
delete tree;
return 0x0;
}
return tree;
}
}

45
Extras/InverseDynamics/MultiBodyTreeCreator.hpp Normal file
View File

@@ -0,0 +1,45 @@
#ifndef MULTI_BODY_TREE_CREATOR_HPP_
#define MULTI_BODY_TREE_CREATOR_HPP_
#include <string>
#include <vector>
#include "BulletInverseDynamics/IDConfig.hpp"
#include "BulletInverseDynamics/IDMath.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "MultiBodyNameMap.hpp"
namespace btInverseDynamics {
/// Interface class for initializing a MultiBodyTree instance.
/// Data to be provided is modeled on the URDF specification.
/// The user can derive from this class in order to programmatically
/// initialize a system.
class MultiBodyTreeCreator {
public:
/// the dtor
virtual ~MultiBodyTreeCreator() {}
/// Get the number of bodies in the system
/// @param num_bodies write number of bodies here
/// @return 0 on success, -1 on error
virtual int getNumBodies(int* num_bodies) const = 0;
/// Interface for accessing link mass properties.
/// For detailed description of data, @sa MultiBodyTree::addBody
/// \copydoc MultiBodyTree::addBody
virtual 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 = 0;
/// @return a pointer to a name mapping utility class, or 0x0 if not available
virtual const MultiBodyNameMap* getNameMap() const {return 0x0;}
};
/// Create a multibody object.
/// @param creator an object implementing the MultiBodyTreeCreator interface
/// that returns data defining the system
/// @return A pointer to an allocated multibodytree instance, or
/// 0x0 if an error occured.
MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator);
}
// does urdf have gravity direction ??
#endif // MULTI_BODY_TREE_CREATOR_HPP_

64
Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp Normal file
View File

@@ -0,0 +1,64 @@
#include "MultiBodyTreeDebugGraph.hpp"
#include <cstdio>
namespace btInverseDynamics {
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename) {
if (0x0 == tree) {
error_message("tree pointer is null\n");
return -1;
}
if (0x0 == filename) {
error_message("filename is null\n");
return -1;
}
FILE* fp = fopen(filename, "w");
if (NULL == fp) {
error_message("cannot open file %s for writing\n", filename);
return -1;
}
fprintf(fp, "// to generate postscript file, run dot -Tps %s -o %s.ps\n"
"// details see graphviz documentation at http://graphviz.org\n"
"digraph tree {\n",
filename, filename);
for (int body = 0; body < tree->numBodies(); body++) {
std::string name;
if (0x0 != map) {
if (-1 == map->getBodyName(body, &name)) {
error_message("can't get name of body %d\n", body);
return -1;
}
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str());
}
}
for (int body = 0; body < tree->numBodies(); body++) {
int parent;
const char* joint_type;
int qi;
if (-1 == tree->getParentIndex(body, &parent)) {
error_message("indexing error\n");
return -1;
}
if (-1 == tree->getJointTypeStr(body, &joint_type)) {
error_message("indexing error\n");
return -1;
}
if (-1 == tree->getDoFOffset(body, &qi)) {
error_message("indexing error\n");
return -1;
}
if (-1 != parent) {
fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
joint_type, qi);
}
}
fprintf(fp, "}\n");
fclose(fp);
return 0;
}
}

17
Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp Normal file
View File

@@ -0,0 +1,17 @@
#ifndef MULTIBODYTREEDEBUGGRAPH_HPP_
#define MULTIBODYTREEDEBUGGRAPH_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "MultiBodyNameMap.hpp"
namespace btInverseDynamics {
/// generate a dot-file of the multibody tree for generating a graph using graphviz' dot tool
/// @param tree the multibody tree
/// @param map to add names of links (if 0x0, no names will be added)
/// @param filename name for the output file
/// @return 0 on success, -1 on error
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename);
}
#endif // MULTIBODYTREEDEBUGGRAPH_HPP

69
Extras/InverseDynamics/SimpleTreeCreator.cpp Normal file
View File

@@ -0,0 +1,69 @@
#include "SimpleTreeCreator.hpp"
#include <cstdio>
namespace btInverseDynamics {
/// minimal "tree" (chain)
SimpleTreeCreator::SimpleTreeCreator(int dim) : m_num_bodies(dim) {
m_mass = 1.0;
m_body_T_parent_ref(0, 0) = 1;
m_body_T_parent_ref(0, 1) = 0;
m_body_T_parent_ref(0, 2) = 0;
m_body_T_parent_ref(1, 0) = 0;
m_body_T_parent_ref(1, 1) = 1;
m_body_T_parent_ref(1, 2) = 0;
m_body_T_parent_ref(2, 0) = 0;
m_body_T_parent_ref(2, 1) = 0;
m_body_T_parent_ref(2, 2) = 1;
m_parent_r_parent_body_ref(0) = 1.0;
m_parent_r_parent_body_ref(1) = 0.0;
m_parent_r_parent_body_ref(2) = 0.0;
m_body_r_body_com(0) = 0.5;
m_body_r_body_com(1) = 0.0;
m_body_r_body_com(2) = 0.0;
m_body_I_body(0, 0) = 1;
m_body_I_body(0, 1) = 0;
m_body_I_body(0, 2) = 0;
m_body_I_body(1, 0) = 0;
m_body_I_body(1, 1) = 1;
m_body_I_body(1, 2) = 0;
m_body_I_body(2, 0) = 0;
m_body_I_body(2, 1) = 0;
m_body_I_body(2, 2) = 1;
m_axis(0) = 0;
m_axis(1) = 0;
m_axis(2) = 1;
}
int SimpleTreeCreator::getNumBodies(int* num_bodies) const {
*num_bodies = m_num_bodies;
return 0;
}
int SimpleTreeCreator::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 {
*parent_index = body_index - 1;
if (body_index % 2) {
*joint_type = PRISMATIC;
} else {
*joint_type = REVOLUTE;
}
*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
if (0 == body_index) {
(*parent_r_parent_body_ref)(2) = 1.0;
}
*body_T_parent_ref = m_body_T_parent_ref;
*body_axis_of_motion = m_axis;
*mass = m_mass;
*body_r_body_com = m_body_r_body_com;
*body_I_body = m_body_I_body;
*user_int = 0;
*user_ptr = 0;
return 0;
}
}

34
Extras/InverseDynamics/SimpleTreeCreator.hpp Normal file
View File

@@ -0,0 +1,34 @@
#ifndef SIMPLETREECREATOR_HPP_
#define SIMPLETREECREATOR_HPP_
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
/// minimal "tree" (chain)
class SimpleTreeCreator : public MultiBodyTreeCreator {
public:
/// ctor
/// @param dim number of bodies
SimpleTreeCreator(int dim);
// dtor
~SimpleTreeCreator() {}
///\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;
idScalar m_mass;
mat33 m_body_T_parent_ref;
vec3 m_parent_r_parent_body_ref;
vec3 m_body_r_body_com;
mat33 m_body_I_body;
vec3 m_axis;
};
}
#endif // SIMPLETREECREATOR_HPP_

99
Extras/InverseDynamics/User2InternalIndex.cpp Normal file
View File

@@ -0,0 +1,99 @@
#include "User2InternalIndex.hpp"
namespace btInverseDynamics {
User2InternalIndex::User2InternalIndex() : m_map_built(false) {}
void User2InternalIndex::addBody(const int body, const int parent) {
m_user_parent_index_map[body] = parent;
}
int User2InternalIndex::findRoot(int index) {
if (0 == m_user_parent_index_map.count(index)) {
return index;
}
return findRoot(m_user_parent_index_map[index]);
}
// modelled after URDF2Bullet.cpp:void ComputeParentIndices(const
// URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex,
// int urdfParentIndex)
void User2InternalIndex::recurseIndexSets(const int user_body_index) {
m_user_to_internal[user_body_index] = m_current_index;
m_current_index++;
for (size_t i = 0; i < m_user_child_indices[user_body_index].size(); i++) {
recurseIndexSets(m_user_child_indices[user_body_index][i]);
}
}
int User2InternalIndex::buildMapping() {
// find root index
int user_root_index = -1;
for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
it != m_user_parent_index_map.end(); it++) {
int current_root_index = findRoot(it->second);
if (it == m_user_parent_index_map.begin()) {
user_root_index = current_root_index;
} else {
if (user_root_index != current_root_index) {
error_message("multiple roots (at least) %d and %d\n", user_root_index,
current_root_index);
return -1;
}
}
}
// build child index map
for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
it != m_user_parent_index_map.end(); it++) {
m_user_child_indices[it->second].push_back(it->first);
}
m_current_index = -1;
// build internal index set
m_user_to_internal[user_root_index] = -1; // add map for root link
recurseIndexSets(user_root_index);
// reverse mapping
for (std::map<int, int>::iterator it = m_user_to_internal.begin();
it != m_user_to_internal.end(); it++) {
m_internal_to_user[it->second] = it->first;
}
m_map_built = true;
return 0;
}
int User2InternalIndex::user2internal(const int user, int *internal) const {
if (!m_map_built) {
return -1;
}
std::map<int, int>::const_iterator it;
it = m_user_to_internal.find(user);
if (it != m_user_to_internal.end()) {
*internal = it->second;
return 0;
} else {
error_message("no user index %d\n", user);
return -1;
}
}
int User2InternalIndex::internal2user(const int internal, int *user) const {
if (!m_map_built) {
return -1;
}
std::map<int, int>::const_iterator it;
it = m_internal_to_user.find(internal);
if (it != m_internal_to_user.end()) {
*user = it->second;
return 0;
} else {
error_message("no internal index %d\n", internal);
return -1;
}
}
}

46
Extras/InverseDynamics/User2InternalIndex.hpp Normal file
View File

@@ -0,0 +1,46 @@
#ifndef USER2INTERNALINDEX_HPP
#define USER2INTERNALINDEX_HPP
#include <map>
#include <vector>
#include "BulletInverseDynamics/IDConfig.hpp"
namespace btInverseDynamics {
/// Convert arbitrary indexing scheme to internal indexing
/// used for MultiBodyTree
class User2InternalIndex {
public:
/// Ctor
User2InternalIndex();
/// add body to index maps
/// @param body index of body to add (external)
/// @param parent index of parent body (external)
void addBody(const int body, const int parent);
/// build mapping from external to internal indexing
/// @return 0 on success, -1 on failure
int buildMapping();
/// get internal index from external index
/// @param user external (user) index
/// @param internal pointer for storage of corresponding internal index
/// @return 0 on success, -1 on failure
int user2internal(const int user, int *internal) const;
/// get internal index from external index
/// @param user external (user) index
/// @param internal pointer for storage of corresponding internal index
/// @return 0 on success, -1 on failure
int internal2user(const int internal, int *user) const;
private:
int findRoot(int index);
void recurseIndexSets(const int user_body_index);
bool m_map_built;
std::map<int, int> m_user_parent_index_map;
std::map<int, int> m_user_to_internal;
std::map<int, int> m_internal_to_user;
std::map<int, std::vector<int> > m_user_child_indices;
int m_current_index;
};
}
#endif // USER2INTERNALINDEX_HPP

91
Extras/InverseDynamics/btMultiBodyFromURDF.hpp Normal file
View File

@@ -0,0 +1,91 @@
#ifndef BTMULTIBODYFROMURDF_HPP
#define BTMULTIBODYFROMURDF_HPP
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#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"
/// Create a btMultiBody model from URDF.
/// This is adapted from Bullet URDF loader example
class MyBtMultiBodyFromURDF {
public:
/// ctor
/// @param gravity gravitational acceleration (in world frame)
/// @param base_fixed if true, the root body is treated as fixed,
/// if false, it is treated as floating
MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed)
: m_gravity(gravity), m_base_fixed(base_fixed) {
m_broadphase = 0x0;
m_dispatcher = 0x0;
m_solver = 0x0;
m_collisionConfiguration = 0x0;
m_dynamicsWorld = 0x0;
m_multibody = 0x0;
}
/// dtor
~MyBtMultiBodyFromURDF() {
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
delete m_multibody;
}
/// @param name path to urdf file
void setFileName(const std::string name) { m_filename = name; }
/// load urdf file and build btMultiBody model
void init() {
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
BulletURDFImporter urdf_importer(&m_nogfx);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
if (loadOk) {
btTransform identityTrans;
identityTrans.setIdentity();
MyMultiBodyCreator creation(&m_nogfx);
const bool use_multibody = true;
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody,
u2b.getPathPrefix());
m_multibody = creation.getBulletMultiBody();
m_dynamicsWorld->stepSimulation(1. / 240., 0);
}
}
/// @return pointer to the btMultiBody model
btMultiBody *getBtMultiBody() { return m_multibody; }
private:
// internal utility function
void createEmptyDynamicsWorld() {
m_collisionConfiguration = new btDefaultCollisionConfiguration();
/// use the default collision dispatcher. For parallel processing you can use a diffent
/// dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btMultiBodyConstraintSolver;
m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver,
m_collisionConfiguration);
m_dynamicsWorld->setGravity(m_gravity);
}
btBroadphaseInterface *m_broadphase;
btCollisionDispatcher *m_dispatcher;
btMultiBodyConstraintSolver *m_solver;
btDefaultCollisionConfiguration *m_collisionConfiguration;
btMultiBodyDynamicsWorld *m_dynamicsWorld;
std::string m_filename;
DummyGUIHelper m_nogfx;
btMultiBody *m_multibody;
const btVector3 m_gravity;
const bool m_base_fixed;
};
#endif // BTMULTIBODYFROMURDF_HPP

270
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp Normal file
View File

@@ -0,0 +1,270 @@
#include "btMultiBodyTreeCreator.hpp"
namespace btInverseDynamics {
btMultiBodyTreeCreator::btMultiBodyTreeCreator() : m_initialized(false) {}
int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) {
if (0x0 == btmb) {
error_message("cannot create MultiBodyTree from null pointer\n");
return -1;
}
// in case this is a second call, discard old data
m_data.clear();
m_initialized = false;
// btMultiBody treats base link separately
m_data.resize(1 + btmb->getNumLinks());
// add base link data
{
LinkData &link = m_data[0];
link.parent_index = -1;
if (btmb->hasFixedBase()) {
link.joint_type = FIXED;
} else {
link.joint_type = FLOATING;
}
btTransform transform(btmb->getBaseWorldTransform());
link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
link.parent_r_parent_body_ref(2) = transform.getOrigin()[2];
link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0];
link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1];
link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2];
link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0];
link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1];
link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2];
link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0];
link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1];
link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2];
// random unit vector. value not used for fixed or floating joints.
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
link.mass = btmb->getBaseMass();
// link frame in the center of mass
link.body_r_body_com(0) = 0;
link.body_r_body_com(1) = 0;
link.body_r_body_com(2) = 0;
// BulletDynamics uses body-fixed frame in the cog, aligned with principal axes
link.body_I_body(0, 0) = btmb->getBaseInertia()[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = btmb->getBaseInertia()[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = btmb->getBaseInertia()[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose) {
id_printf("base: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1],
btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
}
for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++) {
if (verbose) {
id_printf("bt->id: converting link %d\n", bt_index);
}
const btMultibodyLink &bt_link = btmb->getLink(bt_index);
LinkData &link = m_data[bt_index + 1];
link.parent_index = bt_link.m_parent + 1;
link.mass = bt_link.m_mass;
if (verbose) {
id_printf("mass= %f\n", link.mass);
}
// from this body's pivot to this body's com in this body's frame
link.body_r_body_com[0] = bt_link.m_dVector[0];
link.body_r_body_com[1] = bt_link.m_dVector[1];
link.body_r_body_com[2] = bt_link.m_dVector[2];
if (verbose) {
id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1],
link.body_r_body_com[2]);
}
// BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes
link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose) {
id_printf("link %d: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1],
bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
// transform for vectors written in parent frame to this link's body-fixed frame
btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis();
link.body_T_parent_ref(0, 0) = basis[0][0];
link.body_T_parent_ref(0, 1) = basis[0][1];
link.body_T_parent_ref(0, 2) = basis[0][2];
link.body_T_parent_ref(1, 0) = basis[1][0];
link.body_T_parent_ref(1, 1) = basis[1][1];
link.body_T_parent_ref(1, 2) = basis[1][2];
link.body_T_parent_ref(2, 0) = basis[2][0];
link.body_T_parent_ref(2, 1) = basis[2][1];
link.body_T_parent_ref(2, 2) = basis[2][2];
if (verbose) {
id_printf("body_T_parent_ref= %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2],
basis[2][0], basis[2][1], basis[2][2]);
}
switch (bt_link.m_jointType) {
case btMultibodyLink::eRevolute:
link.joint_type = REVOLUTE;
if (verbose) {
id_printf("type= revolute\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2];
// for revolute joints, m_eVector = parentComToThisPivotOffset
// m_dVector = thisPivotToThisComOffset
// from parent com to pivot, in parent frame
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::ePrismatic:
link.joint_type = PRISMATIC;
if (verbose) {
id_printf("type= prismatic\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2];
// for prismatic joints, eVector
// according to documentation :
// parentComToThisComOffset
// but seems to be: from parent's com to parent's
// pivot ??
// m_dVector = thisPivotToThisComOffset
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::eSpherical:
error_message("spherical joints not implemented\n");
return -1;
case btMultibodyLink::ePlanar:
error_message("planar joints not implemented\n");
return -1;
case btMultibodyLink::eFixed:
link.joint_type = FIXED;
// random unit vector
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
// for fixed joints, m_dVector = thisPivotToThisComOffset;
// m_eVector = parentComToThisPivotOffset;
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
default:
error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_link.m_jointType);
return -1;
}
if (link.parent_index > 0) { // parent body isn't the root
const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1);
// from parent pivot to parent com, in parent frame
link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0];
link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1];
link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2];
} else {
// parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies,
// whose link frame is in the CoM (ie, no notion of a pivot point)
}
if (verbose) {
id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0],
link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]);
}
}
m_initialized = true;
return 0;
}
int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const {
if (false == m_initialized) {
error_message("btMultiBody not converted yet\n");
return -1;
}
*num_bodies = static_cast<int>(m_data.size());
return 0;
}
int btMultiBodyTreeCreator::getBody(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 (false == m_initialized) {
error_message("MultiBodyTree not created yet\n");
return -1;
}
if (body_index < 0 || body_index >= static_cast<int>(m_data.size())) {
error_message("index out of range (got %d but only %zu bodies)\n", body_index,
m_data.size());
return -1;
}
*parent_index = m_data[body_index].parent_index;
*joint_type = m_data[body_index].joint_type;
*parent_r_parent_body_ref = m_data[body_index].parent_r_parent_body_ref;
*body_T_parent_ref = m_data[body_index].body_T_parent_ref;
*body_axis_of_motion = m_data[body_index].body_axis_of_motion;
*mass = m_data[body_index].mass;
*body_r_body_com = m_data[body_index].body_r_body_com;
*body_I_body = m_data[body_index].body_I_body;
*user_int = -1;
*user_ptr = 0x0;
return 0;
}
}

50
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp Normal file
View File

@@ -0,0 +1,50 @@
#ifndef BTMULTIBODYTREECREATOR_HPP_
#define BTMULTIBODYTREECREATOR_HPP_
#include <vector>
#include "BulletInverseDynamics/IDConfig.hpp"
#include "MultiBodyTreeCreator.hpp"
#include "BulletDynamics/Featherstone/btMultiBody.h"
namespace btInverseDynamics {
/// MultiBodyTreeCreator implementation for converting
/// a btMultiBody forward dynamics model into a MultiBodyTree inverse dynamics model
class btMultiBodyTreeCreator : public MultiBodyTreeCreator {
public:
/// ctor
btMultiBodyTreeCreator();
/// dtor
~btMultiBodyTreeCreator() {}
/// extract model data from a btMultiBody
/// @param btmb pointer to btMultiBody to convert
/// @param verbose if true, some information is printed
/// @return -1 on error, 0 on success
int createFromBtMultiBody(const btMultiBody *btmb, const bool verbose = false);
/// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int *num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(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
struct LinkData {
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;
};
std::vector<LinkData> m_data;
bool m_initialized;
};
}
#endif // BTMULTIBODYTREECREATOR_HPP_

307
Extras/InverseDynamics/invdyn_bullet_comparison.cpp Normal file
View File

@@ -0,0 +1,307 @@
#include "invdyn_bullet_comparison.hpp"
#include <cmath>
#include "BulletInverseDynamics/IDConfig.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
namespace btInverseDynamics {
int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error) {
// call function and return -1 if it does, printing an error_message
#define RETURN_ON_FAILURE(x) \
do { \
if (-1 == x) { \
error_message("calling " #x "\n"); \
return -1; \
} \
} while (0)
if (verbose) {
printf("\n ===================================== \n");
}
vecx joint_forces(q.size());
// set positions and velocities for btMultiBody
// base link
mat33 world_T_base;
vec3 world_pos_base;
btTransform base_transform;
vec3 base_velocity;
vec3 base_angular_velocity;
RETURN_ON_FAILURE(id_tree->setGravityInWorldFrame(gravity));
RETURN_ON_FAILURE(id_tree->getBodyOrigin(0, &world_pos_base));
RETURN_ON_FAILURE(id_tree->getBodyTransform(0, &world_T_base));
RETURN_ON_FAILURE(id_tree->getBodyAngularVelocity(0, &base_angular_velocity));
RETURN_ON_FAILURE(id_tree->getBodyLinearVelocityCoM(0, &base_velocity));
base_transform.setBasis(world_T_base);
base_transform.setOrigin(world_pos_base);
btmb->setBaseWorldTransform(base_transform);
btmb->setBaseOmega(base_angular_velocity);
btmb->setBaseVel(base_velocity);
btmb->setLinearDamping(0);
btmb->setAngularDamping(0);
// remaining links
int q_index;
if (btmb->hasFixedBase()) {
q_index = 0;
} else {
q_index = 6;
}
if (verbose) {
printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs());
}
for (int l = 0; l < btmb->getNumLinks(); l++) {
const btMultibodyLink &link = btmb->getLink(l);
if (verbose) {
printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount,
link.m_dofCount);
}
if (link.m_posVarCount == 1) {
btmb->setJointPosMultiDof(l, &q(q_index));
btmb->setJointVelMultiDof(l, &u(q_index));
if (verbose) {
printf("set q[%d]= %f, u[%d]= %f\n", q_index, q(q_index), q_index, u(q_index));
}
q_index++;
}
}
// sanity check
if (q_index != q.size()) {
error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// run inverse dynamics to determine joint_forces for given q, u, dot_u
if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces)) {
error_message("calculating inverse dynamics\n");
return -1;
}
// set up bullet forward dynamics model
btScalar dt = 0;
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
// this triggers switch between using either appliedConstraintForce or appliedForce
bool isConstraintPass = false;
// apply gravity forces for btMultiBody model. Must be done manually.
btmb->addBaseForce(btmb->getBaseMass() * gravity);
for (int link = 0; link < btmb->getNumLinks(); link++) {
btmb->addLinkForce(link, gravity * btmb->getLinkMass(link));
if (verbose) {
printf("link %d, applying gravity %f %f %f\n", link,
gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link),
gravity[2] * btmb->getLinkMass(link));
}
}
// apply generalized forces
if (btmb->hasFixedBase()) {
q_index = 0;
} else {
vec3 base_force;
base_force(0) = joint_forces(3);
base_force(1) = joint_forces(4);
base_force(2) = joint_forces(5);
vec3 base_moment;
base_moment(0) = joint_forces(0);
base_moment(1) = joint_forces(1);
base_moment(2) = joint_forces(2);
btmb->addBaseForce(world_T_base * base_force);
btmb->addBaseTorque(world_T_base * base_moment);
if (verbose) {
printf("base force from id: %f %f %f\n", joint_forces(3), joint_forces(4),
joint_forces(5));
printf("base moment from id: %f %f %f\n", joint_forces(0), joint_forces(1),
joint_forces(2));
}
q_index = 6;
}
for (int l = 0; l < btmb->getNumLinks(); l++) {
const btMultibodyLink &link = btmb->getLink(l);
if (link.m_posVarCount == 1) {
if (verbose) {
printf("id:joint_force[%d]= %f, applied to link %d\n", q_index,
joint_forces(q_index), l);
}
btmb->addJointTorque(l, joint_forces(q_index));
q_index++;
}
}
// sanity check
if (q_index != q.size()) {
error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
return -1;
}
// run forward kinematics & forward dynamics
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
btmb->forwardKinematics(world_to_local, local_origin);
btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
// read generalized accelerations back from btMultiBody
// the mapping from scratch variables to accelerations is taken from the implementation
// of stepVelocitiesMultiDof
btScalar *base_accel = &scratch_r[btmb->getNumDofs()];
btScalar *joint_accel = base_accel + 6;
*acc_error = 0;
int dot_u_offset = 0;
if (btmb->hasFixedBase()) {
dot_u_offset = 0;
} else {
dot_u_offset = 6;
}
if (true == btmb->hasFixedBase()) {
for (int i = 0; i < btmb->getNumDofs(); i++) {
if (verbose) {
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i));
}
*acc_error += std::pow(joint_accel[i] - dot_u(i + dot_u_offset), 2);
}
} else {
vec3 base_dot_omega;
vec3 world_dot_omega;
world_dot_omega(0) = base_accel[0];
world_dot_omega(1) = base_accel[1];
world_dot_omega(2) = base_accel[2];
base_dot_omega = world_T_base.transpose() * world_dot_omega;
// com happens to coincide with link origin here. If that changes, we need to calculate
// ddot_com
vec3 base_ddot_com;
vec3 world_ddot_com;
world_ddot_com(0) = base_accel[3];
world_ddot_com(1) = base_accel[4];
world_ddot_com(2) = base_accel[5];
base_ddot_com = world_T_base.transpose()*world_ddot_com;
for (int i = 0; i < 3; i++) {
if (verbose) {
printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i),
i, dot_u[i], base_dot_omega(i) - dot_u[i]);
}
*acc_error += std::pow(base_dot_omega(i) - dot_u(i), 2);
}
for (int i = 0; i < 3; i++) {
if (verbose) {
printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i),
i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]);
}
*acc_error += std::pow(base_ddot_com(i) - dot_u(i + 3), 2);
}
for (int i = 0; i < btmb->getNumDofs(); i++) {
if (verbose) {
printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
dot_u(i + 6), joint_accel[i] - dot_u(i + 6));
}
*acc_error += std::pow(joint_accel[i] - dot_u(i + 6), 2);
}
}
*acc_error = std::sqrt(*acc_error);
if (verbose) {
printf("======dynamics-err: %e\n", *acc_error);
}
*pos_error = 0.0;
{
mat33 world_T_body;
if (-1 == id_tree->getBodyTransform(0, &world_T_body)) {
error_message("getting transform for body %d\n", 0);
return -1;
}
vec3 world_com;
if (-1 == id_tree->getBodyCoM(0, &world_com)) {
error_message("getting com for body %d\n", 0);
return -1;
}
if (verbose) {
printf("id:com: %f %f %f\n", world_com(0), world_com(1), world_com(2));
printf("id:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
world_T_body(0, 0), world_T_body(0, 1), world_T_body(0, 2), world_T_body(1, 0),
world_T_body(1, 1), world_T_body(1, 2), world_T_body(2, 0), world_T_body(2, 1),
world_T_body(2, 2));
}
}
for (int l = 0; l < btmb->getNumLinks(); l++) {
const btMultibodyLink &bt_link = btmb->getLink(l);
vec3 bt_origin = bt_link.m_cachedWorldTransform.getOrigin();
mat33 bt_basis = bt_link.m_cachedWorldTransform.getBasis();
if (verbose) {
printf("------------- link %d\n", l + 1);
printf("bt:com: %f %f %f\n", bt_origin(0), bt_origin(1), bt_origin(2));
printf("bt:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
bt_basis(0, 0), bt_basis(0, 1), bt_basis(0, 2), bt_basis(1, 0), bt_basis(1, 1),
bt_basis(1, 2), bt_basis(2, 0), bt_basis(2, 1), bt_basis(2, 2));
}
mat33 id_world_T_body;
vec3 id_world_com;
if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body)) {
error_message("getting transform for body %d\n", l);
return -1;
}
if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com)) {
error_message("getting com for body %d\n", l);
return -1;
}
if (verbose) {
printf("id:com: %f %f %f\n", id_world_com(0), id_world_com(1), id_world_com(2));
printf("id:transform: %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
id_world_T_body(0, 0), id_world_T_body(0, 1), id_world_T_body(0, 2),
id_world_T_body(1, 0), id_world_T_body(1, 1), id_world_T_body(1, 2),
id_world_T_body(2, 0), id_world_T_body(2, 1), id_world_T_body(2, 2));
}
vec3 diff_com = bt_origin - id_world_com;
mat33 diff_basis = bt_basis - id_world_T_body;
if (verbose) {
printf("diff-com: %e %e %e\n", diff_com(0), diff_com(1), diff_com(2));
printf("diff-transform: %e %e %e %e %e %e %e %e %e\n", diff_basis(0, 0),
diff_basis(0, 1), diff_basis(0, 2), diff_basis(1, 0), diff_basis(1, 1),
diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2));
}
double total_pos_err =
std::sqrt(std::pow(diff_com(0), 2) + std::pow(diff_com(1), 2) +
std::pow(diff_com(2), 2) + std::pow(diff_basis(0, 0), 2) +
std::pow(diff_basis(0, 1), 2) + std::pow(diff_basis(0, 2), 2) +
std::pow(diff_basis(1, 0), 2) + std::pow(diff_basis(1, 1), 2) +
std::pow(diff_basis(1, 2), 2) + std::pow(diff_basis(2, 0), 2) +
std::pow(diff_basis(2, 1), 2) + std::pow(diff_basis(2, 2), 2));
if (verbose) {
printf("======kin-pos-err: %e\n", total_pos_err);
}
if (total_pos_err > *pos_error) {
*pos_error = total_pos_err;
}
}
return 0;
}
}

34
Extras/InverseDynamics/invdyn_bullet_comparison.hpp Normal file
View File

@@ -0,0 +1,34 @@
#ifndef INVDYN_BULLET_COMPARISON_HPP
#define INVDYN_BULLET_COMPARISON_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
/// 1. run inverse dynamics for (q, u, dot_u) to obtain joint forces f
/// 2. run forward dynamics (btMultiBody) for (q,u,f) to obtain dot_u_bullet
/// 3. compare dot_u with dot_u_bullet for cross check of forward and inverse dynamics computations
/// @param btmb the bullet forward dynamics model
/// @param id_tree the inverse dynamics model
/// @param q vector of generalized coordinates (matches id_tree)
/// @param u vector of generalized speeds (matches id_tree)
/// @param gravity gravitational acceleration in world frame
/// @param dot_u vector of generalized accelerations (matches id_tree)
/// @param gravity gravitational acceleration in world frame
/// @param base_fixed set base joint to fixed or
/// @param pos_error is set to the maximum of the euclidean norm of position+rotation errors of all
/// center of gravity positions and link frames
/// @param acc_error is set to the square root of the sum of squared differences of generalized
/// accelerations
/// computed in step 3 relative to dot_u
/// @return -1 on error, 0 on success
int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
double *acc_error);
}
#endif // INVDYN_BULLET_COMPARISON_HPP

12
Extras/InverseDynamics/premake4.lua Normal file
View File

@@ -0,0 +1,12 @@
project "BulletInverseDynamicsUtils"
kind "StaticLib"
includedirs {
"../../src"
}
files {
"*.cpp",
"*.hpp"
}