Merge pull request #755 from erwincoumans/master

[InverseDynamics] Support for Jacobians & derivatives

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();
+}