[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!

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
-	/// indexing!)
+	/// @param body_index is the index of a body
 	/// @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