[InverseDynamics] Support for Jacobians & derivatives

This change adds support for calculating Jacobians and
dot(Jacobian)*u terms, along with the required support for
the 3xN matrices in the standalone math library.
It also adds functions to compute kinematics only (position, velocity, accel).
To facilitate tests, the Cl also adds a RandomTreeCreator to create
randomized multibody trees.
Thanks to Thomas Buschmann for this contribution!

Extras/InverseDynamics/CMakeLists.txt

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

Extras/InverseDynamics/CloneTreeCreator.cpp

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

Extras/InverseDynamics/CloneTreeCreator.hpp

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

Extras/InverseDynamics/IDRandomUtil.cpp

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

Extras/InverseDynamics/IDRandomUtil.hpp

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

Extras/InverseDynamics/RandomTreeCreator.cpp

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

Extras/InverseDynamics/RandomTreeCreator.hpp

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

Extras/InverseDynamics/btMultiBodyFromURDF.hpp

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

Extras/InverseDynamics/btMultiBodyTreeCreator.hpp

@@ -25,10 +25,10 @@ public:
     /// \copydoc MultiBodyTreeCreator::getNumBodies
     int getNumBodies(int *num_bodies) const;
     ///\copydoc MultiBodyTreeCreator::getBody
-    int getBody(const int body_index, int *parent_index, JointType *joint_type,
+	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,
+		vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
-                idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
+		vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com,
-                void **user_ptr) const;
+		mat33 *body_I_body, int *user_int, void **user_ptr) const;
 
 private:
     // internal struct holding data extracted from btMultiBody

Extras/InverseDynamics/invdyn_bullet_comparison.hpp

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

src/BulletInverseDynamics/IDConfig.hpp

@@ -2,8 +2,17 @@
 ///	  such as choice of linear algebra library and underlying scalar type
 #ifndef IDCONFIG_HPP_
 #define IDCONFIG_HPP_
+
+// If true, enable jacobian calculations.
+// This adds a 3xN matrix to every body, + 2 3-Vectors.
+// so it is not advised for large systems if it is not absolutely necessary.
+// Also, this is not required for standard inverse dynamics calculations.
+// Will only work with vector math libraries that support 3xN matrices.
+#define BT_ID_WITH_JACOBIANS
+
 // If we have a custom configuration, compile without using other parts of bullet.
 #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
+#include <cmath>
 #define BT_ID_WO_BULLET
 #define BT_ID_POW(a,b) std::pow(a,b)
 #define BT_ID_SNPRINTF snprintf
@@ -25,7 +34,7 @@
 #define INVDYN_INCLUDE_HELPER_2(x) #x
 #define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
 #include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
-#ifndef btInverseDynamics 
+#ifndef btInverseDynamics
 #error "custom inverse dynamics config, but no custom namespace defined"
 #endif
 

src/BulletInverseDynamics/IDConfigEigen.hpp

@@ -21,20 +21,7 @@ struct idArray {
 typedef std::vector<int>::size_type idArrayIdx;
 // default to standard malloc/free
 #include <cstdlib>
-#define idMalloc ::malloc
+#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-#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!

src/BulletInverseDynamics/IDMath.cpp

@@ -55,6 +55,60 @@ idScalar maxAbs(const vec3 &v) {
 	return result;
 }
 
+#if (defined BT_ID_HAVE_MAT3X)
+idScalar maxAbsMat3x(const mat3x &m) {
+    // only used for tests -- so just loop here for portability
+    idScalar result = 0.0;
+    for (idArrayIdx col = 0; col < m.cols(); col++) {
+        for (idArrayIdx row = 0; row < 3; row++) {
+            result = BT_ID_MAX(result, std::fabs(m(row, col)));
+        }
+    }
+    return result;
+}
+
+void mul(const mat33 &a, const mat3x &b, mat3x *result) {
+    if (b.cols() != result->cols()) {
+        error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+                      static_cast<int>(b.cols()), static_cast<int>(result->cols()));
+        abort();
+    }
+
+    for (idArrayIdx col = 0; col < b.cols(); col++) {
+        const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col);
+        const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col);
+        const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col);
+        setMat3xElem(0, col, x, result);
+        setMat3xElem(1, col, y, result);
+        setMat3xElem(2, col, z, result);
+    }
+}
+void add(const mat3x &a, const mat3x &b, mat3x *result) {
+    if (a.cols() != b.cols()) {
+        error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+                      static_cast<int>(a.cols()), static_cast<int>(b.cols()));
+        abort();
+    }
+    for (idArrayIdx col = 0; col < b.cols(); col++) {
+        for (idArrayIdx row = 0; row < 3; row++) {
+            setMat3xElem(row, col, a(row, col) + b(row, col), result);
+        }
+    }
+}
+void sub(const mat3x &a, const mat3x &b, mat3x *result) {
+    if (a.cols() != b.cols()) {
+        error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+                      static_cast<int>(a.cols()), static_cast<int>(b.cols()));
+        abort();
+    }
+    for (idArrayIdx col = 0; col < b.cols(); col++) {
+        for (idArrayIdx row = 0; row < 3; row++) {
+            setMat3xElem(row, col, a(row, col) - b(row, col), result);
+        }
+    }
+}
+#endif
+
 mat33 transformX(const idScalar &alpha) {
 	mat33 T;
 	const idScalar cos_alpha = std::cos(alpha);

src/BulletInverseDynamics/IDMath.hpp

@@ -19,6 +19,16 @@ idScalar maxAbs(const vecx& v);
 /// return maximum absolute value
 idScalar maxAbs(const vec3& v);
 #endif  //ID_LINEAR_MATH_USE_EIGEN
+
+#if (defined BT_ID_HAVE_MAT3X)
+idScalar maxAbsMat3x(const mat3x& m);
+void setZero(mat3x&m);
+// define math functions on mat3x here to avoid allocations in operators.
+void mul(const mat33&a, const mat3x&b, mat3x* result);
+void add(const mat3x&a, const mat3x&b, mat3x* result);
+void sub(const mat3x&a, const mat3x&b, mat3x* result);
+#endif
+
 /// get offset vector & transform matrix from DH parameters
 /// TODO: add documentation
 void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);

src/BulletInverseDynamics/MultiBodyTree.cpp

@@ -61,6 +61,18 @@ int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_a
 	return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
 }
 
+int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const {
+    return m_impl->getParentRParentBodyRef(body_index, r);
+}
+
+int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const {
+    return m_impl->getBodyTParentRef(body_index, T);
+}
+
+int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const {
+    return m_impl->getBodyAxisOfMotion(body_index, axis);
+}
+
 void MultiBodyTree::printTree() { m_impl->printTree(); }
 void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
 
@@ -96,9 +108,106 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati
 	}
 	return 0;
 }
+
 int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
 	return calculateMassMatrix(q, true, true, true, mass_matrix);
 }
+
+
+
+int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) {
+    vec3 world_gravity(m_impl->m_world_gravity);
+    // temporarily set gravity to zero, to ensure we get the actual accelerations
+    setZero(m_impl->m_world_gravity);
+
+    if (false == m_is_finalized) {
+        error_message("system has not been initialized\n");
+        return -1;
+    }
+    if (-1 == m_impl->calculateKinematics(q, u, dot_u,
+                                          MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
+        error_message("error in kinematics calculation\n");
+        return -1;
+    }
+
+    m_impl->m_world_gravity=world_gravity;
+    return 0;
+}
+
+
+int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
+	if (false == m_is_finalized) {
+		error_message("system has not been initialized\n");
+		return -1;
+	}
+	if (-1 == m_impl->calculateKinematics(q, q, q,
+                                              MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+		error_message("error in kinematics calculation\n");
+		return -1;
+	}
+	return 0;
+}
+
+int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
+	if (false == m_is_finalized) {
+		error_message("system has not been initialized\n");
+		return -1;
+	}
+	if (-1 == m_impl->calculateKinematics(q, u, u,
+                                              MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+		error_message("error in kinematics calculation\n");
+		return -1;
+	}
+	return 0;
+}
+
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
+    if (false == m_is_finalized) {
+        error_message("system has not been initialized\n");
+        return -1;
+    }
+    if (-1 == m_impl->calculateJacobians(q, u,
+                                         MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+        error_message("error in jacobian calculation\n");
+        return -1;
+    }
+    return 0;
+}
+
+int MultiBodyTree::calculateJacobians(const vecx& q){
+    if (false == m_is_finalized) {
+        error_message("system has not been initialized\n");
+        return -1;
+    }
+    if (-1 == m_impl->calculateJacobians(q, q,
+                                         MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
+        error_message("error in jacobian calculation\n");
+        return -1;
+    }
+    return 0;
+}
+
+int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
+    return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u);
+}
+
+int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const {
+    return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u);
+}
+
+int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const {
+    return m_impl->getBodyJacobianTrans(body_index,world_jac_trans);
+}
+
+int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const  {
+    return m_impl->getBodyJacobianRot(body_index,world_jac_rot);
+}
+
+
+#endif
+
 int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
 						   const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
 						   const vec3 &body_axis_of_motion_, idScalar mass,
@@ -186,6 +295,11 @@ int MultiBodyTree::finalize() {
 	const int &num_bodies = m_init_cache->numBodies();
 	const int &num_dofs = m_init_cache->numDoFs();
 
+        if(num_dofs<=0) {
+            error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
+            //return -1;
+        }
+
 	// 1 allocate internal MultiBody structure
 	m_impl = new MultiBodyImpl(num_bodies, num_dofs);
 

src/BulletInverseDynamics/MultiBodyTree.hpp

@@ -51,6 +51,7 @@ enum JointType {
 ///	  - gears and motor inertia
 class MultiBodyTree {
 public:
+        ID_DECLARE_ALIGNED_ALLOCATOR();
 	/// The contructor.
 	/// Initialization & allocation is via addBody and buildSystem calls.
 	MultiBodyTree();
@@ -117,7 +118,10 @@ public:
 	void printTree();
 	/// print tree data to stdout
 	void printTreeData();
-	/// calculate joint forces for given generalized state & derivatives
+	/// Calculate joint forces for given generalized state & derivatives.
+        /// This also updates kinematic terms computed in calculateKinematics.
+        /// If gravity is not set to zero, acceleration terms will contain
+        /// gravitational acceleration.
 	/// @param q generalized coordinates
 	/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
 	/// @param dot_u time derivative of u
@@ -148,6 +152,31 @@ public:
 	/// @return -1 on error, 0 on success
 	int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
 
+
+        /// Calculates kinematics also calculated in calculateInverseDynamics,
+        /// but not dynamics.
+        /// This function ensures that correct accelerations are computed that do not
+        /// contain gravitational acceleration terms.
+        /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
+        int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
+        /// Calculate position kinematics
+        int calculatePositionKinematics(const vecx& q);
+         /// Calculate position and velocity kinematics
+        int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+        /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
+        /// d(Jacobian)/dt*u
+        /// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
+        /// or calculatePositionAndVelocityKinematics
+        int calculateJacobians(const vecx& q, const vecx& u);
+        /// Calculate Jacobians (dvel/du)
+        /// This function assumes that calculateInverseDynamics was called, or
+        /// one of the calculateKineamtics functions
+        int calculateJacobians(const vecx& q);
+#endif // BT_ID_HAVE_MAT3X
+
+
 	/// set gravitational acceleration
 	/// the default is [0;0;-9.8] in the world frame
 	/// @param gravity the gravitational acceleration in world frame
@@ -200,9 +229,20 @@ public:
 	/// @param world_origin pointer for return data
 	/// @return 0 on success, -1 on error
 	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+        // get translational jacobian, in world frame (dworld_velocity/du)
+        int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
+        // get rotational jacobian, in world frame (dworld_omega/du)
+        int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
+        // get product of translational jacobian derivative * generatlized velocities
+        int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
+        // get product of rotational jacobian derivative * generatlized velocities
+        int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
+#endif // BT_ID_HAVE_MAT3X
+
 	/// returns the (internal) index of body
-	/// @param body_index is the index of a body (internal: TODO: fix/clarify
+	/// @param body_index is the index of a body
-	/// 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;
@@ -216,6 +256,21 @@ public:
 	/// @param joint_type string naming the corresponding joint type
 	/// @return 0 on success, -1 on failure
 	int getJointTypeStr(const int body_index, const char** joint_type) const;
+    	/// get offset translation to parent body (see addBody)
+	/// @param body_index index of the body
+	/// @param r the offset translation (see above)
+	/// @return 0 on success, -1 on failure
+        int getParentRParentBodyRef(const int body_index, vec3* r) const;
+	/// get offset rotation to parent body (see addBody)
+	/// @param body_index index of the body
+	/// @param T the transform (see above)
+	/// @return 0 on success, -1 on failure
+        int getBodyTParentRef(const int body_index, mat33* T) const;
+	/// get axis of motion (see addBody)
+	/// @param body_index index of the body
+	/// @param axis the axis (see above)
+	/// @return 0 on success, -1 on failure
+        int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
 	/// get offset for degrees of freedom of this body into the q-vector
 	/// @param body_index index of the body
 	/// @param q_offset offset the q vector

src/BulletInverseDynamics/details/IDEigenInterface.hpp

@@ -2,16 +2,35 @@
 #define INVDYNEIGENINTERFACE_HPP_
 #include "../IDConfig.hpp"
 namespace btInverseDynamics {
+
+#define BT_ID_HAVE_MAT3X
+
 #ifdef BT_USE_DOUBLE_PRECISION
-typedef Eigen::VectorXd vecx;
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
-typedef Eigen::Vector3d vec3;
+typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> vec3;
-typedef Eigen::Matrix3d mat33;
+typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> mat33;
-typedef Eigen::MatrixXd matxx;
+typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
+typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
 #else
-typedef Eigen::VectorXf vecx;
+typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
-typedef Eigen::Vector3f vec3;
+typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> vec3;
-typedef Eigen::Matrix3f mat33;
+typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> mat33;
-typedef Eigen::MatrixXf matxx;
+typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
-#endif  //
+typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
+#endif
+
+inline void resize(mat3x &m, Eigen::Index size) {
+    m.resize(3, size);
+    m.setZero();
+}
+
+inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
+    (*m)(row, col) = val;
+}
+
+inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
+    (*m)(row, col) = val;
+}
+
 }
 #endif  // INVDYNEIGENINTERFACE_HPP_

src/BulletInverseDynamics/details/IDLinearMathInterface.hpp

@@ -8,13 +8,13 @@
 #include "../../LinearMath/btMatrix3x3.h"
 #include "../../LinearMath/btVector3.h"
 #include "../../LinearMath/btMatrixX.h"
+#define BT_ID_HAVE_MAT3X
 
 namespace btInverseDynamics {
 class vec3;
 class vecx;
 class mat33;
 typedef btMatrixX<idScalar> matxx;
-typedef btVectorX<idScalar> vecxx;
 
 class vec3 : public btVector3 {
 public:
@@ -47,11 +47,11 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s)
 
 inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
 
-class vecx : public vecxx {
+class vecx : public btVectorX<idScalar> {
 public:
-	vecx(int size) : vecxx(size) {}
+	vecx(int size) : btVectorX(size) {}
-	const vecx& operator=(const vecxx& rhs) {
+	const vecx& operator=(const btVectorX<idScalar>& rhs) {
-		*static_cast<vecxx*>(this) = rhs;
+		*static_cast<btVectorX*>(this) = rhs;
 		return *this;
 	}
 
@@ -108,6 +108,65 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
 
 	return result;
 }
+
+// use btMatrixX to implement 3xX matrix
+class mat3x : public matxx {
+public:
+    mat3x(){}
+    mat3x(const mat3x&rhs) {
+        matxx::resize(rhs.rows(), rhs.cols());
+        *this = rhs;
+    }
+    mat3x(int rows, int cols): matxx(3,cols) {
+    }
+    void operator=(const mat3x& rhs) {
+	if (m_cols != rhs.m_cols) {
+            error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+            abort();
+	}
+        for(int i=0;i<rows();i++) {
+            for(int k=0;k<cols();k++) {
+                setElem(i,k,rhs(i,k));
+            }
+        }
+    }
+    void setZero() {
+        matxx::setZero();
+    }
+};
+
+
+inline vec3 operator*(const mat3x& a, const vecx& b) {
+    vec3 result;
+    if (a.cols() != b.size()) {
+        error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+        abort();
+    }
+    result(0)=0.0;
+    result(1)=0.0;
+    result(2)=0.0;
+    for(int i=0;i<b.size();i++) {
+        for(int k=0;k<3;k++) {
+            result(k)+=a(k,i)*b(i);
+        }
+    }
+    return result;
+}
+
+
+inline void resize(mat3x &m, idArrayIdx size) {
+    m.resize(3, size);
+    m.setZero();
+}
+
+inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
+    m->setElem(row, col, val);
+}
+
+inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
+    m->setElem(row, col, val);
+}
+
 }
 
 #endif  // IDLINEARMATHINTERFACE_HPP_

src/BulletInverseDynamics/details/IDMatVec.hpp

@@ -5,12 +5,14 @@
 #include <cstdlib>
 
 #include "../IDConfig.hpp"
+#define BT_ID_HAVE_MAT3X
 
 namespace btInverseDynamics {
 class vec3;
 class vecx;
 class mat33;
 class matxx;
+class mat3x;
 
 /// This is a very basic implementation to enable stand-alone use of the library.
 /// The implementation is not really optimized and misses many features that you would
@@ -85,13 +87,17 @@ private:
 
 class matxx {
 public:
+    matxx() {
+        m_data = 0x0;
+        m_cols=0;
+        m_rows=0;
+    }
 	matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
 		m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
 	}
 	~matxx() { idFree(m_data); }
-	const matxx& operator=(const matxx& rhs);
+	idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
-	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_cols + 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; }
 
@@ -101,6 +107,61 @@ private:
 	idScalar* m_data;
 };
 
+class mat3x {
+public:
+    mat3x() {
+        m_data = 0x0;
+        m_cols=0;
+    }
+    mat3x(const mat3x&rhs) {
+        m_cols=rhs.m_cols;
+        allocate();
+        *this = rhs;
+    }
+    mat3x(int rows, int cols): m_cols(cols) {
+        allocate();
+    };
+    void operator=(const mat3x& rhs) {
+	if (m_cols != rhs.m_cols) {
+            error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+            abort();
+	}
+        for(int i=0;i<3*m_cols;i++) {
+            m_data[i] = rhs.m_data[i];
+        }
+    }
+
+    ~mat3x() {
+        free();
+    }
+    idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
+    const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
+    int rows() const { return m_rows; }
+    const int& cols() const { return m_cols; }
+    void resize(int rows, int cols) {
+        m_cols=cols;
+        free();
+        allocate();
+    }
+    void setZero() {
+        memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols);
+    }
+    // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
+private:
+    void allocate(){m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));}
+    void free() { idFree(m_data);}
+    enum {m_rows=3};
+    int m_cols;
+    idScalar* m_data;
+};
+
+inline void resize(mat3x &m, idArrayIdx size) {
+    m.resize(3, size);
+    m.setZero();
+}
+
+//////////////////////////////////////////////////
+// Implementations
 inline const vec3& vec3::operator=(const vec3& rhs) {
 	if (&rhs != this) {
 		memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
@@ -324,5 +385,31 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
 
 	return result;
 }
+
+inline vec3 operator*(const mat3x& a, const vecx& b) {
+    vec3 result;
+    if (a.cols() != b.size()) {
+        error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+        abort();
+    }
+    result(0)=0.0;
+    result(1)=0.0;
+    result(2)=0.0;
+    for(int i=0;i<b.size();i++) {
+        for(int k=0;k<3;k++) {
+            result(k)+=a(k,i)*b(i);
+        }
+    }
+    return result;
 }
+
+inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
+    (*m)(row, col) = val;
+}
+
+inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
+    (*m)(row, col) = val;
+}
+
+} // namespace btInverseDynamcis
 #endif

src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -3,7 +3,15 @@
 namespace btInverseDynamics {
 
 MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
-	: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
+	: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+        ,m_m3x(3,m_num_dofs)
+#endif
+{
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+        resize(m_m3x,m_num_dofs);
+#endif
 	m_body_list.resize(num_bodies_);
 	m_parent_index.resize(num_bodies_);
 	m_child_indices.resize(num_bodies_);
@@ -205,6 +213,20 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
 				// no static data
 				break;
 		}
+
+       // resize & initialize jacobians to zero.
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+        body.m_body_dot_Jac_T_u(0) = 0.0;
+        body.m_body_dot_Jac_T_u(1) = 0.0;
+        body.m_body_dot_Jac_T_u(2) = 0.0;
+        body.m_body_dot_Jac_R_u(0) = 0.0;
+        body.m_body_dot_Jac_R_u(1) = 0.0;
+        body.m_body_dot_Jac_R_u(2) = 0.0;
+        resize(body.m_body_Jac_T,m_num_dofs);
+        resize(body.m_body_Jac_R,m_num_dofs);
+        body.m_body_Jac_T.setZero();
+        body.m_body_Jac_R.setZero();
+#endif //
 	}
 }
 
@@ -218,131 +240,12 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 					  static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
 		return -1;
 	}
-	// 1. update relative kinematics
+	// 1. relative kinematics
-	// 1.1 for revolute
+        if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
-	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+            error_message("error in calculateKinematics\n");
-		RigidBody &body = m_body_list[m_body_revolute_list[i]];
+            return -1;
-		mat33 T;
+        }
-		bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
+        // 2. update contributions to equations of motion for every body.
-		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)
@@ -356,7 +259,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 			body.m_body_force_user;
 	}
 
-	// 4. calculate full set of forces at parent joint
+	// 3. calculate full set of forces at parent joint
 	// (not directly calculating the joint force along the free direction
 	// simplifies inclusion of fixed joints.
 	// An alternative would be to fuse bodies in a pre-processing step,
@@ -416,6 +319,239 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 	return 0;
 }
 
+int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
+                                                      const KinUpdateType type) {
+    	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
+		error_message("wrong vector dimension. system has %d DOFs,\n"
+					  "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
+					  m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
+					  static_cast<int>(dot_u.size()));
+		return -1;
+	}
+        if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
+            error_message("invalid type %d\n", type);
+            return -1;
+        }
+
+	// 1. update relative kinematics
+	// 1.1 for revolute
+	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+		RigidBody &body = m_body_list[m_body_revolute_list[i]];
+		mat33 T;
+		bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
+		body.m_body_T_parent = T * body.m_body_T_parent_ref;
+                if(type >= POSITION_VELOCITY) {
+                    body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
+                }
+                if(type >= POSITION_VELOCITY_ACCELERATION) {
+                    body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
+                }
+	}
+	// 1.2 for prismatic
+	for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
+		RigidBody &body = m_body_list[m_body_prismatic_list[i]];
+		body.m_parent_pos_parent_body =
+			body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
+                if(type >= POSITION_VELOCITY) {
+                    body.m_parent_vel_rel =
+			body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
+                }
+                if(type >= POSITION_VELOCITY_ACCELERATION) {
+                    body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
+                }
+	}
+	// 1.3 fixed joints: nothing to do
+	// 1.4 6dof joints:
+	for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
+		RigidBody &body = m_body_list[m_body_floating_list[i]];
+
+		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
+							   transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
+		body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
+		body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
+		body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
+		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+
+                if(type >= POSITION_VELOCITY) {
+                    body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
+                    body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
+                    body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
+
+                    body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
+                    body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
+                    body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
+
+                    body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+                }
+                if(type >= POSITION_VELOCITY_ACCELERATION) {
+                    body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
+                    body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
+                    body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
+
+                    body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
+                    body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
+                    body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
+
+                    body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+                }
+	}
+
+	// 2. absolute kinematic quantities (vector valued)
+	// NOTE: this should be optimized by specializing for different body types
+	// (e.g., relative rotation is always zero for prismatic joints, etc.)
+
+	// calculations for root body
+	{
+		RigidBody &body = m_body_list[0];
+		// 3.1 update absolute positions and orientations:
+		// will be required if we add force elements (eg springs between bodies,
+		// or contacts)
+		// not required right now, added here for debugging purposes
+		body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
+		body.m_body_T_world = body.m_body_T_parent;
+
+                if(type >= POSITION_VELOCITY) {
+                    // 3.2 update absolute velocities
+                    body.m_body_ang_vel = body.m_body_ang_vel_rel;
+                    body.m_body_vel = body.m_parent_vel_rel;
+                }
+                if(type >= POSITION_VELOCITY_ACCELERATION) {
+                    // 3.3 update absolute accelerations
+                    // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
+                    body.m_body_ang_acc = body.m_body_ang_acc_rel;
+                    body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
+                    // add gravitational acceleration to root body
+                    // this is an efficient way to add gravitational terms,
+                    // but it does mean that the kinematics are no longer
+                    // correct at the acceleration level
+                    // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
+                    body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
+                }
+	}
+
+	for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
+		RigidBody &body = m_body_list[i];
+		RigidBody &parent = m_body_list[m_parent_index[i]];
+		// 2.1 update absolute positions and orientations:
+		// will be required if we add force elements (eg springs between bodies,
+		// or contacts)  not required right now added here for debugging purposes
+		body.m_body_pos =
+			body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
+		body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
+
+                if(type >= POSITION_VELOCITY) {
+                    // 2.2 update absolute velocities
+                    body.m_body_ang_vel =
+			body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
+
+                    body.m_body_vel =
+			body.m_body_T_parent *
+			(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
+			 body.m_parent_vel_rel);
+                }
+                if(type >= POSITION_VELOCITY_ACCELERATION) {
+                    // 2.3 update absolute accelerations
+                    // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
+                    body.m_body_ang_acc =
+			body.m_body_T_parent * parent.m_body_ang_acc -
+			body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
+			body.m_body_ang_acc_rel;
+                    body.m_body_acc =
+			body.m_body_T_parent *
+			(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
+			 parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
+			 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
+                }
+	}
+
+    return 0;
+}
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+
+void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) {
+    const int& idx=body.m_q_index;
+    switch(body.m_joint_type) {
+    case FIXED:
+        break;
+    case REVOLUTE:
+        setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
+        setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
+        setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
+        break;
+    case PRISMATIC:
+        setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0)
+                     +body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1)
+                     +body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2),
+                     &body.m_body_Jac_T);
+        setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0)
+                     +body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1)
+                     +body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2),
+                     &body.m_body_Jac_T);
+        setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0)
+                     +body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1)
+                     +body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2),
+                     &body.m_body_Jac_T);
+        break;
+    case FLOATING:
+        setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R);
+        setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R);
+        setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R);
+        // body_Jac_T = body_T_parent.transpose();
+        setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T);
+        setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T);
+        setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T);
+
+        setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T);
+        setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T);
+        setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T);
+
+        setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T);
+        setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T);
+        setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T);
+
+        break;
+    }
+}
+
+int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
+    if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
+        error_message("wrong vector dimension. system has %d DOFs,\n"
+                      "but dim(q)= %d, dim(u)= %d\n",
+                      m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
+        return -1;
+    }
+    if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
+        error_message("invalid type %d\n", type);
+        return -1;
+    }
+
+    addRelativeJacobianComponent(m_body_list[0]);
+    for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
+        RigidBody &body = m_body_list[i];
+        RigidBody &parent = m_body_list[m_parent_index[i]];
+
+        mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R);
+        body.m_body_Jac_T = parent.m_body_Jac_T;
+        mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x);
+        sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T);
+
+        addRelativeJacobianComponent(body);
+        mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T);
+
+        if(type >= POSITION_VELOCITY) {
+            body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
+                body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
+            body.m_body_dot_Jac_T_u = body.m_body_T_parent *
+                (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
+                 parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
+                 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
+        }
+    }
+    return 0;
+}
+#endif
+
 static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) {
 	switch (dof) {
 		// rotational part
@@ -487,13 +623,6 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 // This implementation, however, handles branched systems and uses a formulation centered
 // on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
 
-// Macro for setting matrix elements depending on underlying math library.
-#ifdef ID_LINEAR_MATH_USE_BULLET
-#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val)
-#else
-#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val
-#endif
-
 	if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
 		mass_matrix->cols() != m_num_dofs) {
 		error_message("Dimension error. System has %d DOFs,\n"
@@ -507,7 +636,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 	if (initialize_matrix) {
 		for (int i = 0; i < m_num_dofs; i++) {
 			for (int j = 0; j < m_num_dofs; j++) {
-				setMassMatrixElem(i, j, 0.0);
+                            setMatxxElem(i, j, 0.0, mass_matrix);
 			}
 		}
 	}
@@ -597,7 +726,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 				body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
 			vec3 body_eom_trans =
 				body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
-			setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
+			setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
 
 			// rest of the mass matrix column upwards
 			{
@@ -609,7 +738,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 					}
 					setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
 					const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-					setMassMatrixElem(col, row, Mrc);
+					setMatxxElem(col, row, Mrc, mass_matrix);
 				}
 				// 2. ancestor dofs
 				int child_idx = i;
@@ -634,7 +763,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 							setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
 						}
 						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMassMatrixElem(col, row, Mrc);
+						setMatxxElem(col, row, Mrc, mass_matrix);
 					}
 
 					child_idx = parent_idx;
@@ -647,12 +776,10 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 	if (set_lower_triangular_matrix) {
 		for (int col = 0; col < m_num_dofs; col++) {
 			for (int row = 0; row < col; row++) {
-				setMassMatrixElem(row, col, (*mass_matrix)(col, row));
+                            setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
 			}
 		}
 	}
-
-#undef setMassMatrixElem
 	return 0;
 }
 
@@ -779,6 +906,32 @@ int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
 	return 0;
 }
 
+int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r)  const{
+    CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    *r=m_body_list[body_index].m_parent_pos_parent_body_ref;
+    return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T)  const{
+    CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    *T=m_body_list[body_index].m_body_T_parent_ref;
+    return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis)  const{
+    CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    if(m_body_list[body_index].m_joint_type == REVOLUTE) {
+        *axis = m_body_list[body_index].m_Jac_JR;
+        return 0;
+    }
+    if(m_body_list[body_index].m_joint_type == PRISMATIC) {
+        *axis = m_body_list[body_index].m_Jac_JT;
+        return 0;
+    }
+    setZero(*axis);
+    return 0;
+}
+
 int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const {
 	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
 	*q_index = m_body_list[body_index].m_q_index;
@@ -841,4 +994,34 @@ int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3
 	return 0;
 }
 
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+int  MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
+    CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    const RigidBody &body = m_body_list[body_index];
+    *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
+    return 0;
+}
+
+int  MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{
+     CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    const RigidBody &body = m_body_list[body_index];
+    *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
+    return 0;
+}
+
+int  MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{
+    CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    const RigidBody &body = m_body_list[body_index];
+    mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans);
+    return 0;
+}
+
+int  MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{
+    CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+    const RigidBody &body = m_body_list[body_index];
+    mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot);
+    return 0;
+}
+
+#endif
 }

src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp

@@ -110,6 +110,19 @@ struct RigidBody {
 	vec3 m_body_subtree_mass_com;
 	/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
 	mat33 m_body_subtree_I_body;
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+    /// translational jacobian in body-fixed frame d(m_body_vel)/du
+    mat3x m_body_Jac_T;
+    /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
+    mat3x m_body_Jac_R;
+    /// components of linear acceleration depending on u
+    /// (same as is d(m_Jac_T)/dt*u)
+    vec3 m_body_dot_Jac_T_u;
+    /// components of angular acceleration depending on u
+    /// (same as is d(m_Jac_T)/dt*u)
+    vec3 m_body_dot_Jac_R_u;
+#endif
 };
 
 /// The MBS implements a tree structured multibody system
@@ -119,6 +132,12 @@ class MultiBodyTree::MultiBodyImpl {
 public:
 	ID_DECLARE_ALIGNED_ALLOCATOR();
 
+        enum KinUpdateType {
+            POSITION_ONLY,
+            POSITION_VELOCITY,
+            POSITION_VELOCITY_ACCELERATION
+        };
+
 	/// constructor
 	/// @param num_bodies the number of bodies in the system
 	/// @param num_dofs number of degrees of freedom in the system
@@ -131,6 +150,25 @@ public:
 	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
 							const bool initialize_matrix, const bool set_lower_triangular_matrix,
 							matxx* mass_matrix);
+    	/// calculate kinematics (vector quantities)
+        /// Depending on type, update positions only, positions & velocities, or positions, velocities
+        /// and accelerations.
+        int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+    	/// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
+    int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
+    /// \copydoc MultiBodyTree::getBodyDotJacobianTransU
+    int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ;
+    /// \copydoc MultiBodyTree::getBodyDotJacobianRotU
+    int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
+    /// \copydoc MultiBodyTree::getBodyJacobianTrans
+    int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ;
+    /// \copydoc MultiBodyTree::getBodyJacobianRot
+    int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
+    /// Add relative Jacobian component from motion relative to parent body
+    /// @param body the body to add the Jacobian component for
+    void addRelativeJacobianComponent(RigidBody&body);
+#endif
 	/// generate additional index sets from the parent_index array
 	/// @return -1 on error, 0 on success
 	int generateIndexSets();
@@ -152,6 +190,12 @@ public:
 	int getJointType(const int body_index, JointType* joint_type) const;
 	/// \copydoc MultiBodyTree::getJointTypeStr
 	int getJointTypeStr(const int body_index, const char** joint_type) const;
+        /// \copydoc MultiBodyTree::getParentRParentBodyRef
+        int getParentRParentBodyRef(const int body_index, vec3* r) const;
+        /// \copydoc MultiBodyTree::getBodyTParentRef
+        int getBodyTParentRef(const int body_index, mat33* T) const;
+        /// \copydoc MultiBodyTree::getBodyAxisOfMotion
+        int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
 	/// \copydoc MultiBodyTree:getDoFOffset
 	int getDoFOffset(const int body_index, int* q_index) const;
 	/// \copydoc MultiBodyTree::getBodyOrigin
@@ -231,6 +275,9 @@ private:
 	idArray<int>::type m_user_int;
 	// a user-provided pointer
 	idArray<void*>::type m_user_ptr;
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+        mat3x m_m3x;
+#endif
 };
 }
 #endif

test/InverseDynamics/CMakeLists.txt

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

test/InverseDynamics/premake4.lua

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

test/InverseDynamics/test_invdyn_bullet.cpp

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

test/InverseDynamics/test_invdyn_jacobian.cpp

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