Merge branch 'master' of https://github.com/erwincoumans/bullet3

Extras/InverseDynamics/CoilCreator.cpp

@@ -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 * BT_ID_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 / BT_ID_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;
+}
+}

Extras/InverseDynamics/CoilCreator.hpp

@@ -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

Extras/InverseDynamics/DillCreator.cpp

@@ -0,0 +1,124 @@
+#include "DillCreator.hpp"
+#include <cmath>
+namespace btInverseDynamics {
+
+DillCreator::DillCreator(int level)
+    : m_level(level),
+      m_num_bodies(BT_ID_POW(2, level))
+     {
+		  m_parent.resize(m_num_bodies);
+		m_parent_r_parent_body_ref.resize(m_num_bodies);
+		m_body_T_parent_ref.resize(m_num_bodies);
+		m_body_axis_of_motion.resize(m_num_bodies);
+		m_mass.resize(m_num_bodies);
+		m_body_r_body_com.resize(m_num_bodies);
+		m_body_I_body.resize(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 * BT_ID_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 * BT_ID_PI / 3.0;
+        m_current_body++;
+        recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
+    }
+
+    return 0;  // ok!
+}
+}

Extras/InverseDynamics/DillCreator.hpp

@@ -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;
+    idArray<int>::type m_parent;
+    idArray<vec3>::type m_parent_r_parent_body_ref;
+    idArray<mat33>::type m_body_T_parent_ref;
+    idArray<vec3>::type m_body_axis_of_motion;
+    idArray<idScalar>::type m_mass;
+    idArray<vec3>::type m_body_r_body_com;
+    idArray<mat33>::type m_body_I_body;
+    int m_current_body;
+};
+}
+#endif

Extras/InverseDynamics/IDRandomUtil.cpp

@@ -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(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) *
+              transformZ(randomFloat(-BT_ID_PI, BT_ID_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;
+}
+}

Extras/InverseDynamics/IDRandomUtil.hpp

@@ -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

Extras/InverseDynamics/MultiBodyNameMap.cpp

@@ -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;
+}
+}

Extras/InverseDynamics/MultiBodyNameMap.hpp

@@ -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_

Extras/InverseDynamics/MultiBodyTreeCreator.cpp

@@ -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;
+}
+}

Extras/InverseDynamics/MultiBodyTreeCreator.hpp

@@ -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_

Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp

@@ -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;
+}
+}

Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp

@@ -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

Extras/InverseDynamics/SimpleTreeCreator.cpp

@@ -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;
+}
+}

Extras/InverseDynamics/SimpleTreeCreator.hpp

@@ -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_

Extras/InverseDynamics/User2InternalIndex.cpp

@@ -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;
+    }
+}
+}

Extras/InverseDynamics/User2InternalIndex.hpp

@@ -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

Extras/InverseDynamics/btMultiBodyFromURDF.hpp

@@ -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

Extras/InverseDynamics/btMultiBodyTreeCreator.cpp

@@ -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;
+}
+}

Extras/InverseDynamics/btMultiBodyTreeCreator.hpp

@@ -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;
+    };
+    idArray<LinkData>::type m_data;
+    bool m_initialized;
+};
+}
+
+#endif  // BTMULTIBODYTREECREATOR_HPP_

Extras/InverseDynamics/invdyn_bullet_comparison.cpp

@@ -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;
+}
+}

Extras/InverseDynamics/invdyn_bullet_comparison.hpp

@@ -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

Extras/InverseDynamics/premake4.lua

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

Extras/premake4.lua

@@ -1,7 +1,7 @@
 
 include "HACD"
 include "ConvexDecomposition"
-
+include "InverseDynamics"
 include "Serialize/BulletFileLoader"
 include "Serialize/BulletWorldImporter"
 include "Serialize/BulletXmlWorldImporter"

build3/premake4.lua

@@ -189,12 +189,14 @@ if not _OPTIONS["ios"] then
                 include "../test/gtest-1.7.0"
 --              include "../test/hello_gtest"
                 include "../test/collision"
+		include "../test/InverseDynamics"
                 include "../test/TestBullet3OpenCL"
                 include "../test/GwenOpenGLTest"
         end
 end
 
 	include "../test/Bullet2"
+	include "../src/BulletInverseDynamics"
  	include "../src/BulletSoftBody"
         include "../src/BulletDynamics"
         include "../src/BulletCollision"

examples/CommonInterfaces/CommonMultiBodyBase.h

@@ -118,14 +118,19 @@ struct CommonMultiBodyBase : public CommonExampleInterface
 		m_collisionShapes.clear();
 
 		delete m_dynamicsWorld;
+		m_dynamicsWorld = 0;
 
 		delete m_solver;
+		m_solver=0;
 
 		delete m_broadphase;
+		m_broadphase=0;
 
 		delete m_dispatcher;
+		m_dispatcher=0;
 
 		delete m_collisionConfiguration;
+		m_collisionConfiguration=0;
 	}
 
 	virtual void syncPhysicsToGraphics()

examples/ExampleBrowser/ExampleEntries.cpp

@@ -40,6 +40,8 @@
 #include "../Tutorial/Tutorial.h"
 #include "../Tutorial/Dof6ConstraintTutorial.h"
 #include "../MultiThreading/MultiThreadingExample.h"
+//#include "../InverseDynamics/InverseDynamicsExample.h"
+
 #ifdef ENABLE_LUA
 #include "../LuaDemo/LuaPhysicsSetup.h"
 #endif
@@ -108,6 +110,9 @@ static ExampleEntry gDefaultExamples[]=
  ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 	ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
 
+//	ExampleEntry(0,"Inverse Dynamics"),
+//	ExampleEntry(1,"Inverse Dynamics URDF", "write some explanation", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
+//	ExampleEntry(1,"Inverse Dynamics Prog", "write some explanation2", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
 	
 	ExampleEntry(0,"Tutorial"),
 	ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY),

examples/ExampleBrowser/premake4.lua

@@ -76,6 +76,8 @@
 		"../MultiThreading/b3PosixThreadSupport.cpp",
 		"../MultiThreading/b3Win32ThreadSupport.cpp",
 		"../MultiThreading/b3ThreadSupportInterface.cpp",
+		"../InverseDynamics/InverseDynamicsExample.cpp",
+		"../InverseDynamics/InverseDynamicsExample.h",
 		"../BasicDemo/BasicExample.*",
 		"../Tutorial/*",
 		"../Collision/*",

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -21,6 +21,7 @@ struct GenericConstraintUserInfo
 
 class MyMultiBodyCreator : public MultiBodyCreationInterface
 {
+protected:
 
 	btMultiBody* m_bulletMultiBody;
     

examples/InverseDynamics/InverseDynamicsExample.cpp

@@ -0,0 +1,167 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "InverseDynamicsExample.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "btBulletDynamicsCommon.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+//those are static global to make it easy for the GUI to tweak them
+static btScalar radius(0.2);
+static btScalar kp = 100;
+static btScalar kd = 20;
+static btScalar maxForce = 100;
+
+class InverseDynamicsExample : public CommonMultiBodyBase
+{
+	btInverseDynamicsExampleOptions m_option;
+	btMultiBody* m_multiBody;
+public:
+    InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option);
+    virtual ~InverseDynamicsExample();
+
+	virtual void initPhysics();
+	virtual void stepSimulation(float deltaTime);
+
+    void setFileName(const char* urdfFileName);
+
+	virtual void resetCamera()
+	{
+		float dist = 3.5;
+		float pitch = -136;
+		float yaw = 28;
+		float targetPos[3]={0.47,0,-0.64};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+};
+
+InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option)
+	:CommonMultiBodyBase(helper),
+	m_option(option),
+	m_multiBody(0)
+{
+}
+
+InverseDynamicsExample::~InverseDynamicsExample()
+{
+	
+}
+
+//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this.
+btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans);
+
+void InverseDynamicsExample::initPhysics()
+{
+	//roboticists like Z up
+	int upAxis = 2;
+	m_guiHelper->setUpAxis(upAxis);
+
+	createEmptyDynamicsWorld();
+	btVector3 gravity(0,0,0);
+	gravity[upAxis]=-9.8;
+	m_dynamicsWorld->setGravity(gravity);
+
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	{
+		SliderParams slider("Kp",&kp);
+		slider.m_minVal=-200;
+		slider.m_maxVal=200;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+	{
+		SliderParams slider("Kd",&kd);
+		slider.m_minVal=-50;
+		slider.m_maxVal=50;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+	{
+		SliderParams slider("max force",&maxForce);
+		slider.m_minVal=0;
+		slider.m_maxVal=100;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+
+	switch (m_option)
+	{
+	case BT_ID_LOAD_URDF:
+		{
+			BulletURDFImporter u2b(m_guiHelper);
+			bool loadOk =  u2b.loadURDF("kuka_lwr/kuka.urdf");
+			if (loadOk)
+			{
+					int rootLinkIndex = u2b.getRootLinkIndex();
+					b3Printf("urdf root link index = %d\n",rootLinkIndex);
+					MyMultiBodyCreator creation(m_guiHelper);
+					btTransform identityTrans;
+					identityTrans.setIdentity();
+					ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix());
+					m_multiBody = creation.getBulletMultiBody();
+					if (m_multiBody)
+					{
+						//kuka without joint control/constraints will gain energy explode soon due to timestep/integrator
+						//temporarily set some extreme damping factors until we have some joint control or constraints
+						m_multiBody->setAngularDamping(0.99);
+						m_multiBody->setLinearDamping(0.99);
+						b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
+					}
+			}
+			break;
+		}
+	case BT_ID_PROGRAMMATICALLY:
+		{
+			btTransform baseWorldTrans;
+			baseWorldTrans.setIdentity();
+			m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans);
+			break;
+		}
+	default:
+		{
+			b3Error("Unknown option in InverseDynamicsExample::initPhysics");
+			b3Assert(0);
+		}
+	};
+}
+
+
+void InverseDynamicsExample::stepSimulation(float deltaTime)
+{
+	if (m_dynamicsWorld)
+	{
+		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+	}
+}
+
+CommonExampleInterface*    InverseDynamicsExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new InverseDynamicsExample(options.m_guiHelper, btInverseDynamicsExampleOptions(options.m_option));
+}
+
+
+

examples/InverseDynamics/InverseDynamicsExample.h

@@ -0,0 +1,28 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef INVERSE_DYNAMICS_EXAMPLE_H
+#define INVERSE_DYNAMICS_EXAMPLE_H
+
+enum btInverseDynamicsExampleOptions
+{
+	BT_ID_LOAD_URDF=1,
+	BT_ID_PROGRAMMATICALLY=2
+};
+
+class CommonExampleInterface*    InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //INVERSE_DYNAMICS_EXAMPLE_H

examples/MultiBody/InvertedPendulumPDControl.cpp

@@ -55,38 +55,8 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl()
 extern  bool gJointFeedbackInWorldSpace;
 extern bool gJointFeedbackInJointFrame;
 
-
+btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans)
-
-void InvertedPendulumPDControl::initPhysics()
 {
-	{
-		SliderParams slider("Kp",&kp);
-		slider.m_minVal=-200;
-		slider.m_maxVal=200;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
-    }
-	{
-		SliderParams slider("Kd",&kd);
-		slider.m_minVal=-50;
-		slider.m_maxVal=50;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
-    }
-	{
-		SliderParams slider("max force",&maxForce);
-		slider.m_minVal=0;
-		slider.m_maxVal=100;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
-    }
-
-	
-
-
-    int upAxis = 1;
-	gJointFeedbackInWorldSpace = true;
-	gJointFeedbackInJointFrame = true;
-
-	m_guiHelper->setUpAxis(upAxis);
-
 	btVector4 colors[4] =
     {
         btVector4(1,0,0,1),
@@ -96,22 +66,6 @@ void InvertedPendulumPDControl::initPhysics()
     };
     int curColor = 0;
 
-
-
-    
-
-	this->createEmptyDynamicsWorld();
-    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
-        //btIDebugDraw::DBG_DrawConstraints
-        +btIDebugDraw::DBG_DrawWireframe
-        +btIDebugDraw::DBG_DrawContactPoints
-        +btIDebugDraw::DBG_DrawAabb
-        );//+btIDebugDraw::DBG_DrawConstraintLimits);
-
-	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
-
-    {
 	bool fixedBase = true;
     bool damping = false;
     bool gyro = false;
@@ -122,7 +76,7 @@ void InvertedPendulumPDControl::initPhysics()
     btVector3 linkHalfExtents(0.05, 0.37, 0.1);
     btVector3 baseHalfExtents(0.04, 0.35, 0.08);
 
-        btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
+    
     //mbC->forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
     //init the base
     btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
@@ -139,13 +93,11 @@ void InvertedPendulumPDControl::initPhysics()
         
     btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
 		
-        m_multiBody = pMultiBody;
+        
-        btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+    
-	//	baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
+	pMultiBody->setBaseWorldTransform(baseWorldTrans);
-        pMultiBody->setBasePos(basePosition);
-        pMultiBody->setWorldToBaseRot(baseOriQuat);
     btVector3 vel(0, 0, 0);
-    //	pMultiBody->setBaseVel(vel);
+//	pMultiBody->setBaseVel(vel);
 
     //init the links
     btVector3 hingeJointAxis(1, 0, 0);
@@ -216,15 +168,7 @@ void InvertedPendulumPDControl::initPhysics()
 
     pMultiBody->finalizeMultiDof();
 
-		//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
+		
-			for (int i=0;i<pMultiBody->getNumLinks();i++)
-		{
-			btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
-			pMultiBody->getLink(i).m_jointFeedback = fb;
-			m_jointFeedbacks.push_back(fb);
-			//break;
-		}
-        btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
 
     ///
     world->addMultiBody(pMultiBody);
@@ -278,7 +222,7 @@ void InvertedPendulumPDControl::initPhysics()
         if (1)
         {
             btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
-                m_guiHelper->createCollisionShapeGraphicsObject(shape);
+            guiHelper->createCollisionShapeGraphicsObject(shape);
 
             btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
             col->setCollisionShape(shape);
@@ -302,7 +246,7 @@ void InvertedPendulumPDControl::initPhysics()
             world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
 
             btVector3 color(0.0,0.0,0.5);
-                m_guiHelper->createCollisionObjectGraphicsObject(col,color);
+            guiHelper->createCollisionObjectGraphicsObject(col,color);
 
 //                col->setFriction(friction);
             pMultiBody->setBaseCollider(col);
@@ -337,7 +281,7 @@ void InvertedPendulumPDControl::initPhysics()
 			shape = new btSphereShape(radius);
 		}
 
-            m_guiHelper->createCollisionShapeGraphicsObject(shape);
+        guiHelper->createCollisionShapeGraphicsObject(shape);
         btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
 
         col->setCollisionShape(shape);
@@ -357,13 +301,70 @@ void InvertedPendulumPDControl::initPhysics()
 				btVector4 color = colors[curColor];
 		curColor++;
 		curColor&=3;
-            m_guiHelper->createCollisionObjectGraphicsObject(col,color);
+        guiHelper->createCollisionObjectGraphicsObject(col,color);
 
 
         pMultiBody->getLink(i).m_collider=col;
 		}
          
     }
+    
+	return pMultiBody;
+}
+
+void InvertedPendulumPDControl::initPhysics()
+{
+	{
+		SliderParams slider("Kp",&kp);
+		slider.m_minVal=-200;
+		slider.m_maxVal=200;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+	{
+		SliderParams slider("Kd",&kd);
+		slider.m_minVal=-50;
+		slider.m_maxVal=50;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+	{
+		SliderParams slider("max force",&maxForce);
+		slider.m_minVal=0;
+		slider.m_maxVal=100;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+
+	
+
+
+    int upAxis = 1;
+	gJointFeedbackInWorldSpace = true;
+	gJointFeedbackInJointFrame = true;
+
+	m_guiHelper->setUpAxis(upAxis);
+
+    
+	this->createEmptyDynamicsWorld();
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+        //btIDebugDraw::DBG_DrawConstraints
+        +btIDebugDraw::DBG_DrawWireframe
+        +btIDebugDraw::DBG_DrawContactPoints
+        +btIDebugDraw::DBG_DrawAabb
+        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
+	btTransform baseWorldTrans;
+	baseWorldTrans.setIdentity();
+	baseWorldTrans.setOrigin(btVector3(1,2,3));
+	m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans);
+
+	//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
+	for (int i=0;i<m_multiBody->getNumLinks();i++)
+	{
+		btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
+		m_multiBody->getLink(i).m_jointFeedback = fb;
+		m_jointFeedbacks.push_back(fb);
+		//break;
 	}
         
 	

src/BulletInverseDynamics/IDConfig.hpp

@@ -0,0 +1,59 @@
+///@file Configuration for Inverse Dynamics Library,
+///	  such as choice of linear algebra library and underlying scalar type
+#ifndef IDCONFIG_HPP_
+#define IDCONFIG_HPP_
+// If we have a custom configuration, compile without using other parts of bullet.
+#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
+#define BT_ID_WO_BULLET
+#define BT_ID_POW(a,b) std::pow(a,b)
+#define BT_ID_SNPRINTF snprintf
+#define BT_ID_PI M_PI
+#define BT_ID_USE_DOUBLE_PRECISION
+#else
+#define BT_ID_POW(a,b) btPow(a,b)
+#define BT_ID_PI SIMD_PI
+#ifdef _WIN32
+	#define BT_ID_SNPRINTF _snprintf
+#else
+	#define BT_ID_SNPRINTF snprintf
+#endif //
+#endif
+// error messages
+#include "IDErrorMessages.hpp"
+
+#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
+#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 
+#error "custom inverse dynamics config, but no custom namespace defined"
+#endif
+#else
+#define btInverseDynamics btInverseDynamicsBullet3
+// Use default configuration with bullet's types
+// Use the same scalar type as rest of bullet library
+#include "LinearMath/btScalar.h"
+typedef btScalar idScalar;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define BT_ID_USE_DOUBLE_PRECISION
+#endif
+// use bullet types for arrays and array indices
+#include "Bullet3Common/b3AlignedObjectArray.h"
+// this is to make it work with C++2003, otherwise we could do this:
+// template <typename T>
+// using idArray = b3AlignedObjectArray<T>;
+template <typename T>
+struct idArray {
+	typedef b3AlignedObjectArray<T> type;
+};
+typedef int idArrayIdx;
+#define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR
+
+// use bullet's allocator functions
+#define idMalloc btAllocFunc
+#define idFree btFreeFunc
+
+#define ID_LINEAR_MATH_USE_BULLET
+#include "details/IDLinearMathInterface.hpp"
+#endif
+#endif

src/BulletInverseDynamics/IDConfigBuiltin.hpp

@@ -0,0 +1,37 @@
+///@file Configuration for Inverse Dynamics Library without external dependencies
+#ifndef INVDYNCONFIG_BUILTIN_HPP_
+#define INVDYNCONFIG_BUILTIN_HPP_
+#define btInverseDynamics btInverseDynamicsBuiltin
+#ifdef BT_USE_DOUBLE_PRECISION
+// choose double/single precision version
+typedef double idScalar;
+#else
+typedef float idScalar;
+#endif
+// use std::vector for arrays
+#include <vector>
+// this is to make it work with C++2003, otherwise we could do this
+// template <typename T>
+// using idArray = std::vector<T>;
+template <typename T>
+struct idArray {
+	typedef std::vector<T> type;
+};
+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*) {}
+
+#include "details/IDMatVec.hpp"
+#endif

src/BulletInverseDynamics/IDConfigEigen.hpp

@@ -0,0 +1,44 @@
+///@file Configuration for Inverse Dynamics Library with Eigen
+#ifndef INVDYNCONFIG_EIGEN_HPP_
+#define INVDYNCONFIG_EIGEN_HPP_
+#define btInverseDynamics btInverseDynamicsEigen
+#ifdef BT_USE_DOUBLE_PRECISION
+// choose double/single precision version
+typedef double idScalar;
+#else
+typedef float idScalar;
+#endif
+
+// use std::vector for arrays
+#include <vector>
+// this is to make it work with C++2003, otherwise we could do this
+// template <typename T>
+// using idArray = std::vector<T>;
+template <typename T>
+struct idArray {
+	typedef std::vector<T> type;
+};
+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*) {}
+
+// Note on interfaces:
+// Eigen::Matrix has data(), to get c-array storage
+// HOWEVER: default storage is column-major!
+#define ID_LINEAR_MATH_USE_EIGEN
+#include "Eigen/Eigen"
+#include "details/IDEigenInterface.hpp"
+#endif

src/BulletInverseDynamics/IDErrorMessages.hpp

@@ -0,0 +1,34 @@
+///@file error message utility functions
+#ifndef IDUTILS_HPP_
+#define IDUTILS_HPP_
+#include <cstring>
+/// name of file being compiled, without leading path components
+#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
+
+#ifndef BT_ID_WO_BULLET
+#include "Bullet3Common/b3Logging.h"
+#define error_message(...) b3Error(__VA_ARGS__)
+#define warning_message(...) b3Warning(__VA_ARGS__)
+#define id_printf(...) b3Printf(__VA_ARGS__)
+#else  // BT_ID_WO_BULLET
+#include <cstdio>
+/// print error message with file/line information
+#define error_message(...)																		 \
+	do {																						   \
+		fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);					   \
+		fprintf(stderr, __VA_ARGS__);															  \
+	} while (0)
+/// print warning message with file/line information
+#define warning_message(...)																	   \
+	do {																						   \
+		fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);					 \
+		fprintf(stderr, __VA_ARGS__);															  \
+	} while (0)
+#define warning_message(...)																		 \
+	do {																						   \
+		fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);					   \
+		fprintf(stderr, __VA_ARGS__);															\
+	} while (0)
+#define id_printf(...) printf(__VA_ARGS__)
+#endif  // BT_ID_WO_BULLET
+#endif

src/BulletInverseDynamics/IDMath.cpp

@@ -0,0 +1,371 @@
+#include "IDMath.hpp"
+
+#include <cmath>
+#include <limits>
+
+namespace btInverseDynamics {
+static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
+// requirements for axis length deviation from 1.0
+// experimentally set from random euler angle rotation matrices
+static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
+
+void setZero(vec3 &v) {
+	v(0) = 0;
+	v(1) = 0;
+	v(2) = 0;
+}
+
+void setZero(vecx &v) {
+	for (int i = 0; i < v.size(); i++) {
+		v(i) = 0;
+	}
+}
+
+void setZero(mat33 &m) {
+	m(0, 0) = 0;
+	m(0, 1) = 0;
+	m(0, 2) = 0;
+	m(1, 0) = 0;
+	m(1, 1) = 0;
+	m(1, 2) = 0;
+	m(2, 0) = 0;
+	m(2, 1) = 0;
+	m(2, 2) = 0;
+}
+
+idScalar maxAbs(const vecx &v) {
+	idScalar result = 0.0;
+	for (int i = 0; i < v.size(); i++) {
+		const idScalar tmp = std::fabs(v(i));
+		if (tmp > result) {
+			result = tmp;
+		}
+	}
+	return result;
+}
+
+idScalar maxAbs(const vec3 &v) {
+	idScalar result = 0.0;
+	for (int i = 0; i < 3; i++) {
+		const idScalar tmp = std::fabs(v(i));
+		if (tmp > result) {
+			result = tmp;
+		}
+	}
+	return result;
+}
+
+mat33 transformX(const idScalar &alpha) {
+	mat33 T;
+	const idScalar cos_alpha = std::cos(alpha);
+	const idScalar sin_alpha = std::sin(alpha);
+	// [1  0 0]
+	// [0  c s]
+	// [0 -s c]
+	T(0, 0) = 1.0;
+	T(0, 1) = 0.0;
+	T(0, 2) = 0.0;
+
+	T(1, 0) = 0.0;
+	T(1, 1) = cos_alpha;
+	T(1, 2) = sin_alpha;
+
+	T(2, 0) = 0.0;
+	T(2, 1) = -sin_alpha;
+	T(2, 2) = cos_alpha;
+
+	return T;
+}
+
+mat33 transformY(const idScalar &beta) {
+	mat33 T;
+	const idScalar cos_beta = std::cos(beta);
+	const idScalar sin_beta = std::sin(beta);
+	// [c 0 -s]
+	// [0 1  0]
+	// [s 0  c]
+	T(0, 0) = cos_beta;
+	T(0, 1) = 0.0;
+	T(0, 2) = -sin_beta;
+
+	T(1, 0) = 0.0;
+	T(1, 1) = 1.0;
+	T(1, 2) = 0.0;
+
+	T(2, 0) = sin_beta;
+	T(2, 1) = 0.0;
+	T(2, 2) = cos_beta;
+
+	return T;
+}
+
+mat33 transformZ(const idScalar &gamma) {
+	mat33 T;
+	const idScalar cos_gamma = std::cos(gamma);
+	const idScalar sin_gamma = std::sin(gamma);
+	// [ c s 0]
+	// [-s c 0]
+	// [ 0 0 1]
+	T(0, 0) = cos_gamma;
+	T(0, 1) = sin_gamma;
+	T(0, 2) = 0.0;
+
+	T(1, 0) = -sin_gamma;
+	T(1, 1) = cos_gamma;
+	T(1, 2) = 0.0;
+
+	T(2, 0) = 0.0;
+	T(2, 1) = 0.0;
+	T(2, 2) = 1.0;
+
+	return T;
+}
+
+mat33 tildeOperator(const vec3 &v) {
+	mat33 m;
+	m(0, 0) = 0.0;
+	m(0, 1) = -v(2);
+	m(0, 2) = v(1);
+	m(1, 0) = v(2);
+	m(1, 1) = 0.0;
+	m(1, 2) = -v(0);
+	m(2, 0) = -v(1);
+	m(2, 1) = v(0);
+	m(2, 2) = 0.0;
+	return m;
+}
+
+void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) {
+	const idScalar sa = std::sin(alpha);
+	const idScalar ca = std::cos(alpha);
+	const idScalar st = std::sin(theta);
+	const idScalar ct = std::cos(theta);
+
+	(*r)(0) = a;
+	(*r)(1) = -sa * d;
+	(*r)(2) = ca * d;
+
+	(*T)(0, 0) = ct;
+	(*T)(0, 1) = -st;
+	(*T)(0, 2) = 0.0;
+
+	(*T)(1, 0) = st * ca;
+	(*T)(1, 1) = ct * ca;
+	(*T)(1, 2) = -sa;
+
+	(*T)(2, 0) = st * sa;
+	(*T)(2, 1) = ct * sa;
+	(*T)(2, 2) = ca;
+}
+
+void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
+	const idScalar c = cos(angle);
+	const idScalar s = -sin(angle);
+	const idScalar one_m_c = 1.0 - c;
+
+	const idScalar &x = axis(0);
+	const idScalar &y = axis(1);
+	const idScalar &z = axis(2);
+
+	(*T)(0, 0) = x * x * one_m_c + c;
+	(*T)(0, 1) = x * y * one_m_c - z * s;
+	(*T)(0, 2) = x * z * one_m_c + y * s;
+
+	(*T)(1, 0) = x * y * one_m_c + z * s;
+	(*T)(1, 1) = y * y * one_m_c + c;
+	(*T)(1, 2) = y * z * one_m_c - x * s;
+
+	(*T)(2, 0) = x * z * one_m_c - y * s;
+	(*T)(2, 1) = y * z * one_m_c + x * s;
+	(*T)(2, 2) = z * z * one_m_c + c;
+}
+
+bool isPositiveDefinite(const mat33 &m) {
+	// test if all upper left determinants are positive
+	if (m(0, 0) <= 0) {  // upper 1x1
+		return false;
+	}
+	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) {  // upper 2x2
+		return false;
+	}
+	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
+		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
+		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
+		return false;
+	}
+	return true;
+}
+
+bool isPositiveSemiDefinite(const mat33 &m) {
+	// test if all upper left determinants are positive
+	if (m(0, 0) < 0) {  // upper 1x1
+		return false;
+	}
+	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) {  // upper 2x2
+		return false;
+	}
+	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
+		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
+		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
+		return false;
+	}
+	return true;
+}
+
+bool isPositiveSemiDefiniteFuzzy(const mat33 &m) {
+	// test if all upper left determinants are positive
+	if (m(0, 0) < -kIsZero) {  // upper 1x1
+		return false;
+	}
+	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) {  // upper 2x2
+		return false;
+	}
+	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
+		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
+		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) {
+		return false;
+	}
+	return true;
+}
+
+idScalar determinant(const mat33 &m) {
+	return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) -
+		   m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2);
+}
+
+bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) {
+	// TODO(Thomas) do we really want this?
+	//			  in cases where the inertia tensor about the center of mass is zero,
+	//			  the determinant of the inertia tensor about the joint axis is almost
+	//			  zero and can have a very small negative value.
+	if (!isPositiveSemiDefiniteFuzzy(I)) {
+		error_message("invalid inertia matrix for body %d, not positive definite "
+					  "(fixed joint)\n",
+					  index);
+		error_message("matrix is:\n"
+					  "[%.20e %.20e %.20e;\n"
+					  "%.20e %.20e %.20e;\n"
+					  "%.20e %.20e %.20e]\n",
+					  I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
+					  I(2, 2));
+
+		return false;
+	}
+
+	// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
+	if (!has_fixed_joint) {
+		if (I(0, 0) + I(1, 1) < I(2, 2)) {
+			error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+			error_message("matrix is:\n"
+						  "[%.20e %.20e %.20e;\n"
+						  "%.20e %.20e %.20e;\n"
+						  "%.20e %.20e %.20e]\n",
+						  I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
+						  I(2, 2));
+			return false;
+		}
+		if (I(0, 0) + I(1, 1) < I(2, 2)) {
+			error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+			error_message("matrix is:\n"
+						  "[%.20e %.20e %.20e;\n"
+						  "%.20e %.20e %.20e;\n"
+						  "%.20e %.20e %.20e]\n",
+						  I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
+						  I(2, 2));
+			return false;
+		}
+		if (I(1, 1) + I(2, 2) < I(0, 0)) {
+			error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
+			error_message("matrix is:\n"
+						  "[%.20e %.20e %.20e;\n"
+						  "%.20e %.20e %.20e;\n"
+						  "%.20e %.20e %.20e]\n",
+						  I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
+						  I(2, 2));
+			return false;
+		}
+	}
+	// check positive/zero diagonal elements
+	for (int i = 0; i < 3; i++) {
+		if (I(i, i) < 0) {  // accept zero
+			error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
+			return false;
+		}
+	}
+	// check symmetry
+	if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) {
+		error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
+					  "%e\n",
+					  index, I(1, 0) - I(0, 1));
+		return false;
+	}
+	if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) {
+		error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
+					  "%e\n",
+					  index, I(2, 0) - I(0, 2));
+		return false;
+	}
+	if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) {
+		error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
+					  I(1, 2) - I(2, 1));
+		return false;
+	}
+	return true;
+}
+
+bool isValidTransformMatrix(const mat33 &m) {
+#define print_mat(x)																			   \
+	error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2),   \
+				  x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
+
+	// check for unit length column vectors
+	for (int i = 0; i < 3; i++) {
+		const idScalar length_minus_1 =
+			std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
+		if (length_minus_1 > kAxisLengthEpsilon) {
+			error_message("Not a valid rotation matrix (column %d not unit length)\n"
+						  "column = [%.18e %.18e %.18e]\n"
+						  "length-1.0= %.18e\n",
+						  i, m(0, i), m(1, i), m(2, i), length_minus_1);
+			print_mat(m);
+			return false;
+		}
+	}
+	// check for orthogonal column vectors
+	if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
+		error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
+		print_mat(m);
+		return false;
+	}
+	if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
+		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+		print_mat(m);
+		return false;
+	}
+	if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
+		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+		print_mat(m);
+		return false;
+	}
+	// check determinant (rotation not reflection)
+	if (determinant(m) <= 0) {
+		error_message("Not a valid rotation matrix (determinant <=0)\n");
+		print_mat(m);
+		return false;
+	}
+	return true;
+}
+
+bool isUnitVector(const vec3 &vector) {
+	return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
+		   kIsZero;
+}
+
+vec3 rpyFromMatrix(const mat33 &rot) {
+	vec3 rpy;
+	rpy(2) = std::atan2(-rot(1, 0), rot(0, 0));
+	rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0));
+	rpy(0) = std::atan2(-rot(2, 0), rot(2, 2));
+	return rpy;
+}
+}

src/BulletInverseDynamics/IDMath.hpp

@@ -0,0 +1,88 @@
+/// @file Math utility functions used in inverse dynamics library.
+///	   Defined here as they may not be provided by the math library.
+
+#ifndef IDMATH_HPP_
+#define IDMATH_HPP_
+#include "IDConfig.hpp"
+
+namespace btInverseDynamics {
+/// set all elements to zero
+void setZero(vec3& v);
+/// set all elements to zero
+void setZero(vecx& v);
+/// set all elements to zero
+void setZero(mat33& m);
+
+/// return maximum absolute value
+idScalar maxAbs(const vecx& v);
+#ifndef ID_LINEAR_MATH_USE_EIGEN
+/// return maximum absolute value
+idScalar maxAbs(const vec3& v);
+#endif  //ID_LINEAR_MATH_USE_EIGEN
+/// 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);
+
+/// Check if a 3x3 matrix is positive definite
+/// @param m a 3x3 matrix
+/// @return true if m>0, false otherwise
+bool isPositiveDefinite(const mat33& m);
+
+/// Check if a 3x3 matrix is positive semi definite
+/// @param m a 3x3 matrix
+/// @return true if m>=0, false otherwise
+bool isPositiveSemiDefinite(const mat33& m);
+/// Check if a 3x3 matrix is positive semi definite within numeric limits
+/// @param m a 3x3 matrix
+/// @return true if m>=-eps, false otherwise
+bool isPositiveSemiDefiniteFuzzy(const mat33& m);
+
+/// Determinant of 3x3 matrix
+/// NOTE: implemented here for portability, as determinant operation
+///	   will be implemented differently for various matrix/vector libraries
+/// @param m a 3x3 matrix
+/// @return det(m)
+idScalar determinant(const mat33& m);
+
+/// Test if a 3x3 matrix satisfies some properties of inertia matrices
+/// @param I a 3x3 matrix
+/// @param index body index (for error messages)
+/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted
+/// @return true if I satisfies inertia matrix properties, false otherwise.
+bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint);
+
+/// Check if a 3x3 matrix is a valid transform (rotation) matrix
+/// @param m a 3x3 matrix
+/// @return true if m is a rotation matrix, false otherwise
+bool isValidTransformMatrix(const mat33& m);
+/// Transform matrix from parent to child frame,
+/// when the child frame is rotated about @param axis by @angle
+/// (mathematically positive)
+/// @param axis the axis of rotation
+/// @param angle rotation angle
+/// @param T pointer to transform matrix
+void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T);
+
+/// Check if this is a unit vector
+/// @param vector
+/// @return true if |vector|=1 within numeric limits
+bool isUnitVector(const vec3& vector);
+
+/// @input a vector in R^3
+/// @returns corresponding spin tensor
+mat33 tildeOperator(const vec3& v);
+/// @param alpha angle in radians
+/// @returns transform matrix for ratation with @param alpha about x-axis
+mat33 transformX(const idScalar& alpha);
+/// @param beta angle in radians
+/// @returns transform matrix for ratation with @param beta about y-axis
+mat33 transformY(const idScalar& beta);
+/// @param gamma angle in radians
+/// @returns transform matrix for ratation with @param gamma about z-axis
+mat33 transformZ(const idScalar& gamma);
+///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix
+/// @param rot rotation matrix
+/// @returns x-y-z Euler angles
+vec3 rpyFromMatrix(const mat33&rot);
+}
+#endif  // IDMATH_HPP_

src/BulletInverseDynamics/MultiBodyTree.cpp

@@ -0,0 +1,331 @@
+#include "MultiBodyTree.hpp"
+
+#include <cmath>
+#include <limits>
+#include <vector>
+
+#include "IDMath.hpp"
+#include "details/MultiBodyTreeImpl.hpp"
+#include "details/MultiBodyTreeInitCache.hpp"
+
+namespace btInverseDynamics {
+
+MultiBodyTree::MultiBodyTree()
+	: m_is_finalized(false),
+	  m_mass_parameters_are_valid(true),
+	  m_accept_invalid_mass_parameters(false),
+	  m_impl(0x0),
+	  m_init_cache(0x0) {
+	m_init_cache = new InitCache();
+}
+
+MultiBodyTree::~MultiBodyTree() {
+	delete m_impl;
+	delete m_init_cache;
+}
+
+void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
+	m_accept_invalid_mass_parameters = flag;
+}
+
+bool MultiBodyTree::getAcceptInvalidMassProperties() const {
+	return m_accept_invalid_mass_parameters;
+}
+
+int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const {
+	return m_impl->getBodyOrigin(body_index, world_origin);
+}
+
+int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const {
+	return m_impl->getBodyCoM(body_index, world_com);
+}
+
+int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const {
+	return m_impl->getBodyTransform(body_index, world_T_body);
+}
+int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const {
+	return m_impl->getBodyAngularVelocity(body_index, world_omega);
+}
+int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const {
+	return m_impl->getBodyLinearVelocity(body_index, world_velocity);
+}
+
+int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const {
+	return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
+}
+
+int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const {
+	return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
+}
+int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const {
+	return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
+}
+
+void MultiBodyTree::printTree() { m_impl->printTree(); }
+void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
+
+int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
+
+int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
+
+int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
+											vecx *joint_forces) {
+	if (false == m_is_finalized) {
+		error_message("system has not been initialized\n");
+		return -1;
+	}
+	if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
+		error_message("error in inverse dynamics calculation\n");
+		return -1;
+	}
+	return 0;
+}
+
+int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
+									   const bool initialize_matrix,
+									   const bool set_lower_triangular_matrix, matxx *mass_matrix) {
+	if (false == m_is_finalized) {
+		error_message("system has not been initialized\n");
+		return -1;
+	}
+	if (-1 ==
+		m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
+									set_lower_triangular_matrix, mass_matrix)) {
+		error_message("error in mass matrix calculation\n");
+		return -1;
+	}
+	return 0;
+}
+int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
+	return calculateMassMatrix(q, true, true, true, mass_matrix);
+}
+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,
+						   const vec3 &body_r_body_com, const mat33 &body_I_body,
+						   const int user_int, void *user_ptr) {
+	if (body_index < 0) {
+		error_message("body index must be positive (got %d)\n", body_index);
+		return -1;
+	}
+	vec3 body_axis_of_motion(body_axis_of_motion_);
+	switch (joint_type) {
+		case REVOLUTE:
+		case PRISMATIC:
+			// check if axis is unit vector
+			if (!isUnitVector(body_axis_of_motion)) {
+				warning_message(
+					"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
+					body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
+				idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) +
+											std::pow(body_axis_of_motion(1), 2) +
+											std::pow(body_axis_of_motion(2), 2));
+				if (length < std::sqrt(std::numeric_limits<idScalar>::min())) {
+					error_message("axis of motion vector too short (%e)\n", length);
+					return -1;
+				}
+				body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
+			}
+			break;
+		case FIXED:
+			break;
+		case FLOATING:
+			break;
+		default:
+			error_message("unknown joint type %d\n", joint_type);
+			return -1;
+	}
+
+	// sanity check for mass properties. Zero mass is OK.
+	if (mass < 0) {
+		m_mass_parameters_are_valid = false;
+		error_message("Body %d has invalid mass %e\n", body_index, mass);
+		if (!m_accept_invalid_mass_parameters) {
+			return -1;
+		}
+	}
+
+	if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) {
+		m_mass_parameters_are_valid = false;
+		// error message printed in function call
+		if (!m_accept_invalid_mass_parameters) {
+			return -1;
+		}
+	}
+
+	if (!isValidTransformMatrix(body_T_parent_ref)) {
+		return -1;
+	}
+
+	return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref,
+								 body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com,
+								 body_I_body, user_int, user_ptr);
+}
+
+int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const {
+	return m_impl->getParentIndex(body_index, parent_index);
+}
+
+int MultiBodyTree::getUserInt(const int body_index, int *user_int) const {
+	return m_impl->getUserInt(body_index, user_int);
+}
+
+int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const {
+	return m_impl->getUserPtr(body_index, user_ptr);
+}
+
+int MultiBodyTree::setUserInt(const int body_index, const int user_int) {
+	return m_impl->setUserInt(body_index, user_int);
+}
+
+int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) {
+	return m_impl->setUserPtr(body_index, user_ptr);
+}
+
+int MultiBodyTree::finalize() {
+	const int &num_bodies = m_init_cache->numBodies();
+	const int &num_dofs = m_init_cache->numDoFs();
+
+	// 1 allocate internal MultiBody structure
+	m_impl = new MultiBodyImpl(num_bodies, num_dofs);
+
+	// 2 build new index set assuring index(parent) < index(child)
+	if (-1 == m_init_cache->buildIndexSets()) {
+		return -1;
+	}
+	m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
+
+	// 3 setup internal kinematic and dynamic data
+	for (int index = 0; index < num_bodies; index++) {
+		InertiaData inertia;
+		JointData joint;
+		if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
+			return -1;
+		}
+		if (-1 == m_init_cache->getJointData(index, &joint)) {
+			return -1;
+		}
+
+		RigidBody &rigid_body = m_impl->m_body_list[index];
+
+		rigid_body.m_mass = inertia.m_mass;
+		rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com;
+		rigid_body.m_body_I_body = inertia.m_body_I_body;
+		rigid_body.m_joint_type = joint.m_type;
+		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
+		rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref;
+		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
+		rigid_body.m_joint_type = joint.m_type;
+
+		// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
+		// matrices.
+		switch (rigid_body.m_joint_type) {
+			case REVOLUTE:
+				rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
+				rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
+				rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2);
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
+			case PRISMATIC:
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0);
+				rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1);
+				rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
+				break;
+			case FIXED:
+				// NOTE/TODO: dimension really should be zero ..
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
+			case FLOATING:
+				// NOTE/TODO: this is not really correct.
+				// the Jacobians should be 3x3 matrices here !
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
+			default:
+				error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
+				return -1;
+		}
+	}
+
+	// 4 assign degree of freedom indices & build per-joint-type index arrays
+	if (-1 == m_impl->generateIndexSets()) {
+		error_message("generating index sets\n");
+		return -1;
+	}
+
+	// 5 do some pre-computations ..
+	m_impl->calculateStaticData();
+
+	// 6. make sure all user forces are set to zero, as this might not happen
+	//	in the vector ctors.
+	m_impl->clearAllUserForcesAndMoments();
+
+	m_is_finalized = true;
+	return 0;
+}
+
+int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
+	return m_impl->setGravityInWorldFrame(gravity);
+}
+
+int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const {
+	return m_impl->getJointType(body_index, joint_type);
+}
+
+int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const {
+	return m_impl->getJointTypeStr(body_index, joint_type);
+}
+
+int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const {
+	return m_impl->getDoFOffset(body_index, q_offset);
+}
+
+int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
+	return m_impl->setBodyMass(body_index, mass);
+}
+
+int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) {
+	return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
+}
+
+int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) {
+	return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
+}
+
+int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const {
+	return m_impl->getBodyMass(body_index, mass);
+}
+
+int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const {
+	return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
+}
+
+int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const {
+	return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
+}
+
+void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
+
+int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) {
+	return m_impl->addUserForce(body_index, body_force);
+}
+
+int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) {
+	return m_impl->addUserMoment(body_index, body_moment);
+}
+
+}

src/BulletInverseDynamics/MultiBodyTree.hpp

@@ -0,0 +1,308 @@
+#ifndef MULTIBODYTREE_HPP_
+#define MULTIBODYTREE_HPP_
+
+#include "IDConfig.hpp"
+#include "IDMath.hpp"
+
+namespace btInverseDynamics {
+
+/// Enumeration of supported joint types
+enum JointType {
+	/// no degree of freedom, moves with parent
+	FIXED = 0,
+	/// one rotational degree of freedom relative to parent
+	REVOLUTE,
+	/// one translational degree of freedom relative to parent
+	PRISMATIC,
+	/// six degrees of freedom relative to parent
+	FLOATING
+};
+
+/// Interface class for calculating inverse dynamics for tree structured
+/// multibody systems
+///
+/// Note on degrees of freedom
+/// The q vector contains the generalized coordinate set defining the tree's configuration.
+/// Every joint adds elements that define the corresponding link's frame pose relative to
+/// its parent. For the joint types that is:
+///	- FIXED:	 none
+///	- REVOLUTE:  angle of rotation [rad]
+///	- PRISMATIC: displacement [m]
+///	- FLOATING:  Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
+///				 (in that order)
+/// The u vector contains the generalized speeds, which are
+///	- FIXED:	 none
+///	- REVOLUTE:  time derivative of angle of rotation [rad/s]
+///	- PRISMATIC: time derivative of displacement [m/s]
+///	- FLOATING:  angular velocity [rad/s] (*not* time derivative of rpy angles)
+///				 and time derivative of displacement in parent frame [m/s]
+///
+/// The q and u vectors are obtained by stacking contributions of all bodies in one
+/// vector in the order of body indices.
+///
+/// Note on generalized forces: analogous to u, i.e.,
+///	 - FIXED:	 none
+///	 - REVOLUTE:  moment [Nm], about joint axis
+///	 - PRISMATIC: force  [N], along joint axis
+///	 - FLOATING:  moment vector [Nm] and force vector [N], both in body-fixed frame
+///				  (in that order)
+///
+/// TODO - force element interface (friction, springs, dampers, etc)
+///	  - gears and motor inertia
+class MultiBodyTree {
+public:
+	/// The contructor.
+	/// Initialization & allocation is via addBody and buildSystem calls.
+	MultiBodyTree();
+	/// the destructor. This also deallocates all memory
+	~MultiBodyTree();
+
+	/// Add body to the system. this allocates memory and not real-time safe.
+	/// This only adds the data to an initial cache. After all bodies have been
+	/// added,
+	/// the system is setup using the buildSystem call
+	/// @param body_index index of the body to be added. Must >=0, <number of bodies,
+	///		and index of parent must be < index of body
+	/// @param parent_index index of the parent body
+	///		The root of the tree has index 0 and its parent (the world frame)
+	///		is assigned index -1
+	///		the rotation and translation relative to the parent are taken as
+	///		pose of the root body relative to the world frame. Other parameters
+	///		are ignored
+	/// @param JointType type of joint connecting the body to the parent
+	/// @param mass the mass of the body
+	/// @param body_r_body_com the center of mass of the body relative to and
+	/// described in
+	///		the body fixed frame, which is located in the joint axis connecting
+	/// the body to its parent
+	/// @param body_I_body the moment of inertia of the body w.r.t the body-fixed
+	/// frame
+	///		(ie, the reference point is the origin of the body-fixed frame and
+	/// the matrix is written
+	///		 w.r.t. those unit vectors)
+	/// @param parent_r_parent_body_ref position of joint relative to the parent
+	/// body's reference frame
+	///		for q=0, written in the parent bodies reference frame
+	/// @param body_axis_of_motion translation/rotation axis in body-fixed frame.
+	///		Ignored for joints that are not revolute or prismatic.
+	///		must be a unit vector.
+	/// @param body_T_parent_ref transform matrix from parent to body reference
+	/// frame for q=0.
+	///		This is the matrix transforming a vector represented in the
+	/// parent's reference frame into one represented
+	///		in this body's reference frame.
+	///		ie, if parent_vec is a vector in R^3 whose components are w.r.t to
+	/// the parent's reference frame,
+	///		then the same vector written w.r.t. this body's frame (for q=0) is
+	/// given by
+	///		body_vec = parent_R_body_ref * parent_vec
+	/// @param user_ptr pointer to user data
+	/// @param user_int pointer to user integer
+	/// @return 0 on success, -1 on error
+	int 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, const vec3& body_r_body_com,
+				const mat33& body_I_body, const int user_int, void* user_ptr);
+	/// set policy for invalid mass properties
+	/// @param flag if true, invalid mass properties are accepted,
+	///		the default is false
+	void setAcceptInvalidMassParameters(bool flag);
+	/// @return the mass properties policy flag
+	bool getAcceptInvalidMassProperties() const;
+	/// build internal data structures
+	/// call this after all bodies have been added via addBody
+	/// @return 0 on success, -1 on error
+	int finalize();
+	/// pretty print ascii description of tree to stdout
+	void printTree();
+	/// print tree data to stdout
+	void printTreeData();
+	/// calculate joint forces for given generalized state & derivatives
+	/// @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
+	/// @param joint_forces this is where the resulting joint forces will be
+	///		stored. dim(joint_forces) = dim(u)
+	/// @return 0 on success, -1 on error
+	int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
+								 vecx* joint_forces);
+	/// Calculate joint space mass matrix
+	/// @param q generalized coordinates
+	/// @param initialize_matrix if true, initialize mass matrix with zero.
+	///		If mass_matrix is initialized to zero externally and only used
+	///		for mass matrix computations for the same system, it is safe to
+	///		set this to false.
+	/// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix
+	///		is also populated, otherwise not.
+	/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
+	/// @return -1 on error, 0 on success
+	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
+							const bool initialize_matrix, const bool set_lower_triangular_matrix,
+							matxx* mass_matrix);
+
+	/// Calculate joint space mass matrix.
+	/// This version will update kinematics, initialize all mass_matrix elements to zero and
+	/// populate all mass matrix entries.
+	/// @param q generalized coordinates
+	/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
+	/// @return -1 on error, 0 on success
+	int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
+
+	/// set gravitational acceleration
+	/// the default is [0;0;-9.8] in the world frame
+	/// @param gravity the gravitational acceleration in world frame
+	/// @return 0 on success, -1 on error
+	int setGravityInWorldFrame(const vec3& gravity);
+	/// returns number of bodies in tree
+	int numBodies() const;
+	/// returns number of mechanical degrees of freedom (dimension of q-vector)
+	int numDoFs() const;
+	/// get origin of a body-fixed frame, represented in world frame
+	/// @param body_index index for frame/body
+	/// @param world_origin pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyOrigin(const int body_index, vec3* world_origin) const;
+	/// get center of mass of a body, represented in world frame
+	/// @param body_index index for frame/body
+	/// @param world_com pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyCoM(const int body_index, vec3* world_com) const;
+	/// get transform from of a body-fixed frame to the world frame
+	/// @param body_index index for frame/body
+	/// @param world_T_body pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyTransform(const int body_index, mat33* world_T_body) const;
+	/// get absolute angular velocity for a body, represented in the world frame
+	/// @param body_index index for frame/body
+	/// @param world_omega pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
+	/// get linear velocity of a body, represented in world frame
+	/// @param body_index index for frame/body
+	/// @param world_velocity pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
+	/// get linear velocity of a body's CoM, represented in world frame
+	/// (not required for inverse dynamics, provided for convenience)
+	/// @param body_index index for frame/body
+	/// @param world_vel_com pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
+	/// get origin of a body-fixed frame, represented in world frame
+	/// @param body_index index for frame/body
+	/// @param world_origin pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
+	/// get origin of a body-fixed frame, represented in world frame
+	/// NOTE: this will include the gravitational acceleration, so the actual acceleration is
+	/// obtainened by setting gravitational acceleration to zero, or subtracting it.
+	/// @param body_index index for frame/body
+	/// @param world_origin pointer for return data
+	/// @return 0 on success, -1 on error
+	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
+	/// returns the (internal) index of body
+	/// @param body_index is the index of a body (internal: TODO: fix/clarify
+	/// indexing!)
+	/// @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;
+	/// get joint type
+	/// @param body_index index of the body
+	/// @param joint_type the corresponding joint type
+	/// @return 0 on success, -1 on failure
+	int getJointType(const int body_index, JointType* joint_type) const;
+	/// get joint type as string
+	/// @param body_index index of the body
+	/// @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 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
+	/// @return -1 on error, 0 on success
+	int getDoFOffset(const int body_index, int* q_offset) const;
+	/// get user integer. not used by the library.
+	/// @param body_index index of the body
+	/// @param user_int   the user integer
+	/// @return 0 on success, -1 on error
+	int getUserInt(const int body_index, int* user_int) const;
+	/// get user pointer. not used by the library.
+	/// @param body_index index of the body
+	/// @param user_ptr   the user pointer
+	/// @return 0 on success, -1 on error
+	int getUserPtr(const int body_index, void** user_ptr) const;
+	/// set user integer. not used by the library.
+	/// @param body_index index of the body
+	/// @param user_int   the user integer
+	/// @return 0 on success, -1 on error
+	int setUserInt(const int body_index, const int user_int);
+	/// set user pointer. not used by the library.
+	/// @param body_index index of the body
+	/// @param user_ptr   the user pointer
+	/// @return 0 on success, -1 on error
+	int setUserPtr(const int body_index, void* const user_ptr);
+	/// set mass for a body
+	/// @param body_index index of the body
+	/// @param mass the mass to set
+	/// @return 0 on success, -1 on failure
+	int setBodyMass(const int body_index, const idScalar mass);
+	/// set first moment of mass for a body
+	/// (mass * center of mass, in body fixed frame, relative to joint)
+	/// @param body_index index of the body
+	/// @param first_mass_moment the vector to set
+	/// @return 0 on success, -1 on failure
+	int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
+	/// set second moment of mass for a body
+	/// (moment of inertia, in body fixed frame, relative to joint)
+	/// @param body_index index of the body
+	/// @param second_mass_moment the inertia matrix
+	/// @return 0 on success, -1 on failure
+	int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
+	/// get mass for a body
+	/// @param body_index index of the body
+	/// @param mass the mass
+	/// @return 0 on success, -1 on failure
+	int getBodyMass(const int body_index, idScalar* mass) const;
+	/// get first moment of mass for a body
+	/// (mass * center of mass, in body fixed frame, relative to joint)
+	/// @param body_index index of the body
+	/// @param first_moment the vector
+	/// @return 0 on success, -1 on failure
+	int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
+	/// get second moment of mass for a body
+	/// (moment of inertia, in body fixed frame, relative to joint)
+	/// @param body_index index of the body
+	/// @param second_mass_moment the inertia matrix
+	/// @return 0 on success, -1 on failure
+	int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
+	/// set all user forces and moments to zero
+	void clearAllUserForcesAndMoments();
+	/// Add an external force to a body, acting at the origin of the body-fixed frame.
+	/// Calls to addUserForce are cumulative. Set the user force and moment to zero
+	/// via clearAllUserForcesAndMoments()
+	/// @param body_force the force represented in the body-fixed frame of reference
+	/// @return 0 on success, -1 on error
+	int addUserForce(const int body_index, const vec3& body_force);
+	/// Add an external moment to a body.
+	/// Calls to addUserMoment are cumulative. Set the user force and moment to zero
+	/// via clearAllUserForcesAndMoments()
+	/// @param body_moment the moment represented in the body-fixed frame of reference
+	/// @return 0 on success, -1 on error
+	int addUserMoment(const int body_index, const vec3& body_moment);
+
+private:
+	// flag indicating if system has been initialized
+	bool m_is_finalized;
+	// flag indicating if mass properties are physically valid
+	bool m_mass_parameters_are_valid;
+	// flag defining if unphysical mass parameters are accepted
+	bool m_accept_invalid_mass_parameters;
+	// This struct implements the inverse dynamics calculations
+	class MultiBodyImpl;
+	MultiBodyImpl* m_impl;
+	// cache data structure for initialization
+	class InitCache;
+	InitCache* m_init_cache;
+};
+}  // namespace btInverseDynamics
+#endif  // MULTIBODYTREE_HPP_

src/BulletInverseDynamics/details/IDEigenInterface.hpp

@@ -0,0 +1,17 @@
+#ifndef INVDYNEIGENINTERFACE_HPP_
+#define INVDYNEIGENINTERFACE_HPP_
+#include "../IDConfig.hpp"
+namespace btInverseDynamics {
+#ifdef BT_USE_DOUBLE_PRECISION
+typedef Eigen::VectorXd vecx;
+typedef Eigen::Vector3d vec3;
+typedef Eigen::Matrix3d mat33;
+typedef Eigen::MatrixXd matxx;
+#else
+typedef Eigen::VectorXf vecx;
+typedef Eigen::Vector3f vec3;
+typedef Eigen::Matrix3f mat33;
+typedef Eigen::MatrixXf matxx;
+#endif  //
+}
+#endif  // INVDYNEIGENINTERFACE_HPP_

src/BulletInverseDynamics/details/IDLinearMathInterface.hpp

@@ -0,0 +1,112 @@
+#ifndef IDLINEARMATHINTERFACE_HPP_
+#define IDLINEARMATHINTERFACE_HPP_
+
+#include <cstdlib>
+
+#include "../IDConfig.hpp"
+
+#include "../../LinearMath/btMatrix3x3.h"
+#include "../../LinearMath/btVector3.h"
+#include "../../LinearMath/btMatrixX.h"
+
+namespace btInverseDynamics {
+class vec3;
+class vecx;
+class mat33;
+typedef btMatrixX<idScalar> matxx;
+
+class vec3 : public btVector3 {
+public:
+	vec3() : btVector3() {}
+	vec3(const btVector3& btv) { *this = btv; }
+	idScalar& operator()(int i) { return (*this)[i]; }
+	const idScalar& operator()(int i) const { return (*this)[i]; }
+	const int size() const { return 3; }
+	const vec3& operator=(const btVector3& rhs) {
+		*static_cast<btVector3*>(this) = rhs;
+		return *this;
+	}
+};
+
+class mat33 : public btMatrix3x3 {
+public:
+	mat33() : btMatrix3x3() {}
+	mat33(const btMatrix3x3& btm) { *this = btm; }
+	idScalar& operator()(int i, int j) { return (*this)[i][j]; }
+	const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
+	const mat33& operator=(const btMatrix3x3& rhs) {
+		*static_cast<btMatrix3x3*>(this) = rhs;
+		return *this;
+	}
+	friend mat33 operator*(const idScalar& s, const mat33& a);
+	friend mat33 operator/(const mat33& a, const idScalar& s);
+};
+
+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 btVectorX<idScalar> {
+public:
+	vecx(int size) : btVectorX(size) {}
+	const vecx& operator=(const btVectorX<idScalar>& rhs) {
+		*static_cast<btVectorX*>(this) = rhs;
+		return *this;
+	}
+
+	idScalar& operator()(int i) { return (*this)[i]; }
+	const idScalar& operator()(int i) const { return (*this)[i]; }
+
+	friend vecx operator*(const vecx& a, const idScalar& s);
+	friend vecx operator*(const idScalar& s, const vecx& a);
+
+	friend vecx operator+(const vecx& a, const vecx& b);
+	friend vecx operator-(const vecx& a, const vecx& b);
+	friend vecx operator/(const vecx& a, const idScalar& s);
+};
+
+inline vecx operator*(const vecx& a, const idScalar& s) {
+	vecx result(a.size());
+	for (int i = 0; i < result.size(); i++) {
+		result(i) = a(i) * s;
+	}
+	return result;
+}
+inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
+inline vecx operator+(const vecx& a, const vecx& b) {
+	vecx result(a.size());
+	// TODO: error handling for a.size() != b.size()??
+	if (a.size() != b.size()) {
+		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		abort();
+	}
+	for (int i = 0; i < a.size(); i++) {
+		result(i) = a(i) + b(i);
+	}
+
+	return result;
+}
+
+inline vecx operator-(const vecx& a, const vecx& b) {
+	vecx result(a.size());
+	// TODO: error handling for a.size() != b.size()??
+	if (a.size() != b.size()) {
+		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		abort();
+	}
+	for (int i = 0; i < a.size(); i++) {
+		result(i) = a(i) - b(i);
+	}
+	return result;
+}
+inline vecx operator/(const vecx& a, const idScalar& s) {
+	vecx result(a.size());
+	for (int i = 0; i < result.size(); i++) {
+		result(i) = a(i) / s;
+	}
+
+	return result;
+}
+}
+
+#endif  // IDLINEARMATHINTERFACE_HPP_

src/BulletInverseDynamics/details/IDMatVec.hpp

@@ -0,0 +1,328 @@
+/// @file Built-In Matrix-Vector functions
+#ifndef IDMATVEC_HPP_
+#define IDMATVEC_HPP_
+
+#include <cstdlib>
+
+#include "../IDConfig.hpp"
+
+namespace btInverseDynamics {
+class vec3;
+class vecx;
+class mat33;
+class matxx;
+
+/// 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
+/// want from a "fully featured" linear math library.
+class vec3 {
+public:
+	idScalar& operator()(int i) { return m_data[i]; }
+	const idScalar& operator()(int i) const { return m_data[i]; }
+	const int size() const { return 3; }
+	const vec3& operator=(const vec3& rhs);
+	const vec3& operator+=(const vec3& b);
+	const vec3& operator-=(const vec3& b);
+	vec3 cross(const vec3& b) const;
+	idScalar dot(const vec3& b) const;
+
+	friend vec3 operator*(const mat33& a, const vec3& b);
+	friend vec3 operator*(const vec3& a, const idScalar& s);
+	friend vec3 operator*(const idScalar& s, const vec3& a);
+
+	friend vec3 operator+(const vec3& a, const vec3& b);
+	friend vec3 operator-(const vec3& a, const vec3& b);
+	friend vec3 operator/(const vec3& a, const idScalar& s);
+
+private:
+	idScalar m_data[3];
+};
+
+class mat33 {
+public:
+	idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
+	const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
+	const mat33& operator=(const mat33& rhs);
+	mat33 transpose() const;
+	const mat33& operator+=(const mat33& b);
+	const mat33& operator-=(const mat33& b);
+
+	friend mat33 operator*(const mat33& a, const mat33& b);
+	friend vec3 operator*(const mat33& a, const vec3& b);
+	friend mat33 operator*(const mat33& a, const idScalar& s);
+	friend mat33 operator*(const idScalar& s, const mat33& a);
+	friend mat33 operator+(const mat33& a, const mat33& b);
+	friend mat33 operator-(const mat33& a, const mat33& b);
+	friend mat33 operator/(const mat33& a, const idScalar& s);
+
+private:
+	// layout is [0,1,2;3,4,5;6,7,8]
+	idScalar m_data[9];
+};
+
+class vecx {
+public:
+	vecx(int size) : m_size(size) {
+		m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
+	}
+	~vecx() { idFree(m_data); }
+	const vecx& operator=(const vecx& rhs);
+	idScalar& operator()(int i) { return m_data[i]; }
+	const idScalar& operator()(int i) const { return m_data[i]; }
+	const int& size() const { return m_size; }
+
+	friend vecx operator*(const vecx& a, const idScalar& s);
+	friend vecx operator*(const idScalar& s, const vecx& a);
+
+	friend vecx operator+(const vecx& a, const vecx& b);
+	friend vecx operator-(const vecx& a, const vecx& b);
+	friend vecx operator/(const vecx& a, const idScalar& s);
+
+private:
+	int m_size;
+	idScalar* m_data;
+};
+
+class matxx {
+public:
+	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]; }
+	const int& rows() const { return m_rows; }
+	const int& cols() const { return m_cols; }
+
+private:
+	int m_rows;
+	int m_cols;
+	idScalar* m_data;
+};
+
+inline const vec3& vec3::operator=(const vec3& rhs) {
+	if (&rhs != this) {
+		memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
+	}
+	return *this;
+}
+
+inline vec3 vec3::cross(const vec3& b) const {
+	vec3 result;
+	result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
+	result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2];
+	result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0];
+
+	return result;
+}
+
+inline idScalar vec3::dot(const vec3& b) const {
+	return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
+}
+
+inline const mat33& mat33::operator=(const mat33& rhs) {
+	if (&rhs != this) {
+		memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
+	}
+	return *this;
+}
+inline mat33 mat33::transpose() const {
+	mat33 result;
+	result.m_data[0] = m_data[0];
+	result.m_data[1] = m_data[3];
+	result.m_data[2] = m_data[6];
+	result.m_data[3] = m_data[1];
+	result.m_data[4] = m_data[4];
+	result.m_data[5] = m_data[7];
+	result.m_data[6] = m_data[2];
+	result.m_data[7] = m_data[5];
+	result.m_data[8] = m_data[8];
+
+	return result;
+}
+
+inline mat33 operator*(const mat33& a, const mat33& b) {
+	mat33 result;
+	result.m_data[0] =
+		a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6];
+	result.m_data[1] =
+		a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7];
+	result.m_data[2] =
+		a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8];
+	result.m_data[3] =
+		a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6];
+	result.m_data[4] =
+		a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7];
+	result.m_data[5] =
+		a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8];
+	result.m_data[6] =
+		a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6];
+	result.m_data[7] =
+		a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7];
+	result.m_data[8] =
+		a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8];
+
+	return result;
+}
+
+inline const mat33& mat33::operator+=(const mat33& b) {
+	for (int i = 0; i < 9; i++) {
+		m_data[i] += b.m_data[i];
+	}
+
+	return *this;
+}
+
+inline const mat33& mat33::operator-=(const mat33& b) {
+	for (int i = 0; i < 9; i++) {
+		m_data[i] -= b.m_data[i];
+	}
+	return *this;
+}
+
+inline vec3 operator*(const mat33& a, const vec3& b) {
+	vec3 result;
+
+	result.m_data[0] =
+		a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2];
+	result.m_data[1] =
+		a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2];
+	result.m_data[2] =
+		a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2];
+
+	return result;
+}
+
+inline const vec3& vec3::operator+=(const vec3& b) {
+	for (int i = 0; i < 3; i++) {
+		m_data[i] += b.m_data[i];
+	}
+	return *this;
+}
+
+inline const vec3& vec3::operator-=(const vec3& b) {
+	for (int i = 0; i < 3; i++) {
+		m_data[i] -= b.m_data[i];
+	}
+	return *this;
+}
+
+inline mat33 operator*(const mat33& a, const idScalar& s) {
+	mat33 result;
+	for (int i = 0; i < 9; i++) {
+		result.m_data[i] = a.m_data[i] * s;
+	}
+	return result;
+}
+
+inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
+
+inline vec3 operator*(const vec3& a, const idScalar& s) {
+	vec3 result;
+	for (int i = 0; i < 3; i++) {
+		result.m_data[i] = a.m_data[i] * s;
+	}
+	return result;
+}
+inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
+
+inline mat33 operator+(const mat33& a, const mat33& b) {
+	mat33 result;
+	for (int i = 0; i < 9; i++) {
+		result.m_data[i] = a.m_data[i] + b.m_data[i];
+	}
+	return result;
+}
+inline vec3 operator+(const vec3& a, const vec3& b) {
+	vec3 result;
+	for (int i = 0; i < 3; i++) {
+		result.m_data[i] = a.m_data[i] + b.m_data[i];
+	}
+	return result;
+}
+
+inline mat33 operator-(const mat33& a, const mat33& b) {
+	mat33 result;
+	for (int i = 0; i < 9; i++) {
+		result.m_data[i] = a.m_data[i] - b.m_data[i];
+	}
+	return result;
+}
+inline vec3 operator-(const vec3& a, const vec3& b) {
+	vec3 result;
+	for (int i = 0; i < 3; i++) {
+		result.m_data[i] = a.m_data[i] - b.m_data[i];
+	}
+	return result;
+}
+
+inline mat33 operator/(const mat33& a, const idScalar& s) {
+	mat33 result;
+	for (int i = 0; i < 9; i++) {
+		result.m_data[i] = a.m_data[i] / s;
+	}
+	return result;
+}
+
+inline vec3 operator/(const vec3& a, const idScalar& s) {
+	vec3 result;
+	for (int i = 0; i < 3; i++) {
+		result.m_data[i] = a.m_data[i] / s;
+	}
+	return result;
+}
+
+inline const vecx& vecx::operator=(const vecx& rhs) {
+	if (size() != rhs.size()) {
+		error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
+		abort();
+	}
+	if (&rhs != this) {
+		memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
+	}
+	return *this;
+}
+inline vecx operator*(const vecx& a, const idScalar& s) {
+	vecx result(a.size());
+	for (int i = 0; i < result.size(); i++) {
+		result.m_data[i] = a.m_data[i] * s;
+	}
+	return result;
+}
+inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
+inline vecx operator+(const vecx& a, const vecx& b) {
+	vecx result(a.size());
+	// TODO: error handling for a.size() != b.size()??
+	if (a.size() != b.size()) {
+		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		abort();
+	}
+	for (int i = 0; i < a.size(); i++) {
+		result.m_data[i] = a.m_data[i] + b.m_data[i];
+	}
+
+	return result;
+}
+inline vecx operator-(const vecx& a, const vecx& b) {
+	vecx result(a.size());
+	// TODO: error handling for a.size() != b.size()??
+	if (a.size() != b.size()) {
+		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		abort();
+	}
+	for (int i = 0; i < a.size(); i++) {
+		result.m_data[i] = a.m_data[i] - b.m_data[i];
+	}
+	return result;
+}
+inline vecx operator/(const vecx& a, const idScalar& s) {
+	vecx result(a.size());
+	for (int i = 0; i < result.size(); i++) {
+		result.m_data[i] = a.m_data[i] / s;
+	}
+
+	return result;
+}
+}
+#endif

src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -0,0 +1,844 @@
+#include "MultiBodyTreeImpl.hpp"
+
+namespace btInverseDynamics {
+
+MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
+	: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
+	m_body_list.resize(num_bodies_);
+	m_parent_index.resize(num_bodies_);
+	m_child_indices.resize(num_bodies_);
+	m_user_int.resize(num_bodies_);
+	m_user_ptr.resize(num_bodies_);
+
+	m_world_gravity(0) = 0.0;
+	m_world_gravity(1) = 0.0;
+	m_world_gravity(2) = -9.8;
+}
+
+const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const {
+	switch (type) {
+		case FIXED:
+			return "fixed";
+		case REVOLUTE:
+			return "revolute";
+		case PRISMATIC:
+			return "prismatic";
+		case FLOATING:
+			return "floating";
+	}
+	return "error: invalid";
+}
+
+inline void indent(const int &level) {
+	for (int j = 0; j < level; j++)
+		id_printf("  ");  // indent
+}
+
+void MultiBodyTree::MultiBodyImpl::printTree() {
+	id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type));
+	printTree(0, 0);
+}
+
+void MultiBodyTree::MultiBodyImpl::printTreeData() {
+	for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+		RigidBody &body = m_body_list[i];
+		id_printf("body: %d\n", static_cast<int>(i));
+		id_printf("type: %s\n", jointTypeToString(body.m_joint_type));
+		id_printf("q_index= %d\n", body.m_q_index);
+		id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2));
+		id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2));
+
+		id_printf("mass = %f\n", body.m_mass);
+		id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1),
+				  body.m_body_mass_com(2));
+		id_printf("I_o= [%f %f %f;\n"
+				  "	  %f %f %f;\n"
+				  "	  %f %f %f]\n",
+				  body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
+				  body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
+				  body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
+
+		id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0),
+				  body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2));
+	}
+}
+int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
+	switch (type) {
+		case FIXED:
+			return 0;
+		case REVOLUTE:
+		case PRISMATIC:
+			return 1;
+		case FLOATING:
+			return 6;
+	}
+	error_message("unknown joint type %d\n", type);
+	return 0;
+}
+
+void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) {
+	// this is adapted from URDF2Bullet.
+	// TODO: fix this and print proper graph (similar to git --log --graph)
+	int num_children = m_child_indices[index].size();
+
+	indentation += 2;
+	int count = 0;
+
+	for (int i = 0; i < num_children; i++) {
+		int child_index = m_child_indices[index][i];
+		indent(indentation);
+		id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index,
+				  jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1,
+				  m_body_list[index].m_q_index,
+				  m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type));
+		// first grandchild
+		printTree(child_index, indentation);
+	}
+}
+
+int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) {
+	m_world_gravity = gravity;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
+	m_body_revolute_list.resize(0);
+	m_body_prismatic_list.resize(0);
+	int q_index = 0;
+	for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+		RigidBody &body = m_body_list[i];
+		body.m_q_index = -1;
+		switch (body.m_joint_type) {
+			case REVOLUTE:
+				m_body_revolute_list.push_back(i);
+				body.m_q_index = q_index;
+				q_index++;
+				break;
+			case PRISMATIC:
+				m_body_prismatic_list.push_back(i);
+				body.m_q_index = q_index;
+				q_index++;
+				break;
+			case FIXED:
+				// do nothing
+				break;
+			case FLOATING:
+				m_body_floating_list.push_back(i);
+				body.m_q_index = q_index;
+				q_index += 6;
+				break;
+			default:
+				error_message("unsupported joint type %d\n", body.m_joint_type);
+				return -1;
+		}
+	}
+	// sanity check
+	if (q_index != m_num_dofs) {
+		error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
+		return -1;
+	}
+
+	m_child_indices.resize(m_body_list.size());
+
+	for (idArrayIdx child = 1; child < m_parent_index.size(); child++) {
+		const int &parent = m_parent_index[child];
+		if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) {
+			m_child_indices[parent].push_back(child);
+		} else {
+			if (-1 == parent) {
+				// multiple bodies are directly linked to the environment, ie, not a single root
+				error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
+			} else {
+				// should never happen
+				error_message(
+					"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
+					child, parent, static_cast<int>(m_parent_index.size()));
+			}
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
+	// relative kinematics that are not a function of q, u, dot_u
+	for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+		RigidBody &body = m_body_list[i];
+		switch (body.m_joint_type) {
+			case REVOLUTE:
+				body.m_parent_vel_rel(0) = 0;
+				body.m_parent_vel_rel(1) = 0;
+				body.m_parent_vel_rel(2) = 0;
+				body.m_parent_acc_rel(0) = 0;
+				body.m_parent_acc_rel(1) = 0;
+				body.m_parent_acc_rel(2) = 0;
+				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
+				break;
+			case PRISMATIC:
+				body.m_body_T_parent = body.m_body_T_parent_ref;
+				body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT;
+				body.m_body_ang_vel_rel(0) = 0;
+				body.m_body_ang_vel_rel(1) = 0;
+				body.m_body_ang_vel_rel(2) = 0;
+				body.m_body_ang_acc_rel(0) = 0;
+				body.m_body_ang_acc_rel(1) = 0;
+				body.m_body_ang_acc_rel(2) = 0;
+				break;
+			case FIXED:
+				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
+				body.m_body_T_parent = body.m_body_T_parent_ref;
+				body.m_body_ang_vel_rel(0) = 0;
+				body.m_body_ang_vel_rel(1) = 0;
+				body.m_body_ang_vel_rel(2) = 0;
+				body.m_parent_vel_rel(0) = 0;
+				body.m_parent_vel_rel(1) = 0;
+				body.m_parent_vel_rel(2) = 0;
+				body.m_body_ang_acc_rel(0) = 0;
+				body.m_body_ang_acc_rel(1) = 0;
+				body.m_body_ang_acc_rel(2) = 0;
+				body.m_parent_acc_rel(0) = 0;
+				body.m_parent_acc_rel(1) = 0;
+				body.m_parent_acc_rel(2) = 0;
+				break;
+			case FLOATING:
+				// no static data
+				break;
+		}
+	}
+}
+
+int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u,
+														   const vecx &dot_u, vecx *joint_forces) {
+	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
+		joint_forces->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, dim(joint_forces)= %d\n",
+					  m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
+					  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);
+	}
+
+	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)
+		body.m_eom_lhs_rotational =
+			body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
+			body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
+			body.m_body_moment_user;
+		body.m_eom_lhs_translational =
+			body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
+			body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
+			body.m_body_force_user;
+	}
+
+	// 4. 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,
+	// but that would make changing masses online harder (eg, payload masses
+	// added with fixed  joints to a gripper)
+	// Also, this enables adding zero weight bodies as a way to calculate frame poses
+	// for force elements, etc.
+
+	for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) {
+		// sum of forces and moments acting on this body from its children
+		vec3 sum_f_children;
+		vec3 sum_m_children;
+		setZero(sum_f_children);
+		setZero(sum_m_children);
+		for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
+			 child_list_idx++) {
+			const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
+			vec3 child_joint_force_in_this_frame =
+				child.m_body_T_parent.transpose() * child.m_force_at_joint;
+			sum_f_children -= child_joint_force_in_this_frame;
+			sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
+							  child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
+		}
+		RigidBody &body = m_body_list[body_idx];
+
+		body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
+		body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
+	}
+
+	// 4. Calculate Joint forces.
+	// These are the components of force_at_joint/moment_at_joint
+	// in the free directions given by Jac_JT/Jac_JR
+	// 4.1 revolute joints
+	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+		RigidBody &body = m_body_list[m_body_revolute_list[i]];
+		// (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
+		(*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
+	}
+	// 4.2 for prismatic joints
+	for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
+		RigidBody &body = m_body_list[m_body_prismatic_list[i]];
+		// (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
+		(*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
+	}
+	// 4.3 floating bodies (6-DoF joints)
+	for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
+		RigidBody &body = m_body_list[m_body_floating_list[i]];
+		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
+		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
+		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
+
+		(*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
+		(*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
+		(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
+	}
+
+	return 0;
+}
+
+static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) {
+	switch (dof) {
+		// rotational part
+		case 0:
+			Jac_JR(0) = 1;
+			Jac_JR(1) = 0;
+			Jac_JR(2) = 0;
+			setZero(Jac_JT);
+			break;
+		case 1:
+			Jac_JR(0) = 0;
+			Jac_JR(1) = 1;
+			Jac_JR(2) = 0;
+			setZero(Jac_JT);
+			break;
+		case 2:
+			Jac_JR(0) = 0;
+			Jac_JR(1) = 0;
+			Jac_JR(2) = 1;
+			setZero(Jac_JT);
+			break;
+		// translational part
+		case 3:
+			setZero(Jac_JR);
+			Jac_JT(0) = 1;
+			Jac_JT(1) = 0;
+			Jac_JT(2) = 0;
+			break;
+		case 4:
+			setZero(Jac_JR);
+			Jac_JT(0) = 0;
+			Jac_JT(1) = 1;
+			Jac_JT(2) = 0;
+			break;
+		case 5:
+			setZero(Jac_JR);
+			Jac_JT(0) = 0;
+			Jac_JT(1) = 0;
+			Jac_JT(2) = 1;
+			break;
+	}
+}
+
+static inline int jointNumDoFs(const JointType &type) {
+	switch (type) {
+		case FIXED:
+			return 0;
+		case REVOLUTE:
+		case PRISMATIC:
+			return 1;
+		case FLOATING:
+			return 6;
+	}
+	// this should never happen
+	error_message("invalid joint type\n");
+	// TODO add configurable abort/crash function
+	abort();
+}
+
+int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
+													  const bool initialize_matrix,
+													  const bool set_lower_triangular_matrix,
+													  matxx *mass_matrix) {
+// This calculates the joint space mass matrix for the multibody system.
+// The algorithm is essentially an implementation of "method 3"
+// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
+// (Later named "Composite Rigid Body Algorithm" by Featherstone).
+//
+// 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"
+					  "but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
+					  m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
+					  static_cast<int>(mass_matrix->cols()));
+		return -1;
+	}
+
+	// TODO add optimized zeroing function?
+	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);
+			}
+		}
+	}
+
+	if (update_kinematics) {
+		// 1. update relative kinematics
+		// 1.1 for revolute joints
+		for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+			RigidBody &body = m_body_list[m_body_revolute_list[i]];
+			// from reference orientation (q=0) of body-fixed frame to current orientation
+			mat33 body_T_body_ref;
+			bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref);
+			body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
+		}
+		// 1.2 for prismatic joints
+		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);
+		}
+		// 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;
+		}
+	}
+	for (int i = m_body_list.size() - 1; i >= 0; i--) {
+		RigidBody &body = m_body_list[i];
+		// calculate mass, center of mass and inertia of "composite rigid body",
+		// ie, sub-tree starting at current body
+		body.m_subtree_mass = body.m_mass;
+		body.m_body_subtree_mass_com = body.m_body_mass_com;
+		body.m_body_subtree_I_body = body.m_body_I_body;
+
+		for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) {
+			RigidBody &child = m_body_list[m_child_indices[i][c]];
+			mat33 body_T_child = child.m_body_T_parent.transpose();
+
+			body.m_subtree_mass += child.m_subtree_mass;
+			body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com +
+											child.m_parent_pos_parent_body * child.m_subtree_mass;
+			body.m_body_subtree_I_body +=
+				body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent;
+
+			if (child.m_subtree_mass > 0) {
+				// Shift the reference point for the child subtree inertia using the
+				// Huygens-Steiner ("parallel axis") theorem.
+				// (First shift from child origin to child com, then from there to this body's
+				// origin)
+				vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass;
+				mat33 tilde_r_child_com = tildeOperator(r_com);
+				mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com);
+				body.m_body_subtree_I_body +=
+					child.m_subtree_mass *
+					(tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com);
+			}
+		}
+	}
+
+	for (int i = m_body_list.size() - 1; i >= 0; i--) {
+		const RigidBody &body = m_body_list[i];
+
+		// determine DoF-range for body
+		const int q_index_min = body.m_q_index;
+		const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1;
+		// loop over the DoFs used by this body
+		// local joint jacobians (ok as is for 1-DoF joints)
+		vec3 Jac_JR = body.m_Jac_JR;
+		vec3 Jac_JT = body.m_Jac_JT;
+		for (int col = q_index_max; col >= q_index_min; col--) {
+			// set jacobians for 6-DoF joints
+			if (FLOATING == body.m_joint_type) {
+				setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
+			}
+
+			vec3 body_eom_rot =
+				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));
+
+			// rest of the mass matrix column upwards
+			{
+				// 1. for multi-dof joints, rest of the dofs of this body
+				for (int row = col - 1; row >= q_index_min; row--) {
+					if (FLOATING != body.m_joint_type) {
+						error_message("??\n");
+						return -1;
+					}
+					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);
+				}
+				// 2. ancestor dofs
+				int child_idx = i;
+				int parent_idx = m_parent_index[i];
+				while (parent_idx >= 0) {
+					const RigidBody &child_body = m_body_list[child_idx];
+					const RigidBody &parent_body = m_body_list[parent_idx];
+
+					const mat33 parent_T_child = child_body.m_body_T_parent.transpose();
+					body_eom_rot = parent_T_child * body_eom_rot;
+					body_eom_trans = parent_T_child * body_eom_trans;
+					body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans);
+
+					const int parent_body_q_index_min = parent_body.m_q_index;
+					const int parent_body_q_index_max =
+						parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
+					vec3 Jac_JR = parent_body.m_Jac_JR;
+					vec3 Jac_JT = parent_body.m_Jac_JT;
+					for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) {
+						// set jacobians for 6-DoF joints
+						if (FLOATING == parent_body.m_joint_type) {
+							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);
+					}
+
+					child_idx = parent_idx;
+					parent_idx = m_parent_index[child_idx];
+				}
+			}
+		}
+	}
+
+	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));
+			}
+		}
+	}
+
+#undef setMassMatrixElem
+	return 0;
+}
+
+// utility macro
+#define CHECK_IF_BODY_INDEX_IS_VALID(index)														\
+	do {																						   \
+		if (index < 0 || index >= m_num_bodies) {												  \
+			error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies);			 \
+			return -1;																			 \
+		}																						  \
+	} while (0)
+
+int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*p = m_parent_index[body_index];
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*user_int = m_user_int[body_index];
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*user_ptr = m_user_ptr[body_index];
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_user_int[body_index] = user_int;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_user_ptr[body_index] = user_ptr;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	*world_origin = body.m_body_T_world.transpose() * body.m_body_pos;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	if (body.m_mass > 0) {
+		*world_com = body.m_body_T_world.transpose() *
+					 (body.m_body_pos + body.m_body_mass_com / body.m_mass);
+	} else {
+		*world_com = body.m_body_T_world.transpose() * (body.m_body_pos);
+	}
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	*world_T_body = body.m_body_T_world.transpose();
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	*world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel;
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
+														vec3 *world_velocity) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	*world_velocity = body.m_body_T_world.transpose() * body.m_body_vel;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
+														   vec3 *world_velocity) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	vec3 com;
+	if (body.m_mass > 0) {
+		com = body.m_body_mass_com / body.m_mass;
+	} else {
+		com(0) = 0;
+		com(1) = 0;
+		com(2) = 0;
+	}
+
+	*world_velocity =
+		body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com));
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index,
+															 vec3 *world_dot_omega) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	*world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc;
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index,
+															vec3 *world_acceleration) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	const RigidBody &body = m_body_list[body_index];
+	*world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*joint_type = m_body_list[body_index].m_joint_type;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
+												  const char **joint_type) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*joint_type = jointTypeToString(m_body_list[body_index].m_joint_type);
+	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;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_body_list[body_index].m_mass = mass;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index,
+														 const vec3& first_mass_moment) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_body_list[body_index].m_body_mass_com = first_mass_moment;
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index,
+														  const mat33& second_mass_moment) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_body_list[body_index].m_body_I_body = second_mass_moment;
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*mass = m_body_list[body_index].m_mass;
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index,
+														 vec3 *first_mass_moment) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*first_mass_moment = m_body_list[body_index].m_body_mass_com;
+	return 0;
+}
+int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index,
+														  mat33 *second_mass_moment) const {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	*second_mass_moment = m_body_list[body_index].m_body_I_body;
+	return 0;
+}
+
+void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() {
+	for (int index = 0; index < m_num_bodies; index++) {
+		RigidBody &body = m_body_list[index];
+		setZero(body.m_body_force_user);
+		setZero(body.m_body_moment_user);
+	}
+}
+
+int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_body_list[body_index].m_body_force_user += body_force;
+	return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) {
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_body_list[body_index].m_body_moment_user += body_moment;
+	return 0;
+}
+
+}

src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp

@@ -0,0 +1,236 @@
+// The structs and classes defined here provide a basic inverse fynamics implementation used
+// by MultiBodyTree
+// User interaction should be through MultiBodyTree
+
+#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_
+#define MULTI_BODY_REFERENCE_IMPL_HPP_
+
+#include "../IDConfig.hpp"
+#include "../MultiBodyTree.hpp"
+
+namespace btInverseDynamics {
+
+/// Structure for for rigid body mass properties, connectivity and kinematic state
+/// all vectors and matrices are in body-fixed frame, if not indicated otherwise.
+/// The body-fixed frame is located in the joint connecting the body to its parent.
+struct RigidBody {
+	ID_DECLARE_ALIGNED_ALLOCATOR();
+	// 1 Inertial properties
+	/// Mass
+	idScalar m_mass;
+	/// Mass times center of gravity in body-fixed frame
+	vec3 m_body_mass_com;
+	/// Moment of inertia w.r.t. body-fixed frame
+	mat33 m_body_I_body;
+
+	// 2 dynamic properties
+	/// Left-hand side of the body equation of motion, translational part
+	vec3 m_eom_lhs_translational;
+	/// Left-hand side of the body equation of motion, rotational part
+	vec3 m_eom_lhs_rotational;
+	/// Force acting at the joint when the body is cut from its parent;
+	/// includes impressed joint force in J_JT direction,
+	/// as well as constraint force,
+	/// in body-fixed frame
+	vec3 m_force_at_joint;
+	/// Moment acting at the joint when the body is cut from its parent;
+	/// includes impressed joint moment in J_JR direction, and constraint moment
+	/// in body-fixed frame
+	vec3 m_moment_at_joint;
+	/// external (user provided) force acting at the body-fixed frame's origin, written in that
+	/// frame
+	vec3 m_body_force_user;
+	/// external (user provided) moment acting at the body-fixed frame's origin, written in that
+	/// frame
+	vec3 m_body_moment_user;
+	// 3 absolute kinematic properties
+	/// Position of body-fixed frame relative to world frame
+	/// this is currently only for debugging purposes
+	vec3 m_body_pos;
+	/// Absolute velocity of body-fixed frame
+	vec3 m_body_vel;
+	/// Absolute acceleration of body-fixed frame
+	/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
+	/// acceleration!
+	vec3 m_body_acc;
+	/// Absolute angular velocity
+	vec3 m_body_ang_vel;
+	/// Absolute angular acceleration
+	/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
+	/// acceleration!
+	vec3 m_body_ang_acc;
+
+	// 4 relative kinematic properties.
+	// these are in the parent body frame
+	/// Transform from world to body-fixed frame;
+	/// this is currently only for debugging purposes
+	mat33 m_body_T_world;
+	/// Transform from parent to body-fixed frame
+	mat33 m_body_T_parent;
+	/// Vector from parent to child frame in parent frame
+	vec3 m_parent_pos_parent_body;
+	/// Relative angular velocity
+	vec3 m_body_ang_vel_rel;
+	/// Relative linear velocity
+	vec3 m_parent_vel_rel;
+	/// Relative angular acceleration
+	vec3 m_body_ang_acc_rel;
+	/// Relative linear acceleration
+	vec3 m_parent_acc_rel;
+
+	// 5 Data describing the joint type and geometry
+	/// Type of joint
+	JointType m_joint_type;
+	/// Position of joint frame (body-fixed frame at q=0) relative to the parent frame
+	/// Components are in body-fixed frame of the parent
+	vec3 m_parent_pos_parent_body_ref;
+	/// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame
+	mat33 m_body_T_parent_ref;
+	/// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute
+	/// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
+	/// For revolute joints this is the joint axis, for prismatic joints it is a null matrix.
+	/// (NOTE: dimensions will have to be dynamic for additional joint types!)
+	vec3 m_Jac_JR;
+	/// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute
+	/// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
+	/// For prismatic joints this is the joint axis, for revolute joints it is a null matrix.
+	/// (NOTE: dimensions might have to be dynamic for additional joint types!)
+	vec3 m_Jac_JT;
+	/// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT
+	vec3 m_parent_Jac_JT;
+	/// Start of index range for the position degree(s) of freedom describing this body's motion
+	/// relative to
+	/// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates.
+	int m_q_index;
+
+	// 6 Scratch data for mass matrix computation using "composite rigid body algorithm"
+	/// mass of the subtree rooted in this body
+	idScalar m_subtree_mass;
+	/// center of mass * mass for subtree rooted in this body, in body-fixed frame
+	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;
+};
+
+/// The MBS implements a tree structured multibody system
+class MultiBodyTree::MultiBodyImpl {
+	friend class MultiBodyTree;
+
+public:
+	ID_DECLARE_ALIGNED_ALLOCATOR();
+
+	/// constructor
+	/// @param num_bodies the number of bodies in the system
+	/// @param num_dofs number of degrees of freedom in the system
+	MultiBodyImpl(int num_bodies_, int num_dofs_);
+
+	/// \copydoc MultiBodyTree::calculateInverseDynamics
+	int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
+								 vecx* joint_forces);
+	///\copydoc MultiBodyTree::calculateMassMatrix
+	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
+							const bool initialize_matrix, const bool set_lower_triangular_matrix,
+							matxx* mass_matrix);
+	/// generate additional index sets from the parent_index array
+	/// @return -1 on error, 0 on success
+	int generateIndexSets();
+	/// set gravity acceleration in world frame
+	/// @param gravity gravity vector in the world frame
+	/// @return 0 on success, -1 on error
+	int setGravityInWorldFrame(const vec3& gravity);
+	/// pretty print tree
+	void printTree();
+	/// print tree data
+	void printTreeData();
+	/// initialize fixed data
+	void calculateStaticData();
+	/// \copydoc MultiBodyTree::getBodyFrame
+	int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
+	/// \copydoc MultiBodyTree::getParentIndex
+	int getParentIndex(const int body_index, int* m_parent_index);
+	/// \copydoc MultiBodyTree::getJointType
+	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:getDoFOffset
+	int getDoFOffset(const int body_index, int* q_index) const;
+	/// \copydoc MultiBodyTree::getBodyOrigin
+	int getBodyOrigin(const int body_index, vec3* world_origin) const;
+	/// \copydoc MultiBodyTree::getBodyCoM
+	int getBodyCoM(const int body_index, vec3* world_com) const;
+	/// \copydoc MultiBodyTree::getBodyTransform
+	int getBodyTransform(const int body_index, mat33* world_T_body) const;
+	/// \copydoc MultiBodyTree::getBodyAngularVelocity
+	int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
+	/// \copydoc MultiBodyTree::getBodyLinearVelocity
+	int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
+	/// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
+	int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
+	/// \copydoc MultiBodyTree::getBodyAngularAcceleration
+	int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
+	/// \copydoc MultiBodyTree::getBodyLinearAcceleration
+	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
+	/// \copydoc MultiBodyTree::getUserInt
+	int getUserInt(const int body_index, int* user_int) const;
+	/// \copydoc MultiBodyTree::getUserPtr
+	int getUserPtr(const int body_index, void** user_ptr) const;
+	/// \copydoc MultiBodyTree::setUserInt
+	int setUserInt(const int body_index, const int user_int);
+	/// \copydoc MultiBodyTree::setUserPtr
+	int setUserPtr(const int body_index, void* const user_ptr);
+	///\copydoc MultiBodytTree::setBodyMass
+	int setBodyMass(const int body_index, const idScalar mass);
+	///\copydoc MultiBodytTree::setBodyFirstMassMoment
+	int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
+	///\copydoc MultiBodytTree::setBodySecondMassMoment
+	int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
+	///\copydoc MultiBodytTree::getBodyMass
+	int getBodyMass(const int body_index, idScalar* mass) const;
+	///\copydoc MultiBodytTree::getBodyFirstMassMoment
+	int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
+	///\copydoc MultiBodytTree::getBodySecondMassMoment
+	int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
+	/// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
+	void clearAllUserForcesAndMoments();
+	/// \copydoc MultiBodyTree::addUserForce
+	int addUserForce(const int body_index, const vec3& body_force);
+	/// \copydoc MultiBodyTree::addUserMoment
+	int addUserMoment(const int body_index, const vec3& body_moment);
+
+private:
+	// debug function. print tree structure to stdout
+	void printTree(int index, int indentation);
+	// get string representation of JointType (for debugging)
+	const char* jointTypeToString(const JointType& type) const;
+	// get number of degrees of freedom from joint type
+	int bodyNumDoFs(const JointType& type) const;
+	// number of bodies in the system
+	int m_num_bodies;
+	// number of degrees of freedom
+	int m_num_dofs;
+	// Gravitational acceleration (in world frame)
+	vec3 m_world_gravity;
+	// vector of bodies in the system
+	// body 0 is used as an environment body and is allways fixed.
+	// The bodies are ordered such that a parent body always has an index
+	// smaller than its child.
+	idArray<RigidBody>::type m_body_list;
+	// Parent_index[i] is the index for i's parent body in body_list.
+	// This fully describes the tree.
+	idArray<int>::type m_parent_index;
+	// child_indices[i] contains a vector of indices of
+	// all children of the i-th body
+	idArray<idArray<int>::type>::type m_child_indices;
+	// Indices of rotary joints
+	idArray<int>::type m_body_revolute_list;
+	// Indices of prismatic joints
+	idArray<int>::type m_body_prismatic_list;
+	// Indices of floating joints
+	idArray<int>::type m_body_floating_list;
+	// a user-provided integer
+	idArray<int>::type m_user_int;
+	// a user-provided pointer
+	idArray<void*>::type m_user_ptr;
+};
+}
+#endif

src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

@@ -0,0 +1,113 @@
+#include "MultiBodyTreeInitCache.hpp"
+
+namespace btInverseDynamics {
+
+MultiBodyTree::InitCache::InitCache() {
+	m_inertias.resize(0);
+	m_joints.resize(0);
+	m_num_dofs = 0;
+	m_root_index=-1;
+}
+
+int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
+									  const JointType joint_type,
+									  const vec3& parent_r_parent_body_ref,
+									  const mat33& body_T_parent_ref,
+									  const vec3& body_axis_of_motion, const idScalar mass,
+									  const vec3& body_r_body_com, const mat33& body_I_body,
+									  const int user_int, void* user_ptr) {
+	switch (joint_type) {
+		case REVOLUTE:
+		case PRISMATIC:
+			m_num_dofs += 1;
+			break;
+		case FIXED:
+			// does not add a degree of freedom
+			// m_num_dofs+=0;
+			break;
+		case FLOATING:
+			m_num_dofs += 6;
+			break;
+		default:
+			error_message("unknown joint type %d\n", joint_type);
+			return -1;
+	}
+
+	if(-1 == parent_index) {
+		if(m_root_index>=0) {
+			error_message("trying to add body %d as root, but already added %d as root body\n",
+						  body_index, m_root_index);
+			return -1;
+		}
+		m_root_index=body_index;
+	}
+
+	JointData joint;
+	joint.m_child = body_index;
+	joint.m_parent = parent_index;
+	joint.m_type = joint_type;
+	joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref;
+	joint.m_child_T_parent_ref = body_T_parent_ref;
+	joint.m_child_axis_of_motion = body_axis_of_motion;
+
+	InertiaData body;
+	body.m_mass = mass;
+	body.m_body_pos_body_com = body_r_body_com;
+	body.m_body_I_body = body_I_body;
+
+	m_inertias.push_back(body);
+	m_joints.push_back(joint);
+	m_user_int.push_back(user_int);
+	m_user_ptr.push_back(user_ptr);
+	return 0;
+}
+int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
+	if (index < 0 || index > static_cast<int>(m_inertias.size())) {
+		error_message("index out of range\n");
+		return -1;
+	}
+
+	*inertia = m_inertias[index];
+	return 0;
+}
+
+int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
+	if (index < 0 || index > static_cast<int>(m_user_int.size())) {
+		error_message("index out of range\n");
+		return -1;
+	}
+	*user_int = m_user_int[index];
+	return 0;
+}
+
+int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
+	if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
+		error_message("index out of range\n");
+		return -1;
+	}
+	*user_ptr = m_user_ptr[index];
+	return 0;
+}
+
+int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
+	if (index < 0 || index > static_cast<int>(m_joints.size())) {
+		error_message("index out of range\n");
+		return -1;
+	}
+	*joint = m_joints[index];
+	return 0;
+}
+
+int MultiBodyTree::InitCache::buildIndexSets() {
+	// NOTE: This function assumes that proper indices were provided
+	//	   User2InternalIndex from utils can be used to facilitate this.
+
+	m_parent_index.resize(numBodies());
+	for (idArrayIdx j = 0; j < m_joints.size(); j++) {
+		const JointData& joint = m_joints[j];
+		m_parent_index[joint.m_child] = joint.m_parent;
+	}
+
+	return 0;
+}
+}

src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp

@@ -0,0 +1,109 @@
+#ifndef MULTIBODYTREEINITCACHE_HPP_
+#define MULTIBODYTREEINITCACHE_HPP_
+
+#include "../IDConfig.hpp"
+#include "../IDMath.hpp"
+#include "../MultiBodyTree.hpp"
+
+namespace btInverseDynamics {
+/// Mass properties of a rigid body
+struct InertiaData {
+	ID_DECLARE_ALIGNED_ALLOCATOR();
+
+	/// mass
+	idScalar m_mass;
+	/// vector from body-fixed frame to center of mass,
+	/// in body-fixed frame, multiplied by the mass
+	vec3 m_body_pos_body_com;
+	/// moment of inertia w.r.t. the origin of the body-fixed
+	/// frame, represented in that frame
+	mat33 m_body_I_body;
+};
+
+/// Joint properties
+struct JointData {
+	ID_DECLARE_ALIGNED_ALLOCATOR();
+
+	/// type of joint
+	JointType m_type;
+	/// index of parent body
+	int m_parent;
+	/// index of child body
+	int m_child;
+	/// vector from parent's body-fixed frame to child's body-fixed
+	/// frame for q=0, written in the parent's body fixed frame
+	vec3 m_parent_pos_parent_child_ref;
+	/// Transform matrix converting vectors written in the parent's frame
+	/// into vectors written in the child's frame for q=0
+	/// ie, child_vector = child_T_parent_ref * parent_vector;
+	mat33 m_child_T_parent_ref;
+	/// Axis of motion for 1 degree-of-freedom joints,
+	/// written in the child's frame
+	/// For revolute joints, the q-value is positive for a positive
+	/// rotation about this axis.
+	/// For prismatic joints, the q-value is positive for a positive
+	/// translation is this direction.
+	vec3 m_child_axis_of_motion;
+};
+
+/// Data structure to store data passed by the user.
+/// This is used in MultiBodyTree::finalize to build internal data structures.
+class MultiBodyTree::InitCache {
+public:
+	ID_DECLARE_ALIGNED_ALLOCATOR();
+	/// constructor
+	InitCache();
+	///\copydoc MultiBodyTree::addBody
+	int addBody(const int body_index, const int parent_index, const JointType joint_type,
+				const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
+				const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com,
+				const mat33 &body_I_body, const int user_int, void *user_ptr);
+	/// build index arrays
+	/// @return 0 on success, -1 on failure
+	int buildIndexSets();
+	/// @return number of degrees of freedom
+	int numDoFs() const { return m_num_dofs; }
+	/// @return number of bodies
+	int numBodies() const { return m_inertias.size(); }
+	/// get inertia data for index
+	/// @param index of the body
+	/// @param inertia pointer for return data
+	/// @return 0 on success, -1 on failure
+	int getInertiaData(const int index, InertiaData *inertia) const;
+	/// get joint data for index
+	/// @param index of the body
+	/// @param joint pointer for return data
+	/// @return 0 on success, -1 on failure
+	int getJointData(const int index, JointData *joint) const;
+	/// get parent index array (paren_index[i] is the index of the parent of i)
+	/// @param parent_index pointer for return data
+	void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; }
+	/// get user integer
+	/// @param index body index
+	/// @param user_int user integer
+	/// @return 0 on success, -1 on failure
+	int getUserInt(const int index, int *user_int) const;
+	/// get user pointer
+	/// @param index body index
+	/// @param user_int user pointer
+	/// @return 0 on success, -1 on failure
+	int getUserPtr(const int index, void **user_ptr) const;
+
+private:
+	// vector of bodies
+	idArray<InertiaData>::type m_inertias;
+	// vector of joints
+	idArray<JointData>::type m_joints;
+	// number of mechanical degrees of freedom
+	int m_num_dofs;
+	// parent index array
+	idArray<int>::type m_parent_index;
+	// user integers
+	idArray<int>::type m_user_int;
+	// user pointers
+	idArray<void *>::type m_user_ptr;
+	// index of root body (or -1 if not set)
+	int m_root_index;
+};
+}
+#endif  // MULTIBODYTREEINITCACHE_HPP_

src/BulletInverseDynamics/premake4.lua

@@ -0,0 +1,12 @@
+	project "BulletInverseDynamics"
+
+	kind "StaticLib"
+	includedirs {
+		"..",
+	}
+	files {
+		"IDMath.cpp",
+		"MultiBodyTree.cpp",
+		"details/MultiBodyTreeInitCache.cpp",
+		"details/MultiBodyTreeImpl.cpp",
+	}

test/InverseDynamics/premake4.lua

@@ -0,0 +1,34 @@
+
+	project "Test_InverseDynamicsKinematics"
+
+	kind "ConsoleApp"
+
+--	defines {  }
+
+
+
+	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_kinematics.cpp",
+	}
+
+	if os.is("Linux") then
+                links {"pthread"}
+        end

test/InverseDynamics/test_invdyn_bullet.cpp

@@ -0,0 +1,115 @@
+/// create a bullet btMultiBody model of a tree structured multibody system,
+/// convert that model to a MultiBodyTree model.
+/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces
+///      - run forward dynamics on (q,u, forces) to get accelerations
+///      - compare input accelerations to inverse dynamics to output from forward dynamics
+
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <functional>
+#include <string>
+#include <random>
+
+#include <btBulletDynamicsCommon.h>
+#include <btMultiBodyTreeCreator.hpp>
+#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
+#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
+#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
+#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
+#include <../CommonInterfaces/CommonGUIHelperInterface.h>
+#include <gflags/gflags.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/Utils/b3ResourcePath.h"
+#include <invdyn_bullet_comparison.hpp>
+#include <btMultiBodyFromURDF.hpp>
+#include <MultiBodyTreeCreator.hpp>
+#include <MultiBodyTreeDebugGraph.hpp>
+
+using namespace btInverseDynamics;
+
+DEFINE_bool(verbose, false, "print extra info");
+
+static btVector3 gravity(0, 0, -10);
+static const bool kBaseFixed = false;
+static const char kUrdfFile[] = "r2d2.urdf";
+
+/// this test loads the a urdf model with fixed, floating, prismatic and rotational joints,
+/// converts in to an inverse dynamics model and compares forward to inverse dynamics for
+/// random input
+TEST(InvDynCompare, bulletUrdfR2D2) {
+    MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed);
+
+    char relativeFileName[1024];
+
+    ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024));
+
+    mb_load.setFileName(relativeFileName);
+    mb_load.init();
+
+    btMultiBodyTreeCreator id_creator;
+    btMultiBody *btmb = mb_load.getBtMultiBody();
+    ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0);
+
+    MultiBodyTree *id_tree = CreateMultiBodyTree(id_creator);
+    ASSERT_EQ(0x0 != id_tree, true);
+
+    vecx q(id_tree->numDoFs());
+    vecx u(id_tree->numDoFs());
+    vecx dot_u(id_tree->numDoFs());
+    vecx joint_forces(id_tree->numDoFs());
+
+    const int kNLoops = 10;
+    double max_pos_error = 0;
+    double max_acc_error = 0;
+
+    std::default_random_engine generator;
+    std::uniform_real_distribution<double> distribution(-M_PI, M_PI);
+    auto rnd = std::bind(distribution, generator);
+
+    for (int loop = 0; loop < kNLoops; loop++) {
+        for (int i = 0; i < q.size(); i++) {
+            q(i) = rnd();
+            u(i) = rnd();
+            dot_u(i) = rnd();
+        }
+
+        double pos_error;
+        double acc_error;
+        btmb->clearForcesAndTorques();
+        id_tree->clearAllUserForcesAndMoments();
+        // call inverse dynamics once, to get global position & velocity of root body
+        // (fixed, so q, u, dot_u arbitrary)
+        EXPECT_EQ(id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces), 0);
+
+        EXPECT_EQ(compareInverseAndForwardDynamics(q, u, dot_u, gravity, FLAGS_verbose, btmb, id_tree,
+                                                   &pos_error, &acc_error),
+                  0);
+
+        if (pos_error > max_pos_error) {
+            max_pos_error = pos_error;
+        }
+        if (acc_error > max_acc_error) {
+            max_acc_error = acc_error;
+        }
+    }
+
+    if (FLAGS_verbose) {
+        printf("max_pos_error= %e\n", max_pos_error);
+        printf("max_acc_error= %e\n", max_acc_error);
+    }
+
+    EXPECT_LT(max_pos_error, std::numeric_limits<idScalar>::epsilon()*1e4);
+    EXPECT_LT(max_acc_error, std::numeric_limits<idScalar>::epsilon()*1e5);
+}
+
+int main(int argc, char **argv) {
+    gflags::SetUsageMessage("Usage: invdyn_from_btmultibody -verbose = true/false");
+    gflags::ParseCommandLineFlags(&argc, &argv, false);
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/InverseDynamics/test_invdyn_kinematics.cpp

@@ -0,0 +1,372 @@
+// 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 "../Extras/InverseDynamics/CoilCreator.hpp"
+#include "../Extras/InverseDynamics/DillCreator.hpp"
+#include "../Extras/InverseDynamics/SimpleTreeCreator.hpp"
+#include "BulletInverseDynamics/MultiBodyTree.hpp"
+
+using namespace btInverseDynamics;
+
+const int kLevel = 5;
+const int kNumBodies = BT_ID_POW(2, kLevel);
+
+// template function for calculating the norm
+template <typename T>
+idScalar calculateNorm(T&);
+// only implemented for vec3
+template <>
+idScalar calculateNorm(vec3& v) {
+    return std::sqrt(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
+}
+
+// template function to convert a DiffType (finite differences)
+// to a ValueType. This is for angular velocity calculations
+// via finite differences.
+template <typename ValueType, typename DiffType>
+DiffType toDiffType(ValueType& fd, ValueType& val);
+
+// vector case: just return finite difference approximation
+template <>
+vec3 toDiffType(vec3& fd, vec3& val) {
+    return fd;
+}
+
+// orientation case: calculate spin tensor and extract angular velocity
+template <>
+vec3 toDiffType(mat33& fd, mat33& val) {
+    // spin tensor
+    mat33 omega_tilde = fd * val.transpose();
+    // extract vector from spin tensor
+    vec3 omega;
+    omega(0) = 0.5 * (omega_tilde(2, 1) - omega_tilde(1, 2));
+    omega(1) = 0.5 * (omega_tilde(0, 2) - omega_tilde(2, 0));
+    omega(2) = 0.5 * (omega_tilde(1, 0) - omega_tilde(0, 1));
+    return omega;
+}
+
+/// Class for calculating finite difference approximation
+/// of time derivatives and comparing it to an analytical solution
+/// DiffType and ValueType can be different, to allow comparison
+/// of angular velocity vectors and orientations given as transform matrices.
+template <typename ValueType, typename DiffType>
+class DiffFD {
+public:
+    DiffFD() : m_dt(0.0), m_num_updates(0), m_max_error(0.0), m_max_value(0.0), m_valid_fd(false) {}
+
+    void init(std::string name, idScalar dt) {
+        m_name = name;
+        m_dt = dt;
+        m_num_updates = 0;
+        m_max_error = 0.0;
+        m_max_value = 0.0;
+        m_valid_fd = false;
+    }
+
+    void update(const ValueType& val, const DiffType& true_diff) {
+        m_val = val;
+        if (m_num_updates > 2) {
+            // 2nd order finite difference approximation for d(value)/dt
+            ValueType diff_value_fd = (val - m_older_val) / (2.0 * m_dt);
+            // convert to analytical diff type. This is for angular velocities
+            m_diff_fd = toDiffType<ValueType, DiffType>(diff_value_fd, m_old_val);
+            // now, calculate the error
+            DiffType error_value_type = m_diff_fd - m_old_true_diff;
+            idScalar error = calculateNorm<DiffType>(error_value_type);
+            if (error > m_max_error) {
+                m_max_error = error;
+            }
+
+            idScalar value = calculateNorm<DiffType>(m_old_true_diff);
+            if (value > m_max_value) {
+                m_max_value = value;
+            }
+
+            m_valid_fd = true;
+        }
+        m_older_val = m_old_val;
+        m_old_val = m_val;
+        m_old_true_diff = true_diff;
+        m_num_updates++;
+        m_time += m_dt;
+    }
+
+    void printMaxError() {
+        printf("max_error: %e dt= %e max_value= %e fraction= %e\n", m_max_error, m_dt, m_max_value,
+               m_max_value > 0.0 ? m_max_error / m_max_value : 0.0);
+    }
+    void printCurrent() {
+        if (m_valid_fd) {
+            // note: m_old_true_diff already equals m_true_diff here, so values are not aligned.
+            //      (but error calculation takes this into account)
+            printf("%s time: %e fd: %e %e %e true: %e %e %e\n", m_name.c_str(), m_time,
+                   m_diff_fd(0), m_diff_fd(1), m_diff_fd(2), m_old_true_diff(0), m_old_true_diff(1),
+                   m_old_true_diff(2));
+        }
+    }
+
+    idScalar getMaxError() const { return m_max_error; }
+    idScalar getMaxValue() const { return m_max_value; }
+
+private:
+    idScalar m_dt;
+    ValueType m_val;
+    ValueType m_old_val;
+    ValueType m_older_val;
+    DiffType m_old_true_diff;
+    DiffType m_diff_fd;
+    int m_num_updates;
+    idScalar m_max_error;
+    idScalar m_max_value;
+    idScalar m_time;
+    std::string m_name;
+    bool m_valid_fd;
+};
+
+template <typename ValueType, typename DiffType>
+class VecDiffFD {
+public:
+    VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) {
+        for (int i = 0; i < m_fd.size(); i++) {
+            char buf[256];
+            BT_ID_SNPRINTF(buf, 256, "%s-%.2d", name.c_str(), i);
+            m_fd[i].init(buf, dt);
+        }
+    }
+    void update(int i, ValueType& val, DiffType& true_diff) { m_fd[i].update(val, true_diff); }
+    idScalar getMaxError() const {
+        idScalar max_error = 0;
+        for (int i = 0; i < m_fd.size(); i++) {
+            const idScalar error = m_fd[i].getMaxError();
+            if (error > max_error) {
+                max_error = error;
+            }
+        }
+        return max_error;
+    }
+    idScalar getMaxValue() const {
+        idScalar max_value = 0;
+        for (int i = 0; i < m_fd.size(); i++) {
+            const idScalar value = m_fd[i].getMaxValue();
+            if (value > max_value) {
+                max_value= value;
+            }
+        }
+        return max_value;
+    }
+    void printMaxError() {
+        printf("%s: total  dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError());
+    }
+
+    void printCurrent() {
+        for (int i = 0; i < m_fd.size(); i++) {
+            m_fd[i].printCurrent();
+        }
+    }
+
+private:
+    std::string m_name;
+    std::vector<DiffFD<ValueType, DiffType> > m_fd;
+    const idScalar m_dt;
+    idScalar m_max_error;
+};
+
+// calculate maximum difference between finite difference and analytical differentiation
+int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar deltaT,
+                                  idScalar endTime, idScalar* max_linear_velocity_error,
+                                  idScalar* max_angular_velocity_error,
+                                  idScalar* max_linear_acceleration_error,
+                                  idScalar* max_angular_acceleration_error) {
+    // setup system
+    MultiBodyTree* tree = CreateMultiBodyTree(creator);
+    if (0x0 == tree) {
+        return -1;
+    }
+    // set gravity to zero, so nothing is added to accelerations in forward kinematics
+    vec3 gravity_zero;
+    gravity_zero(0) = 0;
+    gravity_zero(1) = 0;
+    gravity_zero(2) = 0;
+    tree->setGravityInWorldFrame(gravity_zero);
+    //
+    const idScalar kAmplitude = 1.0;
+    const idScalar kFrequency = 1.0;
+
+    vecx q(tree->numDoFs());
+    vecx dot_q(tree->numDoFs());
+    vecx ddot_q(tree->numDoFs());
+    vecx joint_forces(tree->numDoFs());
+
+    VecDiffFD<vec3, vec3> fd_vel("linear-velocity", tree->numBodies(), deltaT);
+    VecDiffFD<vec3, vec3> fd_acc("linear-acceleration", tree->numBodies(), deltaT);
+    VecDiffFD<mat33, vec3> fd_omg("angular-velocity", tree->numBodies(), deltaT);
+    VecDiffFD<vec3, vec3> fd_omgd("angular-acceleration", tree->numBodies(), deltaT);
+
+    for (idScalar t = 0.0; t < endTime; t += deltaT) {
+        for (int body = 0; body < tree->numBodies(); body++) {
+            q(body) = kAmplitude * sin(t * 2.0 * BT_ID_PI * kFrequency);
+            dot_q(body) = kAmplitude * 2.0 * BT_ID_PI * kFrequency * cos(t * 2.0 * BT_ID_PI * kFrequency);
+            ddot_q(body) =
+                -kAmplitude * pow(2.0 * BT_ID_PI * kFrequency, 2) * sin(t * 2.0 * BT_ID_PI * kFrequency);
+        }
+
+        if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) {
+            delete tree;
+            return -1;
+        }
+
+        // position/velocity
+        for (int body = 0; body < tree->numBodies(); body++) {
+            vec3 pos;
+            vec3 vel;
+            mat33 world_T_body;
+            vec3 omega;
+            vec3 dot_omega;
+            vec3 acc;
+
+            tree->getBodyOrigin(body, &pos);
+            tree->getBodyTransform(body, &world_T_body);
+            tree->getBodyLinearVelocity(body, &vel);
+            tree->getBodyAngularVelocity(body, &omega);
+            tree->getBodyLinearAcceleration(body, &acc);
+            tree->getBodyAngularAcceleration(body, &dot_omega);
+
+            fd_vel.update(body, pos, vel);
+            fd_omg.update(body, world_T_body, omega);
+            fd_acc.update(body, vel, acc);
+            fd_omgd.update(body, omega, dot_omega);
+
+
+//        fd_vel.printCurrent();
+//fd_acc.printCurrent();
+//fd_omg.printCurrent();
+//fd_omgd.printCurrent();
+        }
+    }
+
+    *max_linear_velocity_error = fd_vel.getMaxError()/fd_vel.getMaxValue();
+    *max_angular_velocity_error = fd_omg.getMaxError()/fd_omg.getMaxValue();
+    *max_linear_acceleration_error = fd_acc.getMaxError()/fd_acc.getMaxValue();
+    *max_angular_acceleration_error = fd_omgd.getMaxError()/fd_omgd.getMaxValue();
+
+    delete tree;
+    return 0;
+}
+
+// first test: absolute difference between numerical and numerial
+// differentiation should be small
+TEST(InvDynKinematicsDifferentiation, errorAbsolute) {
+    //CAVEAT:these values are hand-tuned to work for the specific trajectory defined above.
+#ifdef BT_ID_USE_DOUBLE_PRECISION
+    const idScalar kDeltaT = 1e-7;
+	const idScalar kAcceptableError = 1e-4;
+#else
+    const idScalar kDeltaT = 1e-4;
+	const idScalar kAcceptableError = 5e-3;
+#endif
+    const idScalar kDuration = 0.01;
+    
+
+    CoilCreator coil_creator(kNumBodies);
+    DillCreator dill_creator(kLevel);
+    SimpleTreeCreator simple_creator(kNumBodies);
+
+    idScalar max_linear_velocity_error;
+    idScalar max_angular_velocity_error;
+    idScalar max_linear_acceleration_error;
+    idScalar max_angular_acceleration_error;
+
+    // test serial chain
+    calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error,
+                                  &max_angular_velocity_error, &max_linear_acceleration_error,
+                                  &max_angular_acceleration_error);
+
+    EXPECT_LT(max_linear_velocity_error, kAcceptableError);
+    EXPECT_LT(max_angular_velocity_error, kAcceptableError);
+    EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
+    EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
+
+    // test branched tree
+    calculateDifferentiationError(dill_creator, kDeltaT, kDuration, &max_linear_velocity_error,
+                                  &max_angular_velocity_error, &max_linear_acceleration_error,
+                                  &max_angular_acceleration_error);
+
+    EXPECT_LT(max_linear_velocity_error, kAcceptableError);
+    EXPECT_LT(max_angular_velocity_error, kAcceptableError);
+    EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
+    EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
+
+    // test system with different joint types
+    calculateDifferentiationError(simple_creator, kDeltaT, kDuration, &max_linear_velocity_error,
+                                  &max_angular_velocity_error, &max_linear_acceleration_error,
+                                  &max_angular_acceleration_error);
+
+    EXPECT_LT(max_linear_velocity_error, kAcceptableError);
+    EXPECT_LT(max_angular_velocity_error, kAcceptableError);
+    EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
+    EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
+}
+
+// second test: check if the change in the differentiation error
+// is consitent with the second order approximation, ie, error ~ O(dt^2)
+TEST(InvDynKinematicsDifferentiation, errorOrder) {
+    const idScalar kDeltaTs[2] = {1e-4, 1e-5};
+    const idScalar kDuration = 1e-2;
+
+    CoilCreator coil_creator(kNumBodies);
+    //    DillCreator dill_creator(kLevel);
+    //    SimpleTreeCreator simple_creator(kNumBodies);
+
+    idScalar max_linear_velocity_error[2];
+    idScalar max_angular_velocity_error[2];
+    idScalar max_linear_acceleration_error[2];
+    idScalar max_angular_acceleration_error[2];
+
+    // test serial chain
+    calculateDifferentiationError(coil_creator, kDeltaTs[0], kDuration,
+                                  &max_linear_velocity_error[0], &max_angular_velocity_error[0],
+                                  &max_linear_acceleration_error[0],
+                                  &max_angular_acceleration_error[0]);
+
+    calculateDifferentiationError(coil_creator, kDeltaTs[1], kDuration,
+                                  &max_linear_velocity_error[1], &max_angular_velocity_error[1],
+                                  &max_linear_acceleration_error[1],
+                                  &max_angular_acceleration_error[1]);
+
+    const idScalar expected_linear_velocity_error_1 =
+        max_linear_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
+    const idScalar expected_angular_velocity_error_1 =
+        max_angular_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
+    const idScalar expected_linear_acceleration_error_1 =
+        max_linear_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
+    const idScalar expected_angular_acceleration_error_1 =
+        max_angular_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
+
+    printf("linear vel error: %e %e  %e\n", max_linear_velocity_error[1],
+           expected_linear_velocity_error_1,
+           max_linear_velocity_error[1] - expected_linear_velocity_error_1);
+    printf("angular vel error: %e %e  %e\n", max_angular_velocity_error[1],
+           expected_angular_velocity_error_1,
+           max_angular_velocity_error[1] - expected_angular_velocity_error_1);
+    printf("linear acc error: %e %e  %e\n", max_linear_acceleration_error[1],
+           expected_linear_acceleration_error_1,
+           max_linear_acceleration_error[1] - expected_linear_acceleration_error_1);
+    printf("angular acc error: %e %e  %e\n", max_angular_acceleration_error[1],
+           expected_angular_acceleration_error_1,
+           max_angular_acceleration_error[1] - expected_angular_acceleration_error_1);
+}
+
+int main(int argc, char** argv) {
+
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+
+    return EXIT_SUCCESS;
+}