Commits

Anonymous committed ac2217b Draft

Basic source build and install working. Moving on to tests

Comments (0)

Files changed (57)

 LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake )
 
 INCLUDE_DIRECTORIES ( 
-	${CMAKE_CURRENT_BINARY_DIR}/src
-	src
+	${CMAKE_CURRENT_BINARY_DIR}/include
+	include
 )
 
 SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
 ENDIF (RBDL_STORE_VERSION)
 
 CONFIGURE_FILE (
-	"${CMAKE_CURRENT_SOURCE_DIR}/src/rbdl_config.h.cmake" 
-	"${CMAKE_CURRENT_BINARY_DIR}/src/rbdl_config.h"
+	"${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/rbdl_config.h.cmake" 
+	"${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h"
 	)
 
 FILE ( GLOB headers 
-	${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
-	${CMAKE_CURRENT_BINARY_DIR}/src/rbdl_config.h
+	${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/*.h
+	${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h
 	)
 
 INSTALL ( FILES ${headers} DESTINATION include/rbdl )
 
 # Setup of SimpleMath install settings
 IF (RBDL_USE_SIMPLE_MATH)
-	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src/SimpleMath"
+	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/SimpleMath"
 		DESTINATION include/rbdl
 	)
 ENDIF (RBDL_USE_SIMPLE_MATH)

include/rbdl/Body.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _BODY_H
+#define _BODY_H
+
+#include <rbdl/rbdl_math.h>
+#include <rbdl/rbdl_mathutils.h>
+#include <assert.h>
+#include <iostream>
+#include "rbdl/Logging.h"
+
+namespace RigidBodyDynamics {
+
+/** \brief Describes all properties of a single body 
+ *
+ * A Body contains information about mass, the location of its center of
+ * mass, and the ineria tensor in the center of mass. This class is
+ * designed to use the given information and transform it such that it can
+ * directly be used by the spatial algebra.
+ * 
+ * The spatial inertia of the member variable Body::mSpatialInertia is
+ * expressed at the origin of the coordinate frame.
+ *
+ */
+struct Body {
+	Body() :
+		mMass (1.),
+		mCenterOfMass (0., 0., 0.),
+		mInertia (Math::Matrix3d::Zero(3,3)),
+		mSpatialInertia (Math::SpatialMatrix::Zero(6,6))
+	{ };
+	Body(const Body &body) :
+		mMass (body.mMass),
+		mCenterOfMass (body.mCenterOfMass),
+		mInertia (body.mInertia),
+		mSpatialInertia (body.mSpatialInertia)
+	{};
+	Body& operator= (const Body &body) {
+		if (this != &body) {
+			mMass = body.mMass;
+			mInertia = body.mInertia;
+			mCenterOfMass = body.mCenterOfMass;
+			mSpatialInertia = body.mSpatialInertia;
+		}
+
+		return *this;
+	}
+
+	/** \brief Constructs a body from mass, center of mass and radii of gyration 
+	 * 
+	 * This constructor eases the construction of a new body as all the
+	 * required parameters can simply be specified as parameters to the
+	 * constructor. These are then used to generate the spatial inertia
+	 * matrix which is expressed at the origin.
+	 * 
+	 * \param mass the mass of the body
+	 * \param com  the position of the center of mass in the bodies coordinates
+	 * \param gyration_radii the radii of gyration at the center of mass of the body
+	 */
+	Body(const double &mass,
+			const Math::Vector3d &com,
+			const Math::Vector3d &gyration_radii) :
+		mMass (mass),
+		mCenterOfMass(com) {
+			Math::Matrix3d com_cross (
+					0., -com[2],  com[1],
+					com[2],      0., -com[0],
+					-com[1],  com[0],      0.
+					);
+			Math::Matrix3d parallel_axis;
+			parallel_axis = mass * com_cross * com_cross.transpose();
+
+			Math::Vector3d gr (gyration_radii);
+			Math::Matrix3d pa (parallel_axis);
+			Math::Matrix3d mcc = mass * com_cross;
+			Math::Matrix3d mccT = mcc.transpose();
+
+			mSpatialInertia.set (
+					gr[0] + pa(0, 0), pa(0, 1), pa(0, 2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
+					pa(1, 0), gr[1] + pa(1, 1), pa(1, 2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
+					pa(2, 0), pa(2, 1), gr[2] + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
+					mccT(0, 0), mccT(0, 1), mccT(0, 2), mass, 0., 0.,
+					mccT(1, 0), mccT(1, 1), mccT(1, 2), 0., mass, 0.,
+					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass
+					);
+
+			mInertia = mSpatialInertia.block<3,3>(0,0);
+		}
+
+	/** \brief Constructs a body from mass, center of mass, and a 3x3 inertia matrix 
+	 * 
+	 * This constructor eases the construction of a new body as all the
+	 * required parameters can simply be specified as parameters to the
+	 * constructor. These are then used to generate the spatial inertia
+	 * matrix which is expressed at the origin.
+	 *
+	 * \param mass the mass of the body
+	 * \param com  the position of the center of mass in the bodies coordinates
+	 * \param inertia_C the inertia at the center of mass
+	 */
+	Body(const double &mass,
+			const Math::Vector3d &com,
+			const Math::Matrix3d &inertia_C) :
+		mMass (mass),
+		mCenterOfMass(com),
+		mInertia (inertia_C) {
+			Math::Matrix3d com_cross (
+					0., -com[2],  com[1],
+					com[2],      0., -com[0],
+					-com[1],  com[0],      0.
+					);
+			Math::Matrix3d parallel_axis = mass * com_cross * com_cross.transpose();
+
+			LOG << "parrallel axis = " << std::endl << parallel_axis << std::endl;
+
+			Math::Matrix3d pa (parallel_axis);
+			Math::Matrix3d mcc = mass * com_cross;
+			Math::Matrix3d mccT = mcc.transpose();
+
+			mSpatialInertia.set (
+					inertia_C(0,0) + pa(0, 0), inertia_C(0,1) + pa(0, 1), inertia_C(0,2) + pa(0, 2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
+					inertia_C(1,0) + pa(1, 0), inertia_C(1,1) + pa(1, 1), inertia_C(1,2) + pa(1, 2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
+					inertia_C(2,0) + pa(2, 0), inertia_C(2,1) + pa(2, 1), inertia_C(2,2) + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
+					mccT(0, 0), mccT(0, 1), mccT(0, 2), mass, 0., 0.,
+					mccT(1, 0), mccT(1, 1), mccT(1, 2), 0., mass, 0.,
+					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass
+					);
+		}
+
+	/** \brief Constructs a body out of the given parameters
+	 * 
+	 * This constructor eases the construction of a new body as all the
+	 * required parameters can simply be specified as parameters to the
+	 * constructor. These are then used to generate the spatial inertia
+	 * matrix which is expressed at the origin.
+	 * 
+	 * \param mass the mass of the body
+	 * \param com  the position of the center of mass in the bodies coordinates
+	 * \param length the length of the segment (needed to compute the inertia at the CoM
+	 * \param gyration_radii the radii of gyration at the center of mass of the body in percentage of the segment length
+	 */
+	Body(const double &mass,
+			const Math::Vector3d &com,
+			const double &length,
+			const Math::Vector3d &gyration_radii) :
+		mMass (mass),
+		mCenterOfMass(com) {
+			Math::Matrix3d com_cross (
+					0., -com[2],  com[1],
+					com[2],      0., -com[0],
+					-com[1],  com[0],      0.
+					);
+			Math::Matrix3d parallel_axis;
+			parallel_axis = mass * com_cross * com_cross.transpose();
+
+			LOG << "parrallel axis = " << parallel_axis << std::endl;
+
+			Math::Vector3d gr = mass * Math::Vector3d(
+					gyration_radii[0] * gyration_radii[0] * length * length,
+					gyration_radii[1] * gyration_radii[1] * length * length,
+					gyration_radii[2] * gyration_radii[2] * length * length
+					);
+			Math::Matrix3d pa (parallel_axis);
+			Math::Matrix3d mcc = mass * com_cross;
+			Math::Matrix3d mccT = mcc.transpose();
+
+			mInertia.set (
+					gr[0], 0., 0.,
+					0., gr[1], 0.,
+					0., 0., gr[2]
+					);
+
+			mSpatialInertia.set (
+					gr[0] + pa(0, 0), pa(0, 1), pa(0, 2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
+					pa(1, 0), gr[1] + pa(1, 1), pa(1, 2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
+					pa(2, 0), pa(2, 1), gr[2] + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
+					mccT(0, 0), mccT(0, 1), mccT(0, 2), mass, 0., 0.,
+					mccT(1, 0), mccT(1, 1), mccT(1, 2), 0., mass, 0.,
+					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass
+					);
+
+			LOG << "spatial inertia = " << mSpatialInertia << std::endl;
+		}
+		
+	/** \brief Joins inertial parameters of two bodies to create a composite
+	 * body.
+	 *
+	 * This function can be used to joint inertial parameters of two bodies
+	 * to create a composite body that has the inertial properties as if the
+	 * two bodies were joined by a fixed joint.
+	 *
+	 * \note Both bodies have to have their inertial parameters expressed in
+	 * the same orientation.
+	 *
+	 * \param transform The frame transformation from the origin of the
+	 * original body to the origin of the added body
+	 */
+	void Join (const Math::SpatialTransform &transform, const Body &other_body) {
+		// nothing to do if we join a massles body to the current.
+		if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) {
+			return;
+		}
+
+		double other_mass = other_body.mMass;
+		double new_mass = mMass + other_mass;
+
+		if (new_mass == 0.) {
+			std::cerr << "Error: cannot join bodies as both have zero mass!" << std::endl;
+			assert (false);
+			abort();
+		}
+
+		Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r;
+		Math::Vector3d new_com = (1 / new_mass ) * (mMass * mCenterOfMass + other_mass * other_com);
+
+		LOG << "other_com = " << std::endl << other_com.transpose() << std::endl;
+		LOG << "rotation = " << std::endl << transform.E << std::endl;
+
+		// We have to transform the inertia of other_body to the new COM. This
+		// is done in 4 steps:
+		//
+		// 1. Transform the inertia from other origin to other COM
+		// 2. Rotate the inertia that it is aligned to the frame of this body
+		// 3. Transform inertia of other_body to the origin of the frame of
+		// this body
+		// 4. Sum the two inertias
+		// 5. Transform the summed inertia to the new COM
+
+		Math::Matrix3d inertia_other = other_body.mSpatialInertia.block<3,3>(0,0);
+		LOG << "inertia_other = " << std::endl << inertia_other << std::endl;
+
+		// 1. Transform the inertia from other origin to other COM
+		Math::Matrix3d other_com_cross = Math::VectorCrossMatrix(other_body.mCenterOfMass);
+		Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross * other_com_cross.transpose();
+		LOG << "inertia_other_com = " << std::endl << inertia_other_com << std::endl;
+
+		// 2. Rotate the inertia that it is aligned to the frame of this body
+		Math::Matrix3d inertia_other_com_rotated = transform.E.transpose() * inertia_other_com * transform.E;
+		LOG << "inertia_other_com_rotated = " << std::endl << inertia_other_com_rotated << std::endl;
+
+		// 3. Transform inertia of other_body to the origin of the frame of this body
+		Math::Matrix3d inertia_other_com_rotated_this_origin = Math::parallel_axis (inertia_other_com_rotated, other_mass, other_com);
+		LOG << "inertia_other_com_rotated_this_origin = " << std::endl << inertia_other_com_rotated_this_origin << std::endl;
+
+		// 4. Sum the two inertias
+		Math::Matrix3d inertia_summed = Math::Matrix3d (mSpatialInertia.block<3,3>(0,0)) + inertia_other_com_rotated_this_origin;
+		LOG << "inertia_summed  = " << std::endl << inertia_summed << std::endl;
+
+		// 5. Transform the summed inertia to the new COM
+		Math::Matrix3d new_inertia = inertia_summed - new_mass * Math::VectorCrossMatrix (new_com) * Math::VectorCrossMatrix(new_com).transpose();
+
+		LOG << "new_mass = " << new_mass << std::endl;
+		LOG << "new_com  = " << new_com.transpose() << std::endl;
+		LOG << "new_inertia  = " << std::endl << new_inertia << std::endl;
+
+		*this = Body (new_mass, new_com, new_inertia);
+	}
+
+	~Body() {};
+
+
+	/// \brief The mass of the body
+	double mMass;
+	/// \brief The position of the center of mass in body coordinates
+	Math::Vector3d mCenterOfMass;
+	/// \brief Inertia matrix at the center of mass
+	Math::Matrix3d mInertia;
+	/// \brief The spatial inertia that contains both mass and inertia information
+	Math::SpatialMatrix mSpatialInertia;
+};
+
+/** \brief Keeps the information of a body and how it is attached to another body.
+ *
+ * When using fixed bodies, i.e. a body that is attached to anothe via a
+ * fixed joint, the attached body is merged onto its parent. By doing so
+ * adding fixed joints do not have an impact on runtime.
+ */
+struct FixedBody {
+	/// \brief The mass of the body
+	double mMass;
+	/// \brief The position of the center of mass in body coordinates
+	Math::Vector3d mCenterOfMass;
+	/// \brief The spatial inertia that contains both mass and inertia information
+	Math::SpatialMatrix mSpatialInertia;
+
+	/// \brief Id of the movable body that this fixed body is attached to.
+	unsigned int mMovableParent;
+	/// \brief Transforms spatial quantities expressed for the parent to the
+	// fixed body. 
+	Math::SpatialTransform mParentTransform;
+	Math::SpatialTransform mBaseTransform;
+
+	static FixedBody CreateFromBody (const Body& body) {
+		FixedBody fbody;
+
+		fbody.mMass = body.mMass;
+		fbody.mCenterOfMass = body.mCenterOfMass;
+		fbody.mSpatialInertia = body.mSpatialInertia;
+
+		return fbody;
+	}
+};
+
+}
+
+#endif /* _BODY_H */

include/rbdl/Contacts.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _CONTACTS_H
+#define _CONTACTS_H
+
+#include <rbdl/rbdl_math.h>
+#include <rbdl/rbdl_mathutils.h>
+
+namespace RigidBodyDynamics {
+
+/** \defgroup contacts_group External Contacts
+ *
+ * External contacts are handled by specification of a \link
+ * RigidBodyDynamics::ForwardDynamicsContacts::ConstraintSet
+ * ConstraintSet \endlink which contains all informations about the
+ * current contacts and workspace memory.
+ *
+ * Separate contacts can be specified by calling
+ * ConstraintSet::AddConstraint(). After all constraints have been
+ * specified, this \link
+ * RigidBodyDynamics::ForwardDynamicsContacts::ConstraintSet
+ * ConstraintSet \endlink has to be bound to the model via
+ * ConstraintSet::Bind(). This initializes workspace memory that is
+ * later used when calling one of the contact functions, such as
+ * ForwardDynamicsContacts() or ForwardDynamicsContactsLagrangian().
+ *
+ * The values in the vectors ConstraintSet::constraint_force and
+ * ConstraintSet::constraint_impulse contain the computed force or
+ * impulse values for each constraint when returning from one of the
+ * contact functions.
+ *
+ * @{
+ */
+
+struct Model;
+
+/** \brief Structure that contains both constraint information and workspace memory.
+ *
+ * This structure is used to reduce the amount of memory allocations that
+ * are needed when computing constraint forces.
+ *
+ * The ConstraintSet has to be bound to a model using ConstraintSet::Bind()
+ * before it can be used in \link RigidBodyDynamics::ForwardDynamicsContacts
+ * ForwardDynamicsContacts \endlink.
+ */
+struct ConstraintSet {
+	ConstraintSet() :
+		linear_solver (Math::LinearSolverColPivHouseholderQR),
+		bound (false)
+	{}
+
+	/** \brief Adds a constraint to the constraint set.
+	 *
+	 * \param body_id the body which is affected directly by the constraint
+	 * \param body_point the point that is constrained relative to the
+	 * contact body
+	 * \param world_normal the normal along the constraint acts (in base
+	 * coordinates)
+	 * \param constraint_name a human readable name (optional, default: NULL)
+	 * \param acceleration the acceleration of the contact along the normal
+	 * (optional, default: 0.)
+	 */
+	unsigned int AddConstraint (
+			unsigned int body_id,
+			const Math::Vector3d &body_point,
+			const Math::Vector3d &world_normal,
+			const char *constraint_name = NULL,
+			double acceleration = 0.);
+
+	/** \brief Copies the constraints and resets its ConstraintSet::bound
+	 * flag.
+	 */
+	ConstraintSet Copy() {
+		ConstraintSet result (*this);
+		result.bound = false;
+
+		return result;
+	}
+
+	/** \brief Specifies which method should be used for solving undelying linear systems.
+	 */
+	void SetSolver (Math::LinearSolver solver) {
+		linear_solver = solver;
+	}
+
+	/** \brief Initializes and allocates memory for the constraint set.
+	 *
+	 * This function allocates memory for temporary values and matrices that
+	 * are required for contact force computation. Both model and constraint
+	 * set must not be changed after a binding as the required memory is
+	 * dependent on the model size (i.e. the number of bodies and degrees of
+	 * freedom) and the number of constraints in the Constraint set.
+	 *
+	 * The values of ConstraintSet::constraint_acceleration may still be
+	 * modified after the set is bound to the model.
+	 */
+	bool Bind (const Model &model);
+
+	/** \brief Returns the number of constraints. */
+	unsigned int size() {
+		return constraint_acceleration.size();
+	}
+
+	/** \brief Clears all variables in the constraint set. */
+	void clear ();
+
+	/// Method that should be used to solve internal linear systems.
+	Math::LinearSolver linear_solver;
+	/// Whether the constraint set was bound to a model (mandatory!).
+	bool bound;
+
+	std::vector<std::string> name;
+	std::vector<unsigned int> body;
+	std::vector<Math::Vector3d> point;
+	std::vector<Math::Vector3d> normal;
+
+	/** Enforced accelerations of the contact points along the contact
+	 * normal. */
+	Math::VectorNd constraint_acceleration;
+	/** Actual constraint forces along the contact normals. */
+	Math::VectorNd constraint_force;
+	/** Actual constraint impulses along the contact normals. */
+	Math::VectorNd constraint_impulse;
+
+	// Variables used by the Lagrangian methods
+
+	/// Workspace for the joint space inertia matrix.
+	Math::MatrixNd H;
+	/// Workspace for the coriolis forces.
+	Math::VectorNd C;
+	/// Workspace of the lower part of b.
+	Math::VectorNd gamma;
+	Math::MatrixNd G;
+	/// Workspace for the Lagrangian left-hand-side matrix.
+	Math::MatrixNd A;
+	/// Workspace for the Lagrangian right-hand-side.
+	Math::VectorNd b;
+	/// Workspace for the Lagrangian solution.
+	Math::VectorNd x;
+
+	// Variables used by the IABI methods
+
+	/// Workspace for the Inverse Articulated-Body Inertia.
+	Math::MatrixNd K;
+	/// Workspace for the accelerations of due to the test forces
+	Math::VectorNd a;
+	/// Workspace for the test accelerations.
+	Math::VectorNd QDDot_t;
+	/// Workspace for the default accelerations.
+	Math::VectorNd QDDot_0;
+	/// Workspace for the test forces.
+	std::vector<Math::SpatialVector> f_t;
+	/// Workspace for the actual spatial forces.
+	std::vector<Math::SpatialVector> f_ext_constraints;
+	/// Workspace for the default point accelerations.
+	std::vector<Math::Vector3d> point_accel_0;
+
+	/// Workspace for the bias force due to the test force
+	std::vector<Math::SpatialVector> d_pA;
+	/// Workspace for the acceleration due to the test force
+	std::vector<Math::SpatialVector> d_a;
+	Math::VectorNd d_u;
+
+	/// Workspace for the inertia when applying constraint forces
+	std::vector<Math::SpatialMatrix> d_IA;
+	/// Workspace when applying constraint forces
+	std::vector<Math::SpatialVector> d_U;
+	/// Workspace when applying constraint forces
+	Math::VectorNd d_d;
+};
+
+/** \brief Computes forward dynamics with contact by constructing and solving the full lagrangian equation
+ *
+ * This method builds and solves the linear system \f[
+ \left(
+   \begin{array}{cc}
+	   H & G^T \\
+		 G & 0
+   \end{array}
+ \right)
+ \left(
+   \begin{array}{c}
+	   \ddot{q} \\
+		 \lambda
+   \end{array}
+ \right)
+ =
+ \left(
+   \begin{array}{c}
+	   -C + \tau \\
+		 -\gamma
+   \end{array}
+ \right)
+ * \f] where \f$H\f$ is the joint space inertia matrix computed with the
+ * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the
+ * contact points, \f$C\f$ the bias force (sometimes called "non-linear
+ * effects"), and \f$\gamma\f$ the generalized acceleration independent
+ * part of the contact point accelerations.
+ *
+ * \note So far, only constraints acting along cartesian coordinate axes
+ * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must
+ * not specify redundant constraints!
+ * 
+ * \par 
+ *
+ * \note To increase performance group constraints body and pointwise such
+ * that constraints acting on the same body point are sequentially in
+ * ConstraintSet. This can save computation of point jacobians \f$G\f$.
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the internal joints
+ * \param QDot  velocity vector of the internal joints
+ * \param Tau   actuations of the internal joints
+ * \param ConstraintSet list of all contact points
+ * \param QDDot accelerations of the internals joints (output)
+ *
+ * \note During execution of this function values such as
+ * ConstraintSet::constraint_force get modified and will contain the value
+ * of the force acting along the normal.
+ *
+ */
+void ForwardDynamicsContactsLagrangian (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &Tau,
+		ConstraintSet &CS,
+		Math::VectorNd &QDDot
+		);
+
+/** \brief Computes forward dynamics with contact by constructing and solving the full lagrangian equation
+ *
+ * This method builds and solves the linear system \f[
+ \left(
+   \begin{array}{cc}
+	   H & G^T \\
+		 G & 0
+   \end{array}
+ \right)
+ \left(
+   \begin{array}{c}
+	   \dot{q}^{+} \\
+		 \Lambda
+   \end{array}
+ \right)
+ =
+ \left(
+   \begin{array}{c}
+	   H \dot{q}^{-} \\
+		v^{+} 
+   \end{array}
+ \right)
+ * \f] where \f$H\f$ is the joint space inertia matrix computed with the
+ * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the
+ * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the
+ * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$
+ * the generalized velocity before the impact, and \f$v^{+}\f$ the desired
+ * velocity of each constraint after the impact (known beforehand, usually
+ * 0).
+ *
+ * \note So far, only constraints acting along cartesian coordinate axes
+ * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must
+ * not specify redundant constraints!
+ * 
+ * \par 
+ *
+ * \note To increase performance group constraints body and pointwise such
+ * that constraints acting on the same body point are sequentially in
+ * ConstraintSet. This can save computation of point jacobians \f$G\f$.
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the internal joints
+ * \param QDotMinus  velocity vector of the internal joints before the impact
+ * \param CS the set of active constraints
+ * \param QDotPlus velocities of the internals joints after the impact (output)
+ */
+void ComputeContactImpulsesLagrangian (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDotMinus,
+		ConstraintSet &CS,
+		Math::VectorNd &QDotPlus
+		);
+
+/** \brief Computes forward dynamics that accounts for active contacts in ConstraintSet.
+ *
+ * The method used here is the one described by Kokkevis and Metaxas in the
+ * Paper "Practical Physics for Articulated Characters", Game Developers
+ * Conference, 2004.
+ *
+ * It does this by recursively computing the inverse articulated-body inertia (IABI)
+ * \f$\Phi_{i,j}\f$ which is then used to build and solve a system of the form:
+ \f[
+ \left(
+   \begin{array}{c}
+	   \dot{v}_1 \\
+		 \dot{v}_2 \\
+		 \vdots \\
+		 \dot{v}_n
+   \end{array}
+ \right)
+ =
+ \left(
+   \begin{array}{cccc}
+	   \Phi_{1,1} & \Phi_{1,2} & \cdots & \Phi{1,n} \\
+	   \Phi_{2,1} & \Phi_{2,2} & \cdots & \Phi{2,n} \\
+	   \cdots & \cdots & \cdots & \vdots \\
+	   \Phi_{n,1} & \Phi_{n,2} & \cdots & \Phi{n,n} 
+   \end{array}
+ \right)
+ \left(
+   \begin{array}{c}
+	   f_1 \\
+		 f_2 \\
+		 \vdots \\
+		 f_n
+   \end{array}
+ \right)
+ + 
+ \left(
+   \begin{array}{c}
+	 \phi_1 \\
+	 \phi_2 \\
+	 \vdots \\
+	 \phi_n
+   \end{array}
+ \right).
+ \f]
+ Here \f$n\f$ is the number of constraints and the method for building the system
+ uses the Articulated Body Algorithm to efficiently compute entries of the system. The
+ values \f$\dot{v}_i\f$ are the constraint accelerations, \f$f_i\f$ the constraint forces,
+ and \f$\phi_i\f$ are the constraint bias forces.
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the internal joints
+ * \param QDot  velocity vector of the internal joints
+ * \param Tau   actuations of the internal joints
+ * \param CS a list of all contact points
+ * \param QDDot accelerations of the internals joints (output)
+ *
+ * \note During execution of this function values such as
+ * ConstraintSet::constraint_force get modified and will contain the value
+ * of the force acting along the normal.
+ *
+ * \todo Allow for external forces
+ */void ForwardDynamicsContacts (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &Tau,
+		ConstraintSet &CS,
+		Math::VectorNd &QDDot
+		);
+
+/** @} */
+
+} /* namespace RigidBodyDynamics */
+
+#endif /* _CONTACTS_H */

include/rbdl/Dynamics.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _DYNAMICS_H
+#define _DYNAMICS_H
+
+#include <assert.h>
+#include <iostream>
+
+#include <rbdl/rbdl_math.h>
+#include <rbdl/rbdl_mathutils.h>
+
+#include "rbdl/Logging.h"
+
+namespace RigidBodyDynamics {
+
+struct Model;
+
+/** \defgroup dynamics_group Dynamics
+ * @{
+ */
+
+/** \brief Computes forward dynamics with the Articulated Body Algorithm
+ *
+ * This function computes the generalized accelerations from given
+ * generalized states, velocities and forces:
+ *   \f$ \ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)\f$
+ * It does this by using the recursive Articulated Body Algorithm that runs
+ * in \f$O(n_{dof})\f$ with \f$n_{dof}\f$ being the number of joints.
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the internal joints
+ * \param QDot  velocity vector of the internal joints
+ * \param Tau   actuations of the internal joints
+ * \param QDDot accelerations of the internal joints (output)
+ * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL)
+ */
+void ForwardDynamics (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &Tau,
+		Math::VectorNd &QDDot,
+		std::vector<Math::SpatialVector> *f_ext = NULL
+		);
+
+/** \brief Computes forward dynamics by building and solving the full Lagrangian equation
+ *
+ * This method builds and solves the linear system
+ * \f[ 	H \ddot{q} = -C + \tau	\f]
+ * for \f$\ddot{q}\f$ where \f$H\f$ is the joint space inertia matrix
+ * computed with the CompositeRigidBodyAlgorithm(), \f$C\f$ the bias
+ * force (sometimes called "non-linear effects").
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the internal joints
+ * \param QDot  velocity vector of the internal joints
+ * \param Tau   actuations of the internal joints
+ * \param QDDot accelerations of the internal joints (output)
+ * \param linear_solver specification which method should be used for solving the linear system
+ * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL)
+ */
+void ForwardDynamicsLagrangian (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &Tau,
+		Math::VectorNd &QDDot,
+		Math::LinearSolver linear_solver = Math::LinearSolverColPivHouseholderQR,
+		std::vector<Math::SpatialVector> *f_ext = NULL
+		);
+
+/** \brief Computes inverse dynamics with the Newton-Euler Algorithm
+ *
+ * This function computes the generalized forces from given generalized
+ * states, velocities, and accelerations:
+ *   \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the internal joints
+ * \param QDot  velocity vector of the internal joints
+ * \param QDDot accelerations of the internals joints
+ * \param Tau   actuations of the internal joints (output)
+ * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL)
+ */
+void InverseDynamics (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &QDDot,
+		Math::VectorNd &Tau,
+		std::vector<Math::SpatialVector> *f_ext = NULL
+		);
+
+/** \brief Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm
+ *
+ * This function computes the joint space inertia matrix from a given model and
+ * the generalized state vector:
+ *   \f$ M(q) \f$
+ *
+ * \param model rigid body model
+ * \param Q     state vector of the model
+ * \param H     a matrix where the result will be stored in
+ * \param update_kinematics  whether the kinematics should be updated
+ * (safer, but at a higher computational cost!)
+ */
+void CompositeRigidBodyAlgorithm (
+		Model& model,
+		const Math::VectorNd &Q,
+		Math::MatrixNd &H,
+		bool update_kinematics = true
+		);
+
+/** @} */
+
+}
+
+#endif /* _DYNAMICS_H */

include/rbdl/Dynamics_experimental.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _DYNAMICS_EXPERIMENTAL_H
+#define _DYNAMICS_EXPERIMENTAL_H
+
+#include <rbdl/rbdl_math.h>
+#include <assert.h>
+#include <iostream>
+#include "rbdl/Logging.h"
+
+#include "rbdl/Model.h"
+
+namespace RigidBodyDynamics {
+
+/** \brief Namespace for experimental code that is not thoroughly tested
+ */
+namespace Experimental {
+
+}
+
+}
+
+#endif /* _DYNAMICS_EXPERIMENTAL_H */

include/rbdl/Joint.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _JOINT_H
+#define _JOINT_H
+
+#include <rbdl/rbdl_math.h>
+#include <assert.h>
+#include <iostream>
+#include "rbdl/Logging.h"
+
+namespace RigidBodyDynamics {
+
+struct Model;
+
+/** \brief General types of joints
+ */
+enum JointType {
+	JointTypeUndefined = 0,
+	JointTypeRevolute,
+	JointTypePrismatic,
+	JointTypeFixed,
+
+	JointType1DoF,
+	JointType2DoF,
+	JointType3DoF,
+	JointType4DoF,
+	JointType5DoF,
+	JointType6DoF
+};
+
+/** \brief Describes a joint relative to the predecessor body.
+ *
+ * This class contains all information required for one single joint. This
+ * contains the joint type and the axis of the joint.
+ */
+struct Joint {
+	Joint() :
+		mJointAxes (NULL),
+		mJointType (JointTypeUndefined),
+		mDoFCount (0) {};
+	Joint (JointType type) :
+		mJointAxes (NULL),
+		mJointType (type),
+	  mDoFCount (0) {
+			if (type != JointTypeFixed) {
+				std::cerr << "Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type == JointTypeFixed." << std::endl;
+				assert (0);
+				abort();
+			}
+		}
+	Joint (const Joint &joint) :
+		mJointType (joint.mJointType),
+		mDoFCount (joint.mDoFCount) {
+			mJointAxes = new Math::SpatialVector[mDoFCount];
+
+			for (unsigned int i = 0; i < mDoFCount; i++)
+				mJointAxes[i] = joint.mJointAxes[i];
+		};
+	Joint& operator= (const Joint &joint) {
+		if (this != &joint) {
+			if (mDoFCount > 0) {
+				assert (mJointAxes);
+				delete[] mJointAxes;
+			}
+			mJointType = joint.mJointType;
+			mDoFCount = joint.mDoFCount;
+
+			mJointAxes = new Math::SpatialVector[mDoFCount];
+
+			for (unsigned int i = 0; i < mDoFCount; i++)
+				mJointAxes[i] = joint.mJointAxes[i];
+		}
+		return *this;
+	}
+	~Joint() {
+		if (mJointAxes) {
+			assert (mJointAxes);
+			delete[] mJointAxes;
+			mJointAxes = NULL;
+			mDoFCount = 0;
+		}
+	}
+
+	/** \brief Constructs a joint from the given cartesian parameters.
+	 *
+	 * This constructor creates all the required spatial values for the given
+	 * cartesian parameters.
+	 *
+	 * \param joint_type whether the joint is revolute or prismatic
+	 * \param joint_axis the axis of rotation or translation
+	 */
+	Joint (
+			const JointType joint_type,
+			const Math::Vector3d &joint_axis
+			) {
+		mDoFCount = 1;
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		// Some assertions, as we concentrate on simple cases
+	
+		// Only rotation around the Z-axis
+		assert ( joint_type == JointTypeRevolute || joint_type == JointTypePrismatic );
+
+		mJointType = joint_type;
+
+		if (joint_type == JointTypeRevolute) {
+			// make sure we have a unit axis
+			mJointAxes[0].set (
+					joint_axis[0],
+					joint_axis[1], 
+					joint_axis[2], 
+					0., 0., 0.
+					);
+
+		} else if (joint_type == JointTypePrismatic) {
+			// make sure we have a unit axis
+			assert (joint_axis.squaredNorm() == 1.);
+
+			mJointAxes[0].set (
+					0., 0., 0.,
+					joint_axis[0],
+					joint_axis[1],
+					joint_axis[2]
+					);
+		}
+	}
+
+	/** \brief Constructs a 1 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0
+			) {
+		mJointType = JointType1DoF;
+		mDoFCount = 1;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+		mJointAxes[0] = axis_0;
+
+		validate_spatial_axis (mJointAxes[0]);
+	}
+
+	/** \brief Constructs a 2 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1
+			) {
+		mJointType = JointType2DoF;
+		mDoFCount = 2;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+	}
+
+	/** \brief Constructs a 3 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2
+			) {
+		mJointType = JointType3DoF;
+		mDoFCount = 3;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+	}
+
+	/** \brief Constructs a 4 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 * \param axis_3 Motion subspace for axis 3
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2,
+			const Math::SpatialVector &axis_3
+			) {
+		mJointType = JointType4DoF;
+		mDoFCount = 4;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+		mJointAxes[3] = axis_3;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+		validate_spatial_axis (mJointAxes[3]);
+	}
+
+	/** \brief Constructs a 5 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 * \param axis_3 Motion subspace for axis 3
+	 * \param axis_4 Motion subspace for axis 4
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2,
+			const Math::SpatialVector &axis_3,
+			const Math::SpatialVector &axis_4
+			) {
+		mJointType = JointType5DoF;
+		mDoFCount = 5;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+		mJointAxes[3] = axis_3;
+		mJointAxes[4] = axis_4;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+		validate_spatial_axis (mJointAxes[3]);
+		validate_spatial_axis (mJointAxes[4]);
+	}
+
+	/** \brief Constructs a 6 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 * \param axis_3 Motion subspace for axis 3
+	 * \param axis_4 Motion subspace for axis 4
+	 * \param axis_5 Motion subspace for axis 5
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2,
+			const Math::SpatialVector &axis_3,
+			const Math::SpatialVector &axis_4,
+			const Math::SpatialVector &axis_5
+			) {
+		mJointType = JointType6DoF;
+		mDoFCount = 6;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+		mJointAxes[3] = axis_3;
+		mJointAxes[4] = axis_4;
+		mJointAxes[5] = axis_5;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+		validate_spatial_axis (mJointAxes[3]);
+		validate_spatial_axis (mJointAxes[4]);
+		validate_spatial_axis (mJointAxes[5]);
+	}
+
+	/** \brief Checks whether we have pure rotational or translational axis.
+	 *
+	 * This function is mainly used to print out warnings when specifying an
+	 * axis that might not be intended.
+	 */
+	bool validate_spatial_axis (Math::SpatialVector &axis) {
+		if (fabs(axis.norm() - 1.0) > 1.0e-8) {
+			std::cerr << "Warning: joint axis is not unit!" << std::endl;
+		}
+
+		bool axis_rotational = false;
+		bool axis_translational = false;
+
+		Math::Vector3d rotation (axis[0], axis[1], axis[2]);
+		Math::Vector3d translation (axis[3], axis[4], axis[5]);
+
+		if (fabs(translation.norm()) < 1.0e-8)
+			axis_rotational = true;
+
+		if (fabs(rotation.norm()) < 1.0e-8)
+			axis_translational = true;
+
+		return axis_rotational || axis_translational;
+	}
+
+	/// \brief The spatial axis of the joint
+	Math::SpatialVector* mJointAxes;
+	/// \brief Type of joint (rotational or prismatic)
+	JointType mJointType;
+	unsigned int mDoFCount;
+};
+
+/** \brief Computes all variables for a joint model
+ *
+ *	By appropriate modification of this function all types of joints can be
+ *	modeled. See RBDA Section 4.4 for details.
+ *
+ * \param model    the rigid body model
+ * \param joint_id the id of the joint we are interested in (output)
+ * \param XJ       the joint transformation (output)
+ * \param S        motion subspace of the joint (output)
+ * \param v_J      joint velocity (output)
+ * \param c_J      joint acceleration for rhenomic joints (output)
+ * \param q        joint state variable
+ * \param qdot     joint velocity variable
+ */
+void jcalc (
+		const Model &model,
+		const unsigned int &joint_id,
+		Math::SpatialTransform &XJ,
+		Math::SpatialVector &S,
+		Math::SpatialVector &v_J,
+		Math::SpatialVector &c_J,
+		const double &q,
+		const double &qdot
+		);
+
+}
+
+#endif /* _JOINT_H */

include/rbdl/Kinematics.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _KINEMATICS_H
+#define _KINEMATICS_H
+
+#include <rbdl/rbdl_math.h>
+#include <assert.h>
+#include <iostream>
+#include "rbdl/Logging.h"
+
+namespace RigidBodyDynamics {
+
+/** \defgroup kinematics_group Kinematics
+ * @{
+ *
+ * \note Please note that in the Rigid %Body Dynamics Library all angles
+ * are specified in radians.
+ *
+ */
+
+/** \brief Updates and computes velocities and accelerations of the bodies
+ *
+ * This function updates the kinematic variables such as body velocities
+ * and accelerations in the model to reflect the variables passed to this function.
+ *
+ * \param model the model
+ * \param Q     the positional variables of the model
+ * \param QDot  the generalized velocities of the joints
+ * \param QDDot the generalized accelerations of the joints
+ */
+void UpdateKinematics (Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &QDDot
+		);
+
+/** \brief Selectively updates model internal states of body positions, velocities and/or accelerations.
+ *
+ * This function updates the kinematic variables such as body velocities and
+ * accelerations in the model to reflect the variables passed to this function.
+ *
+ * In contrast to UpdateKinematics() this function allows to update the model
+ * state with values one is interested and thus reduce computations (e.g. only
+ * positions, only positions + accelerations, only velocities, etc.).
+ 
+ * \param model the model
+ * \param Q     the positional variables of the model
+ * \param QDot  the generalized velocities of the joints
+ * \param QDDot the generalized accelerations of the joints
+ */
+void UpdateKinematicsCustom (Model &model,
+		const Math::VectorNd *Q,
+		const Math::VectorNd *QDot,
+		const Math::VectorNd *QDDot
+		);
+
+/** \brief Returns the base coordinates of a point given in body coordinates.
+ *
+ * \param model the rigid body model
+ * \param Q the curent genereralized positions
+ * \param body_id id of the body for which the point coordinates are expressed
+ * \param point_body_coordinates coordinates of the point in body coordinates
+ * \param update_kinematics whether UpdateKinematics() should be called
+ * or not (default: true)
+ *
+ * \returns a 3-D vector with coordinates of the point in base coordinates
+ */
+Math::Vector3d CalcBodyToBaseCoordinates (
+		Model &model,
+		const Math::VectorNd &Q,
+		unsigned int body_id,
+		const Math::Vector3d &body_point_position,
+		bool update_kinematics = true);
+
+/** \brief Returns the body coordinates of a point given in base coordinates.
+ *
+ * \param model the rigid body model
+ * \param Q the curent genereralized positions
+ * \param body_id id of the body for which the point coordinates are expressed
+ * \param point_base_coordinates coordinates of the point in base coordinates
+ * \param update_kinematics whether UpdateKinematics() should be called or not
+ * (default: true).
+ *
+ * \returns a 3-D vector with coordinates of the point in body coordinates
+ */
+Math::Vector3d CalcBaseToBodyCoordinates (
+		Model &model,
+		const Math::VectorNd &Q,
+		unsigned int body_id,
+		const Math::Vector3d &base_point_position,
+		bool update_kinematics = true);
+
+/** \brief Returns the orientation of a given body as 3x3 matrix
+ *
+ * \param model the rigid body model
+ * \param Q the curent genereralized positions
+ * \param body_id id of the body for which the point coordinates are expressed
+ * \param update_kinematics whether UpdateKinematics() should be called or not
+ * (default: true).
+ *
+ * \returns An orthonormal 3x3 matrix that rotates vectors from base coordinates
+ * to body coordinates.
+ */
+Math::Matrix3d CalcBodyWorldOrientation (
+		Model &model,
+		const Math::VectorNd &Q,
+		const unsigned int body_id,
+		bool update_kinematics = true);
+
+/** \brief Computes the point jacobian for a point on a body
+ *
+ * If a position of a point is computed by a function \f$g(q(t))\f$ for which its
+ * time derivative is \f$\frac{d}{dt} g(q(t)) = G(q)\dot{q}\f$ then this
+ * function computes the jacobian matrix \f$G(q)\f$.
+ *
+ * \param model   rigid body model
+ * \param Q       state vector of the internal joints
+ * \param body_id the id of the body
+ * \param point_position the position of the point in body-local data
+ * \param G       a matrix where the result will be stored in
+ * \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
+ *
+ * \returns A 3 x \#dof_count matrix of the point jacobian
+ */
+void CalcPointJacobian (Model &model,
+		const Math::VectorNd &Q,
+		unsigned int body_id,
+		const Math::Vector3d &point_position,
+		Math::MatrixNd &G,
+		bool update_kinematics = true
+		);
+
+/** \brief Computes the velocity of a point on a body 
+ *
+ * \param model   rigid body model
+ * \param Q       state vector of the internal joints
+ * \param QDot    velocity vector of the internal joints
+ * \param body_id the id of the body
+ * \param point_position the position of the point in body-local data
+ * \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
+ *
+ * \returns The cartesian velocity of the point in global frame (output)
+ */
+Math::Vector3d CalcPointVelocity (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		unsigned int body_id,
+		const Math::Vector3d &point_position,
+		bool update_kinematics = true
+		);
+
+/** \brief Computes the acceleration of a point on a body 
+ *
+ * \param model   rigid body model
+ * \param Q       state vector of the internal joints
+ * \param QDot    velocity vector of the internal joints
+ * \param QDDot    velocity vector of the internal joints
+ * \param body_id the id of the body
+ * \param point_position the position of the point in body-local data
+ * \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
+ *
+ * \returns The cartesian acceleration of the point in global frame (output)
+ *
+ * The kinematic state of the model has to be updated before valid
+ * values can be obtained. This can either be done by calling
+ * UpdateKinematics() or setting the last parameter update_kinematics to
+ * true (default).
+ *
+ * \note During the execution of ForwardDynamics() the acceleration
+ * is only applied on the root body and propagated form there. Therefore
+ * in the internal state the accelerations of the bodies only represent
+ * the relative accelerations without any gravitational effects.
+ *
+ * \warning  If this function is called after ForwardDynamics() without
+ * an update of the kinematic state one has to add the gravity
+ * acceleration has to be added to the result.
+ */
+
+Math::Vector3d CalcPointAcceleration (
+		Model &model,
+		const Math::VectorNd &Q,
+		const Math::VectorNd &QDot,
+		const Math::VectorNd &QDDot,
+		unsigned int body_id,
+		const Math::Vector3d &point_position,
+		bool update_kinematics = true
+	);
+
+/** \brief Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method
+ *
+ * \param model rigid body model
+ * \param Qinit initial guess for the state
+ * \param body_id a vector of all bodies for which we we have kinematic target positions
+ * \param body_point a vector of points in body local coordinates that are
+ * to be matched to target positions
+ * \param target_pos a vector of target positions
+ * \param Qres output of the computed inverse kinematics
+ * \param step_tol tolerance used for convergence detection
+ * \param lambda damping factor for the least squares function
+ * \param max_iter maximum number of steps that should be performed
+ * \returns true on success, false otherwise
+ *
+ * This function repeatedly computes
+ *   \f[ Qres = Qres + \Delta \theta\f]
+ *   \f[ \Delta \theta = G^T (G^T G + \lambda^2 I)^{-1} e \f]
+ * where \f$G = G(q) = \frac{d}{dt} g(q(t))\f$ and \f$e\f$ is the
+ * correction of the body points so that they coincide with the target
+ * positions. The function returns true when \f$||\Delta \theta||_2 \le\f$
+ * step_tol or if the error between body points and target gets smaller
+ * than step_tol. Otherwise it returns false.
+ *
+ * The parameter \f$\lambda\f$ is the damping factor that has to
+ * be chosen carefully. In case of unreachable positions higher values (e.g
+ * 0.9) can be helpful. Otherwise values of 0.0001, 0.001, 0.01, 0.1 might
+ * yield good results. See the literature for best practices.
+ *
+ * \warning The actual accuracy might be rather low (~1.0e-2)! Use this function with a
+ * grain of suspicion.
+ */
+bool InverseKinematics (
+		Model &model,
+		const Math::VectorNd &Qinit,
+		const std::vector<unsigned int>& body_id,
+		const std::vector<Math::Vector3d>& body_point,
+		const std::vector<Math::Vector3d>& target_pos,
+		Math::VectorNd &Qres,
+		double step_tol = 1.0e-12,
+		double lambda = 0.01,
+		unsigned int max_iter = 50
+		);
+
+/** @} */
+
+}
+
+#endif /* _KINEMATICS_H */

include/rbdl/Logging.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef LOGGING_H
+#define LOGGING_H
+
+#include <sstream>
+
+class _NoLogging;
+
+/** \def RBDL_ENABLE_LOGGING
+ *
+ * Enables/Disables logging
+ *
+ * \warning Logging has a huge impact on performance.
+ */
+
+#ifndef RBDL_ENABLE_LOGGING
+	#define LOG if (false) LogOutput 
+	#define SUPPRESS_LOGGING ;
+#else
+	#define LOG LogOutput
+	#define SUPPRESS_LOGGING _NoLogging _nolog
+#endif
+
+extern std::ostringstream LogOutput;
+void ClearLogOutput ();
+
+/** \brief Helper object to ignore any logs that happen during its lifetime
+ *
+ * If an instance of this class exists all logging gets suppressed. This
+ * allows to disable logging for a certain scope or a single function call,
+ * e.g.
+ *
+ * \code
+ * {
+ *   // logging will be active
+ *   do_some_stuff();
+ *  
+ *   // now create a new scope in which a _NoLogging instance exists
+ *   {
+ *     _NoLogging ignore_logging;
+ *    
+ *     // as a _Nologging instance exists, all logging will be discarded
+ *     do_some_crazy_stuff();
+ *   }
+ *
+ *   // logging will be active again
+ *   do_some_more_stuff();
+ * }
+ * \endcode
+ *
+ */
+class _NoLogging {
+	public:
+		_NoLogging() {
+			log_backup.str("");
+			log_backup << LogOutput.str();
+		}
+		~_NoLogging() {
+			LogOutput.str("");
+			LogOutput << log_backup.str();
+		}
+
+	private:
+		std::ostringstream log_backup;
+};
+
+#endif /* LOGGING_H */

include/rbdl/MatrixAddons.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _MATRIXADDONS_H
+#define _MATRIXADDONS_H
+
+	  /** \brief Constructs an initialized 3x3 matrix with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(
+        const Scalar& m00, const Scalar& m01, const Scalar& m02,
+        const Scalar& m10, const Scalar& m11, const Scalar& m12,
+        const Scalar& m20, const Scalar& m21, const Scalar& m22
+        )
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, 3, 3)
+      m_storage.data()[0] = m00;
+      m_storage.data()[1] = m01;
+      m_storage.data()[2] = m02;
+
+      m_storage.data()[3] = m10;
+      m_storage.data()[4] = m11;
+      m_storage.data()[5] = m12;
+
+      m_storage.data()[6] = m20;
+      m_storage.data()[7] = m21;
+      m_storage.data()[8] = m22;
+    }
+
+    /** \brief Constructs an initialized 6D vector with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(const Scalar& v0, const Scalar& v1, const Scalar& v2, const Scalar& v3, const Scalar& v4, const Scalar& v5)
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 6)
+      m_storage.data()[0] = v0;
+      m_storage.data()[1] = v1;
+      m_storage.data()[2] = v2;
+      m_storage.data()[3] = v3;
+      m_storage.data()[4] = v4;
+      m_storage.data()[5] = v5;
+    }
+
+    /** \brief Constructs an initialized 6x6 matrix with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(
+        const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05,
+        const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15,
+        const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25,
+        const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35,
+        const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45,
+        const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55
+        )
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, 6, 6)
+      m_storage.data()[ 0] = m00;
+      m_storage.data()[ 1] = m01;
+      m_storage.data()[ 2] = m02;
+      m_storage.data()[ 3] = m03;
+      m_storage.data()[ 4] = m04;
+      m_storage.data()[ 5] = m05;
+
+      m_storage.data()[ 6] = m10;
+      m_storage.data()[ 7] = m11;
+      m_storage.data()[ 8] = m12;
+      m_storage.data()[ 9] = m13;
+      m_storage.data()[10] = m14;
+      m_storage.data()[11] = m15;
+
+      m_storage.data()[12] = m20;
+      m_storage.data()[13] = m21;
+      m_storage.data()[14] = m22;
+      m_storage.data()[15] = m23;
+      m_storage.data()[16] = m24;
+      m_storage.data()[17] = m25;
+
+      m_storage.data()[18] = m30;
+      m_storage.data()[19] = m31;
+      m_storage.data()[20] = m32;
+      m_storage.data()[21] = m33;
+      m_storage.data()[22] = m34;
+      m_storage.data()[23] = m35;
+
+      m_storage.data()[24] = m40;
+      m_storage.data()[25] = m41;
+      m_storage.data()[26] = m42;
+      m_storage.data()[27] = m43;
+      m_storage.data()[28] = m44;
+      m_storage.data()[29] = m45;
+
+      m_storage.data()[30] = m50;
+      m_storage.data()[31] = m51;
+      m_storage.data()[32] = m52;
+      m_storage.data()[33] = m53;
+      m_storage.data()[34] = m54;
+      m_storage.data()[35] = m55;
+    }
+
+		void  set(
+        const Scalar& v0, const Scalar& v1, const Scalar& v2
+         )
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, 3, 1) 
+      m_storage.data()[0] = v0;
+      m_storage.data()[1] = v1;
+      m_storage.data()[2] = v2;
+    }
+
+		void  set(
+        const Scalar& m00, const Scalar& m01, const Scalar& m02,
+        const Scalar& m10, const Scalar& m11, const Scalar& m12,
+        const Scalar& m20, const Scalar& m21, const Scalar& m22
+        )
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, 3, 3)
+      m_storage.data()[0] = m00;
+      m_storage.data()[1] = m01;
+      m_storage.data()[2] = m02;
+
+      m_storage.data()[3] = m10;
+      m_storage.data()[4] = m11;
+      m_storage.data()[5] = m12;
+
+      m_storage.data()[6] = m20;
+      m_storage.data()[7] = m21;
+      m_storage.data()[8] = m22;
+    }
+
+    void set (const Scalar& v0, const Scalar& v1, const Scalar& v2, const Scalar& v3, const Scalar& v4, const Scalar& v5)
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 6)
+      m_storage.data()[0] = v0;
+      m_storage.data()[1] = v1;
+      m_storage.data()[2] = v2;
+      m_storage.data()[3] = v3;
+      m_storage.data()[4] = v4;
+      m_storage.data()[5] = v5;
+    }
+
+    void set(
+        const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05,
+        const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15,
+        const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25,
+        const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35,
+        const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45,
+        const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55
+        )
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, 6, 6)
+      m_storage.data()[ 0] = m00;
+      m_storage.data()[ 1] = m01;
+      m_storage.data()[ 2] = m02;
+      m_storage.data()[ 3] = m03;
+      m_storage.data()[ 4] = m04;
+      m_storage.data()[ 5] = m05;
+
+      m_storage.data()[ 6] = m10;
+      m_storage.data()[ 7] = m11;
+      m_storage.data()[ 8] = m12;
+      m_storage.data()[ 9] = m13;
+      m_storage.data()[10] = m14;
+      m_storage.data()[11] = m15;
+
+      m_storage.data()[12] = m20;
+      m_storage.data()[13] = m21;
+      m_storage.data()[14] = m22;
+      m_storage.data()[15] = m23;
+      m_storage.data()[16] = m24;
+      m_storage.data()[17] = m25;
+
+      m_storage.data()[18] = m30;
+      m_storage.data()[19] = m31;
+      m_storage.data()[20] = m32;
+      m_storage.data()[21] = m33;
+      m_storage.data()[22] = m34;
+      m_storage.data()[23] = m35;
+
+      m_storage.data()[24] = m40;
+      m_storage.data()[25] = m41;
+      m_storage.data()[26] = m42;
+      m_storage.data()[27] = m43;
+      m_storage.data()[28] = m44;
+      m_storage.data()[29] = m45;
+
+      m_storage.data()[30] = m50;
+      m_storage.data()[31] = m51;
+      m_storage.data()[32] = m52;
+      m_storage.data()[33] = m53;
+      m_storage.data()[34] = m54;
+      m_storage.data()[35] = m55;
+    }
+#endif

include/rbdl/Model.h

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#ifndef _MODEL_H
+#define _MODEL_H
+
+#include <rbdl/rbdl_math.h>
+#include <map>
+#include <list>
+#include <assert.h>
+#include <iostream>
+#include <limits>
+#include <cstring>
+
+#include "rbdl/Logging.h"
+#include "rbdl/Joint.h"
+#include "rbdl/Body.h"
+
+// std::vectors containing any objectst that have Eigen matrices or vectors
+// as members need to have a special allocater. This can be achieved with
+// the following macro.
+
+#ifdef EIGEN_CORE_H
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody);
+#endif
+
+/** \brief Namespace for all structures of the RigidBodyDynamics library
+ */
+namespace RigidBodyDynamics {
+
+/** \defgroup model_group Modelling
+ * @{
+ *
+ * There are two ways of creating models for RBDL:
+ *
+ *   \li Using \ref luamodel_introduction that uses Lua files or
+ *   \li using the C++ interface.
+ *
+ * The first approach requires the addon \ref luamodel_introduction to be
+ * activated which is done by enabling BUILD_ADDON_LUAMODEL in CMake and is
+ * recommended when one is not interested in the details of RBDL and simply
+ * wants to create a model.
+ *
+ * \section modeling_lua Modeling using Lua
+ *
+ * For this see the documentation of \ref luamodel_introduction.
+ *
+ * \section modeling_cpp Modeling using C++
+ *
+ * Using the C++ interface is more advanced but gives some overview about the
+ * internals of RBDL.
+ *
+ * \subsection modeling_overview Overview
+ *
+ * All model related values are stored in the model structure \link
+ * RigidBodyDynamics::Model\endlink. The functions 
+ * \link RigidBodyDynamics::Model::Init Model::Init()\endlink,
+ * \link RigidBodyDynamics::Model::AddBody Model::AddBody(...)\endlink,
+ * \link RigidBodyDynamics::Model::AppendBody Model::AppendBody(...)\endlink, and
+ * \link RigidBodyDynamics::Model::GetBodyId Model::GetBodyId(...)\endlink,
+ * are used to initialize and construct the \ref model_structure.
+ *
+ * \section model_construction Model Construction
+ *
+ * The construction of \link RigidBodyDynamics::Model Models \endlink makes
+ * use of carefully designed constructors of the classes \link
+ * RigidBodyDynamics::Body Body \endlink and \link RigidBodyDynamics::Joint
+ * Joint \endlink to ease the process of articulated models. Adding bodies to
+ * the model is done by specifying the parent body by its id, the
+ * transformation from the parent origin to the joint origin, the joint
+ * specification as an object, and the body itself. These parameters are
+ * then fed to the function RigidBodyDynamics::Model::AddBody().
+ *
+ * To create a model with a floating base (a.k.a a model with a free-flyer
+ * joint) it is recommended to use \link
+ * RigidBodyDynamics::Model::SetFloatingBaseBody
+ * Model::SetFloatingBaseBody(...)\endlink.
+ *
+ * Once this is done, the model structure can be used with the functions of \ref
+ * kinematics_group, \ref dynamics_group, \ref contacts_group, to perform
+ * computations.
+ *
+ * A simple example can be found \ref SimpleExample "here".
+ *
+ * \subsection model_structure Model Structure
+ *
+ * The model structure contains all the parameters of the rigid multi-body
+ * model such as joint informations, mass and inertial parameters of the
+ * rigid bodies, etc. It also contains storage for the transformations and
+ * current state, such as velocity and acceleration of each body.
+ *
+ * \subsection joint_models Joint Modeling
+ *
+ * The Rigid Body Dynamics Library supports models with multiple degrees of
+ * freedom. When a joint with more than one degrees of freedom is used,
+ * additional virtual bodies with zero mass that are connected by 1 degree
+ * of freedom joints to simulate the multiple degrees of freedom joint. Even
+ * though this adds some overhead in terms of memory usage, it allows to
+ * exploit fast computations on fixed size elements of the underlying math
+ * library Eigen3.
+ *
+ * Joints are defined by their motion subspace. For each degree of freedom
+ * a one dimensional motion subspace is specified as a Math::SpatialVector.
+ * This vector follows the following convention:
+ *   \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+ *
+ * To specify a planar joint with three degrees of freedom for which the
+ * first two are translations in \f$x\f$ and \f$y\f$ direction and the last
+ * is a rotation around \f$z\f$, the following joint definition can be used:
+ *
+ * \code
+ * Joint planar_joint = Joint ( 
+ *     Math::SpatialVector (0., 0., 0., 1., 0., 0.),
+ *     Math::SpatialVector (0., 0., 0., 0., 1., 0.),
+ *     Math::SpatialVector (0., 0., 1., 0., 0., 0.)
+ *     );
+ * \endcode
+ *
+ * \subsubsection joint_models_fixed Fixed Joints
+ *
+ * Fixed joints do not add an additional degree of freedom to the model. 
+ * When adding a body that via a fixed joint (i.e. when the type is
+ * JointTypeFixed) then the dynamical parameters mass and inertia are
+ * merged onto its moving parent. By doing so fixed bodies do not add
+ * computational costs when performing dynamics computations.
+ 
+ * To ensure a consistent API for the Kinematics such fixed bodies have a
+ * different range of ids. Where as the ids start at 1 get incremented for
+ * each added body, fixed bodies start at Model::fixed_body_discriminator
+ * which has a default value of std::numeric_limits<unsigned int>::max() /
+ * 2. This means theoretical a maximum of each 2147483646 movable and fixed
+ * bodies are possible.
+ 
+ * To check whether a body is connected by a fixed joint you can use the
+ * function Model::IsFixedBodyId().
+ *
+ * See also: \link RigidBodyDynamics::Joint Joint\endlink.
+ *
+ * \note Please note that in the Rigid %Body Dynamics Library all angles
+ * are specified in radians.
+ */
+
+/** \brief Contains all information about the rigid body model
+ *
+ * This class contains all information required to perform the forward
+ * dynamics calculation. The variables in this class are also used for
+ * storage of temporary values. It is designed for use of the Articulated
+ * Rigid Body Algorithm (which is implemented in ForwardDynamics()) and
+ * follows the numbering as described in Featherstones book.
+ *
+ * An important note is that body 0 is the root body and the moving bodies
+ * start at index 1. Additionally the vectors for the states q, qdot, etc.
+ * have \#Model::mBodies + 1 entries where always the first entry (e.g.
+ * q[0]) contains the value for the base (or "root" body). Thus the
+ * numbering might be confusing as q[1] holds the position variable of the
+ * first added joint. This numbering scheme is very beneficial in terms of
+ * readability of the code as the resulting code is very similar to the
+ * pseudo-code in the RBDA book.
+ *
+ * \note To query the number of degrees of freedom use Model::dof_count.
+ */
+struct Model {
+	// Structural information
+
+	/// \brief The id of the parents body
+	std::vector<unsigned int> lambda;
+	/// \brief Contains the ids of all the children of a given body
+	std::vector<std::vector<unsigned int> >mu;
+
+	/** \brief number of degrees of freedoms of the model
+	 *
+	 * This value contains the number of entries in the generalized state (q)
+	 * velocity (qdot), acceleration (qddot), and force (tau) vector.
+	 */
+	unsigned int dof_count;
+	/// \brief Id of the previously added body, required for Model::AppendBody()
+	unsigned int previously_added_body_id;
+
+	/// \brief the cartesian vector of the gravity
+	Math::Vector3d gravity;
+
+	// State information
+	/// \brief The spatial velocity of the bodies
+	std::vector<Math::SpatialVector> v;
+	/// \brief The spatial acceleration of the bodies
+	std::vector<Math::SpatialVector> a;
+
+	////////////////////////////////////
+	// Joints
+
+	/// \brief All joints
+	
+	std::vector<Joint> mJoints;
+	/// \brief The joint axis for joint i
+	std::vector<Math::SpatialVector> S;
+	/// \brief Transformations from the parent body to the frame of the joint
+	std::vector<Math::SpatialTransform> X_T;
+	/// \brief The number of fixed joints that have been declared before each joint.
+	std::vector<unsigned int> mFixedJointCount;
+
+	////////////////////////////////////
+	// Dynamics variables
+
+	/// \brief The velocity dependent spatial acceleration
+	std::vector<Math::SpatialVector> c;
+	/// \brief The spatial inertia of the bodies 
+	std::vector<Math::SpatialMatrix> IA;
+	/// \brief The spatial bias force
+	std::vector<Math::SpatialVector> pA;
+	/// \brief Temporary variable U_i (RBDA p. 130)
+	std::vector<Math::SpatialVector> U;
+	/// \brief Temporary variable D_i (RBDA p. 130)
+	Math::VectorNd d;
+	/// \brief Temporary variable u (RBDA p. 130)
+	Math::VectorNd u;
+	/// \brief Internal forces on the body (used only InverseDynamics())
+	std::vector<Math::SpatialVector> f;
+	/// \brief The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
+	std::vector<Math::SpatialRigidBodyInertia> Ic;
+
+	////////////////////////////////////
+	// Bodies
+
+	/// \brief Transformation from the parent body to the current body
+	std::vector<Math::SpatialTransform> X_lambda;
+	/// \brief Transformation from the base to bodies reference frame
+	std::vector<Math::SpatialTransform> X_base;
+
+	/// \brief All bodies that are attached to a body via a fixed joint.
+	std::vector<FixedBody> mFixedBodies;
+	/** \brief Value that is used to discriminate between fixed and movable
+	 * bodies.
+	 *
+	 * Bodies with id 1 .. (fixed_body_discriminator - 1) are moving bodies
+	 * while bodies with id fixed_body_discriminator .. max (unsigned int)
+	 * are fixed to a moving body. The value of max(unsigned int) is
+	 * determined via std::numeric_limits<unsigned int>::max() and the
+	 * default value of fixed_body_discriminator is max (unsigned int) / 2.
+	 * 
+	 * On normal systems max (unsigned int) is 4294967294 which means there
+	 * could be a total of 2147483646 movable and / or fixed bodies.
+	 */
+	unsigned int fixed_body_discriminator;
+
+	/** \brief All bodies 0 ... N_B, including the base
+	 *
+	 * mBodies[0] - base body <br>
+	 * mBodies[1] - 1st moveable body <br>
+	 * ... <br>
+	 * mBodies[N_B] - N_Bth moveable body <br>
+	 */
+	std::vector<Body> mBodies;
+
+	/// \brief Human readable names for the bodies
+	std::map<std::string, unsigned int> mBodyNameMap;
+
+	/** \brief Connects a given body to the model
+	 *
+	 * When adding a body there are basically informations required:
+	 * - what kind of body will be added?
+	 * - where is the new body to be added?
+	 * - by what kind of joint should the body be added?
+	 *
+	 * The first information "what kind of body will be added" is contained
+	 * in the Body class that is given as a parameter.
+	 *
+	 * The question "where is the new body to be added?" is split up in two
+	 * parts: first the parent (or successor) body to which it is added and
+	 * second the transformation to the origin of the joint that connects the
+	 * two bodies. With these two informations one specifies the relative
+	 * positions of the bodies when the joint is in neutral position.gk
+	 *
+	 * The last question "by what kind of joint should the body be added?" is
+	 * again simply contained in the Joint class.
+	 *
+	 * \param parent_id   id of the parent body
+	 * \param joint_frame the transformation from the parent frame to the origin
+	 *                    of the joint frame (represents X_T in RBDA)
+	 * \param joint       specification for the joint that describes the connection
+	 * \param body        specification of the body itself
+	 * \param body_name   human readable name for the body (can be used to retrieve its id
+	 *                    with GetBodyId())
+	 *
+	 * \returns id of the added body
+	 */
+	unsigned int AddBody (
+			const unsigned int parent_id,
+			const