Commits

Martin Felis committed 0f2b7a0 Merge

merge spherical_joint->default

Comments (0)

Files changed (37)

 Introduction
 ============
 
-The RBDL is a highly efficient C++ library that contains some essential
-rigid body dynamics algorithms such as the Articulated Body Algorithm (ABA)
-for forward dynamics, Newton-Euler Algorithm for inverse dynamics and the
-Composite Rigid Body Algorithm (CRBA) for the efficient computation of
-the joint space inertia matrix. It further contains code for forward and
+RBDL is a highly efficient C++ library that contains some essential rigid
+body dynamics algorithms such as the Articulated Body Algorithm (ABA) for
+forward dynamics, Newton-Euler Algorithm for inverse dynamics and the
+Composite Rigid Body Algorithm (CRBA) for the efficient computation of the
+joint space inertia matrix. It further contains code for forward and
 inverse kinematics and handling of external constraints such as contacts
 and collisions.
 
 
 Recent Changes
 ==============
+   * 28 October 2013: New version 2.2.0: added support for spherical joints that do not suffer from joint singularities
    * 29 September 2013: New version 2.1.0: adjusted build settings and symbol export to be debian compatible. Removed vendor code such as Lua 5.2 and UnitTest++. Must be pre-installed if tests or LuaModel Addon is enabled.
    * 05 September 2013: New version 2.0.1: fixed some errors on older compilers and CMake configuration of examples. No changes required when migrating from 2.0.0.
    * 18 July 2013: API version 2.0.0: removed Eigen3 sources, removed Model::Init(), inverted sign of contact forces/impulses
 This is the full license text (zlib license):
 
     RBDL - Rigid Body Dynamics Library
-    Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+    Copyright (c) 2011-2013 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
     
     This software is provided 'as-is', without any express or implied
     warranty. In no event will the authors be held liable for any damages
        appreciated but is not required.
     
        2. Altered source versions must be plainly marked as such, and must not
-			 be misrepresented as being the original software.
+       be misrepresented as being the original software.
     
        3. This notice may not be removed or altered from any source
        distribution.

addons/luamodel/luamodel.cc

File contents unchanged.
  *
  * \section recent_changes Recent Changes :
  * <ul>
+ * <li> 28 October 2013: New version 2.2.0: added support for
+ * spherical joints that do not suffer from \ref joint_singularities
  * <li> 29 September 2013: New version 2.1.0: adjusted build settings and symbol export to be debian compatible. Removed vendor code such as Lua 5.2 and UnitTest++. Must be pre-installed if tests or LuaModel Addon is enabled.</li>
  * <li>05 September 2013: New version 2.0.1: fixed some errors on older compilers and CMake configuration of examples. No changes required when migrating from 2.0.0.</li>
  * <li> 18. July 2013: new API version 2.0.0 for details see (\ref
  * <li> 01. June 2012: added support of \ref joint_models_fixed</li>
  * <li> 14. May 2012: fixed Body constructor as reported by Maxime Reis</li>
  * <li> 04. April 2012: added benchmark tool for CRBA</li>
- * <li> 01. March 2012: added multi degree of freedom \ref joint_models</li>
+ * <li> 01. March 2012: added multi degree of freedom \ref
+ * joint_description</li>
  * <li> 06. Februry 2012: restructured constraint handling using \ref RigidBodyDynamics::ConstraintSet</li>
  * <li> 24. January 2012: implemented compact and fast representation of \ref RigidBodyDynamics::Math::SpatialTransform </li>
  * </ul>
  *
- * \section Example Example
+ * \section Example Examples
  *
  * A simple example for creation of a model and computation of the forward
  * dynamics using the C++ API can be found \ref SimpleExample "here".
  * \section ModuleOverview API reference separated by functional modules
  * 
  * \li \subpage modeling_page
+ * \li \subpage joint_description
  * \li \subpage kinematics_page
  * \li \subpage dynamics_page
  * \li \subpage contacts_page

doc/api_changes.txt

+2.1.0 -> 2.2.0
+- added spherical joints that do not suffer from singularities:
+  Use joint type JointTypeSpherical
+- added Model::q_size, and Model::qdot_size that report the sizes including
+	all 4 Quaternion parameters when spherical joints are used.
+	User is advised to use Model::q_size and Model::qdot_size instead of
+	Model::dof_count.
+- removed "constraint_" prefix from constraint impulses, forces and
+	accelerations from the ConstraintSets:
+	renaming required if values are queried	
+- Contact impulses: specification of a contact velocity after a collision:
+	added ConstraintSet::v_plus which can be set for the desired constraint
+	velocity after a collision. Previously it used the values stored in 
+  ConstraintSet::constraint_acceleration.
+	User has to store desired exit velocities manually in CS::v_plus
+
 2.0.1 -> 2.1.0
 - made codebase compatible to Debian
 	Binary symbol export was changed. No change in user code required,

examples/luamodel/example_luamodel.cc

 
 	model = new Model();
 
-	if (!Addons::LuaModelReadFromFile ("./samplemodel.lua", model)) {
+	if (!Addons::LuaModelReadFromFile ("./samplemodel.lua", model, false)) {
 		std::cerr << "Error loading model ./samplemodel.lua" << std::endl;
 		abort();
 	}

include/rbdl/Body.h

 	 *
 	 * \param transform The frame transformation from the origin of the
 	 * original body to the origin of the added body
+	 * \param other_body The other body that will be merged with *this.
 	 */
 	void Join (const Math::SpatialTransform &transform, const Body &other_body) {
 		// nothing to do if we join a massles body to the current.

include/rbdl/Contacts.h

 #ifndef _CONTACTS_H
 #define _CONTACTS_H
 
-#include "rbdl/rbdl_math.h"
-#include "rbdl/rbdl_mathutils.h"
+#include <rbdl/rbdl_math.h>
+#include <rbdl/rbdl_mathutils.h>
 
 namespace RigidBodyDynamics {
 
 /** \page contacts_page External Contacts
  *
+ * All functions related to contacts are specified in the \ref
+ * contacts_group "Contacts Module".
+
+ * \defgroup contacts_group Contacts
+ *
  * External contacts are handled by specification of a \link
- * RigidBodyDynamics::ForwardDynamicsContacts::ConstraintSet
+ * RigidBodyDynamics::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
+ * RigidBodyDynamics::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 are for each
- * constraint the force that is excerted on the body by the constraint.
- * Similarly ConstraintSet::constraint_impulse holds for each constraint
- * the impulse due to a collision that is excerted by the constraint on the
- * body.
+ * The values in the vectors ConstraintSet::force and
+ * ConstraintSet::impulse contain the computed force or
+ * impulse values for each constraint when returning from one of the
+ * contact functions.
  *
- * All functions related to contacts are specified in the \ref
- * contacts_group "Contacts Module".
- *
- * \defgroup contacts_group Contacts
+*
  * @{
  */
 
 	 * \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
+	 * \param normal_acceleration the acceleration of the contact along the normal
 	 * (optional, default: 0.)
 	 */
 	unsigned int AddConstraint (
 			const Math::Vector3d &body_point,
 			const Math::Vector3d &world_normal,
 			const char *constraint_name = NULL,
-			double acceleration = 0.);
+			double normal_acceleration = 0.);
 
 	/** \brief Copies the constraints and resets its ConstraintSet::bound
 	 * flag.
 	 * 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
+	 * The values of ConstraintSet::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();
+		return acceleration.size();
 	}
 
 	/** \brief Clears all variables in the constraint set. */
 
 	/** Enforced accelerations of the contact points along the contact
 	 * normal. */
-	Math::VectorNd constraint_acceleration;
-	/** Actual constraint forces along the contact normals that is excerted on
-	 * the body. */
-	Math::VectorNd constraint_force;
-	/** Actual constraint impulses along the contact normals that is acting
-	 * on the body. */
-	Math::VectorNd constraint_impulse;
+	Math::VectorNd acceleration;
+	/** Actual constraint forces along the contact normals. */
+	Math::VectorNd force;
+	/** Actual constraint impulses along the contact normals. */
+	Math::VectorNd impulse;
+	/** The velocities we want to have along the contact normals after
+	 * calling ComputeContactImpulsesLagrangian */
+	Math::VectorNd v_plus;
 
 	// Variables used by the Lagrangian methods
 
 	std::vector<Math::SpatialVector> d_U;
 	/// Workspace when applying constraint forces
 	Math::VectorNd d_d;
+
+	std::vector<Math::Matrix63> d_multdof3_U;
+	std::vector<Math::Matrix3d> d_multdof3_Dinv;
+	std::vector<Math::Vector3d> d_multdof3_u;
 };
 
 /** \brief Computes forward dynamics with contact by constructing and solving the full lagrangian equation
  * \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 CS    the description of all acting constraints
  * \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
+ * ConstraintSet::force get modified and will contain the value
  * of the force acting along the normal.
  *
  */
  \left(
    \begin{array}{c}
 	   \dot{q}^{+} \\
-		 -\Lambda
+		 \Lambda
    \end{array}
  \right)
  =
  * 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).
+ * 0). The value of \f$v^{+}\f$ can is specified via the variable
+ * ConstraintSet::v_plus and defaults to 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
  * \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
+ * ConstraintSet::force get modified and will contain the value
  * of the force acting along the normal.
  *
  * \todo Allow for external forces

include/rbdl/Joint.h

 
 struct Model;
 
+/** \page joint_description Joint Description
+ *
+ * The Rigid Body Dynamics Library supports models with multiple degrees of
+ * freedom. By default, a joint with multiple degrees of freedom is split
+ * up into multiple single degrees of freedom joints. This simplifies the
+ * required algebra and code branching within RBDL. However this approach
+ * may lead to models, that suffer from singularities. For this case RBDL
+ * contains a special joint that can be used to model singularity-free
+ * models.
+ *
+ * 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
+
+ * \note Please note that in the Rigid %Body Dynamics Library all angles
+ * are specified in radians.
+ 
+ * \section 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().
+ 
+ * \section joint_singularities Singularities
+ 
+ * Singularities in the models arise when a joint has three rotational
+ * degrees of freedom and the rotations are described by Euler- or
+ * Cardan-angles. The singularities present in these rotation
+ * parametrizations (e.g. for ZYX Euler-angles for rotations where a 
+ * +/- 90 degrees rotation around the Y-axis) may cause problems in
+ * dynamics calculations, such as a rank-deficit joint-space inertia matrix
+ * or exploding accelerations in the forward dynamics calculations.
+ *
+ * For this case RBDL has the special joint type
+ * RigidBodyDynamics::JointTypeSpherical. When using this joint type the
+ * model does not suffer from singularities, however this also results in
+ * a change of interpretation for the values \f$\mathbf{q}, \mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, and \f$\mathbf{\tau}\f$:
+ *
+ * <ul>
+ * <li> The values in \f$\mathbf{q}\f$ for the joint parameterizes the orientation of a joint using a
+ * Quaternion \f$q_i\f$ </li>
+ * <li> The values in \f$\mathbf{\dot{q}}\f$ for the joint describe the angular
+ * velocity \f$\omega\f$ of the joint in body coordinates</li>
+ * <li> The values in \f$\mathbf{\ddot{q}}\f$ for the joint describe the angular
+ * acceleration \f$\dot{\omega}\f$ of the joint in body coordinates</li>
+ * <li> The values in \f$\mathbf{\tau}\f$ for the joint describe the three couples
+ * acting on the body in body coordinates that are actuated by the joint.</li>
+ * </ul>
+  
+ * As a result, the dimension of the vector \f$\mathbf{q}\f$ is higher than
+ * of the vector of the velocity variables. Additionally, the values in
+ * \f$\mathbf{\dot{q}}\f$ are \b not the derivative of \f$q\f$ and are therefore
+ * denoted by \f$\mathbf{\bar{q}}\f$ (and similarly for the joint
+ * accelerations).
+ 
+ * RBDL stores the Quaternions in \f$\mathbf{q}\f$ such that the 4th component of
+ * the joint is appended to \f$\mathbf{q}\f$. E.g. for a model with the joints:
+ * TX, Spherical, TY, Spherical, the values of \f$\mathbf{q},\mathbf{\bar{q}},\mathbf{\bar{\bar{q}}},\mathbf{\tau}\f$ are:
+ *
+ * \f{eqnarray*}
+\mathbf{q} &=& ( q_{tx}, q_{q1,x}, q_{q1,y}, q_{q1,z}, q_{ty}, q_{q2,x}, q_{q2,y}, q_{q2,z}, q_{q1,w}, q_{q2,w})^T \\
+\mathbf{\bar{q}} &=& ( \dot{q}_{tx}, \omega_{1,x}, \omega_{1,y}, \omega_{1,z}, \dot{q}_{ty}, \omega_{2,x}, \omega_{2,y}, \omega_{2,z} )^T \\
+\mathbf{\bar{\bar{q}}} &=& ( \ddot{q}_{tx}, \dot{\omega}_{1,x}, \dot{\omega}_{1,y}, \dot{\omega}_{1,z}, \ddot{q}_{ty}, \dot{\omega}_{2,x}, \dot{\omega}_{2,y}, \dot{\omega}_{2,z} )^T \\
+\mathbf{\tau} &=& ( \tau_{tx}, \tau_{1,x}, \tau_{1,y}, \tau_{1,z}, \tau_{ty}, \tau_{2,x}, \tau_{2,y}, \tau_{2,z} )^T 
+\f}
+
+ * \subsection spherical_integration Numerical Integration of Quaternions
+ *
+ * An additional consequence of this is, that special treatment is required
+ * when numerically integrating the angular velocities. One possibility is
+ * to interpret the angular velocity as an axis-angle pair scaled by the
+ * timestep and use it create a quaternion that is applied to the previous
+ * Quaternion. Another is to compute the quaternion rates from the angular
+ * velocity. For details see James Diebel "Representing Attitude: Euler
+ * Angles, Unit Quaternions, and Rotation Vectors", 2006,
+ * http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134.
+ *
+ */
+
 /** \brief General types of joints
  */
 enum RBDL_DLLAPI JointType {
 	JointTypeUndefined = 0,
 	JointTypeRevolute,
 	JointTypePrismatic,
+	JointTypeSpherical,
 	JointTypeFixed,
 
 	JointType1DoF,
 	JointType3DoF,
 	JointType4DoF,
 	JointType5DoF,
-	JointType6DoF
+	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.
+ * contains the joint type and the axis of the joint. See \ref joint_description for detailed description.
+ *
  */
 struct RBDL_DLLAPI Joint {
 	Joint() :
 		mJointAxes (NULL),
 		mJointType (JointTypeUndefined),
-		mDoFCount (0) {};
+		mDoFCount (0),
+		q_index (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;
+	  mDoFCount (0),
+		q_index (0) {
+			if (type == JointTypeSpherical) {
+				mDoFCount = 3;
+
+				mJointAxes = new Math::SpatialVector[mDoFCount];
+
+				mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
+				mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
+				mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
+			} else if (type != JointTypeFixed) {
+				std::cerr << "Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type == JointTypeFixed or JointTypeSpherical." << std::endl;
 				assert (0);
 				abort();
 			}
 		}
 	Joint (const Joint &joint) :
 		mJointType (joint.mJointType),
-		mDoFCount (joint.mDoFCount) {
+		mDoFCount (joint.mDoFCount),
+		q_index (joint.q_index) {
 			mJointAxes = new Math::SpatialVector[mDoFCount];
 
 			for (unsigned int i = 0; i < mDoFCount; i++)
 
 			for (unsigned int i = 0; i < mDoFCount; i++)
 				mJointAxes[i] = joint.mJointAxes[i];
+
+			q_index = joint.q_index;
 		}
 		return *this;
 	}
 	/// \brief Type of joint (rotational or prismatic)
 	JointType mJointType;
 	unsigned int mDoFCount;
+	unsigned int q_index;
 };
 
 /** \brief Computes all variables for a joint model
  * \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
+ * \param q        joint state variables
+ * \param qdot     joint velocity variables
  */
 RBDL_DLLAPI
 void jcalc (
-		const Model &model,
-		const unsigned int &joint_id,
+		Model &model,
+		unsigned int joint_id,
 		Math::SpatialTransform &XJ,
-		Math::SpatialVector &S,
 		Math::SpatialVector &v_J,
 		Math::SpatialVector &c_J,
-		const double &q,
-		const double &qdot
+		const Math::VectorNd &q,
+		const Math::VectorNd &qdot
 		);
-
 }
 
 #endif /* _JOINT_H */

include/rbdl/Kinematics.h

  * \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 body_point_position coordinates of the point in body coordinates
  * \param update_kinematics whether UpdateKinematics() should be called
  * or not (default: true)
  *
  * \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 base_point_position coordinates of the point in base coordinates
  * \param update_kinematics whether UpdateKinematics() should be called or not
  * (default: true).
  *

include/rbdl/Model.h

  */
 namespace RigidBodyDynamics {
 
-/** \page modeling_page Modelling
+/** \page modeling_page Model 
  *
- * There are two ways of creating models for RBDL:
+ * There are two ways of creating \link RigidBodyDynamics::Model Models\endlink for RBDL:
  *
  *   \li Using \ref luamodel_introduction that uses Lua files or
  *   \li using the C++ interface.
  *
  * \section model_construction Model Construction
  *
- * The construction of \link RigidBodyDynamics::Model Models \endlink makes
+ * The construction of \link RigidBodyDynamics::Model Model Structures \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
  * 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
+ * kinematics_group, \ref dynamics_group, \ref contacts_page, 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.
+ * The \link RigidBodyDynamics::Model Model Structure \endlink 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.
  
- * 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.
+ * See also \ref joint_description for information about Joint modeling.
  */
 
 /** \brief Contains all information about the rigid body model
 	 * velocity (qdot), acceleration (qddot), and force (tau) vector.
 	 */
 	unsigned int dof_count;
+
+	/** \brief The size of the \f$\mathbf{q}\f$-vector. 
+	 * For models without spherical joints the value is the same as
+	 * Model::dof_count, otherwise additional values for the w-component of the
+	 * Quaternion is stored at the end of \f$\mathbf{q}\f$. 
+	 *
+	 * \sa \ref joint_description for more details.
+	 */
+	unsigned int q_size;
+	/** \brief The size of the \f$\mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, and \f$\mathbf{\tau}\f$-vector. 
+	 *
+	 * \sa \ref joint_description for more details.
+	 */
+	unsigned int qdot_size;
+
 	/// \brief Id of the previously added body, required for Model::AppendBody()
 	unsigned int previously_added_body_id;
 
 	std::vector<unsigned int> mFixedJointCount;
 
 	////////////////////////////////////
+	// Special variables for joints with 3 degrees of freedom
+	/// \brief Motion subspace for joints with 3 degrees of freedom
+	std::vector<Math::Matrix63> multdof3_S;
+	std::vector<Math::Matrix63> multdof3_U;
+	std::vector<Math::Matrix3d> multdof3_Dinv;
+	std::vector<Math::Vector3d> multdof3_u;
+	std::vector<unsigned int> multdof3_w_index;
+
+	////////////////////////////////////
 	// Dynamics variables
 
 	/// \brief The velocity dependent spatial acceleration
 			std::string body_name = "" 
 			);
 
+	unsigned int AddBodySphericalJoint (
+			const unsigned int parent_id,
+			const Math::SpatialTransform &joint_frame,
+			const Joint &joint,
+			const Body &body,
+			std::string body_name = "" 
+			);
+
 	/** \brief Adds a Body to the model such that the previously added Body is the Parent.
 	 *
 	 * This function is basically the same as Model::AddBody() however the
 		}
 	}
 
+	Math::Quaternion GetQuaternion (unsigned int i, const Math::VectorNd &Q) const {
+		assert (mJoints[i].mJointType == JointTypeSpherical);
+		unsigned int q_index = mJoints[i].q_index;
+		return Math::Quaternion (Q[q_index], Q[q_index + 1], Q[q_index + 2], Q[multdof3_w_index[i]]);
+	}
 
+	void SetQuaternion (unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const {
+		assert (mJoints[i].mJointType == JointTypeSpherical);
+		unsigned int q_index = mJoints[i].q_index;
+		
+		Q[q_index] = quat[0];
+		Q[q_index + 1] = quat[1];
+		Q[q_index + 2] = quat[2];
+		Q[multdof3_w_index[i]] = quat[3];
+	}
 };
 
 /** @} */

include/rbdl/Quaternion.h

+#ifndef _RBDL_QUATERNION_H
+#define _RBDL_QUATERNION_H
+
+#include <cmath>
+
+namespace RigidBodyDynamics {
+
+namespace Math {
+
+/** Quaternion 
+ *
+ * order: x,y,z,w
+ */
+class Quaternion : public Vector4d {
+	public:
+		Quaternion () :
+			Vector4d (0.f, 0.f, 0.f, 1.f)
+		{}
+		Quaternion (const Vector4d vec4) :
+			Vector4d (vec4)
+		{}
+		Quaternion (double x, double y, double z, double w):
+			Vector4d (x, y, z, w)
+		{}
+		Quaternion operator* (const double &s) const {
+			return Quaternion (
+					(*this)[0] * s,
+					(*this)[1] * s,
+					(*this)[2] * s,
+					(*this)[3] * s
+					);
+		}
+		/** This function is equivalent to multiplicate their corresponding rotation matrices */
+		Quaternion operator* (const Quaternion &q) const {
+			return Quaternion (
+					q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1],
+					q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2],
+					q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0],
+					q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2]
+					);
+		}
+		Quaternion& operator*=(const Quaternion &q) {
+			set (
+					q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1],
+					q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2],
+					q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0],
+					q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2]
+					);
+			return *this;
+		}
+
+		static Quaternion fromGLRotate (double angle, double x, double y, double z) {
+			double st = std::sin (angle * M_PI / 360.f);
+			return Quaternion (
+						st * x,
+						st * y,
+						st * z,
+						cosf (angle * M_PI / 360.f)
+						);
+		}
+
+		Quaternion slerp (double alpha, const Quaternion &quat) const {
+			// check whether one of the two has 0 length
+			double s = std::sqrt (squaredNorm() * quat.squaredNorm());
+
+			// division by 0.f is unhealthy!
+			assert (s != 0.);
+
+			double angle = acos (dot(quat) / s);
+			if (angle == 0. || isnan(angle)) {
+				return *this;
+			}
+			assert(!isnan(angle));
+
+			double d = 1. / std::sin (angle);
+			double p0 = std::sin ((1. - alpha) * angle);
+			double p1 = std::sin (alpha * angle);
+
+			if (dot (quat) < 0.) {
+				return Quaternion( ((*this) * p0 - quat * p1) * d);
+			}
+			return Quaternion( ((*this) * p0 + quat * p1) * d);
+		}
+
+		static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) {
+			double d = axis.norm();
+			double s2 = std::sin (angle_rad * 0.5) / d;
+			return Quaternion (
+					axis[0] * s2,
+					axis[1] * s2,
+					axis[2] * s2,
+					std::cos(angle_rad * 0.5)
+					);
+		}
+
+		static Quaternion fromMatrix (const Matrix3d &mat) {
+			double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
+			return Quaternion (
+					(mat(1,2) - mat(2,1)) / (w * 4.),
+					(mat(2,0) - mat(0,2)) / (w * 4.),
+					(mat(0,1) - mat(1,0)) / (w * 4.),
+					w);
+		}
+
+		static Quaternion fromZYXAngles (const Vector3d &zyx_angles) {
+			return Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]) 
+				* Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), zyx_angles[1])
+				* Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), zyx_angles[0]);
+		}
+
+		Matrix3d toMatrix() const {
+			double x = (*this)[0];
+			double y = (*this)[1];
+			double z = (*this)[2];
+			double w = (*this)[3];
+			return Matrix3d (
+					1 - 2*y*y - 2*z*z,
+					2*x*y + 2*w*z,
+					2*x*z - 2*w*y,
+
+					2*x*y - 2*w*z,
+					1 - 2*x*x - 2*z*z,
+					2*y*z + 2*w*x,
+
+					2*x*z + 2*w*y,
+					2*y*z - 2*w*x,
+					1 - 2*x*x - 2*y*y
+
+/*
+					1 - 2*y*y - 2*z*z,
+					2*x*y - 2*w*z,
+					2*x*z + 2*w*y,
+
+					2*x*y + 2*w*z,
+					1 - 2*x*x - 2*z*z,
+					2*y*z - 2*w*x,
+
+					2*x*z - 2*w*y,
+					2*y*z + 2*w*x,
+					1 - 2*x*x - 2*y*y
+					*/
+			);
+		}
+
+		Quaternion conjugate() const {
+			return Quaternion (
+					-(*this)[0],
+					-(*this)[1],
+					-(*this)[2],
+					(*this)[3]);
+		}
+
+		Quaternion timeStep (const Vector3d &omega, double dt) {
+			double omega_norm = omega.norm();
+			return (*this) * Quaternion::fromAxisAngle (omega / omega_norm, dt * omega_norm);
+		}
+
+		Vector3d rotate (const Vector3d &vec) const {
+			Vector3d vn (vec);
+			Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;
+
+			res_quat = vec_quat * (*this);
+			res_quat = conjugate() * res_quat;
+
+			return Vector3d (res_quat[0], res_quat[1], res_quat[2]);
+		}
+};
+
+}
+
+}
+
+/* _RBDL_QUATERNION_H */
+#endif

include/rbdl/SimpleMath/SimpleMathBlock.h

File contents unchanged.

include/rbdl/SimpleMath/SimpleMathDynamic.h

 			zero();
 		}
 
-		val_type norm() {
+		val_type norm() const {
 			return sqrt(this->squaredNorm());
 		}
 
-		void normalize() {
+		Matrix<val_type> normalize() {
 			val_type length = this->norm();
 
 			for (unsigned int i = 0; i < ncols * nrows; i++)
 				mData[i] /= length;
+
+			return *this;
+		}
+
+		matrix_type normalized() {
+			return matrix_type (*this) / this->norm();
 		}
 
 		Matrix<val_type> cross(const Matrix<val_type> &other_vector) {
 			return mData[0];
 		}
 
-		const HouseholderQR<matrix_type> householderQR() const {
+		Matrix inverse() const {
+			return colPivHouseholderQr().inverse();
+		}
+
+		const HouseholderQR<matrix_type> householderQr() const {
 			return HouseholderQR<matrix_type>(*this);
 		}
-		const ColPivHouseholderQR<matrix_type> colPivHouseholderQR() const {
+		const ColPivHouseholderQR<matrix_type> colPivHouseholderQr() const {
 			return ColPivHouseholderQR<matrix_type>(*this);
 		}
 

include/rbdl/SimpleMath/SimpleMathFixed.h

 			zero();
 		}
 
-		val_type norm() {
+		val_type norm() const {
 			return sqrt(this->squaredNorm());
 		}
 
 			return *this;
 		}
 
+		matrix_type normalized() {
+			return matrix_type (*this) / this->norm();
+		}
+
 		Matrix<val_type, 3, 1> cross(const Matrix<val_type, 3, 1> &other_vector) {
 			COMPILE_ASSERT (nrows * ncols == 3);
 
 			return *this * -1.;
 		}
 
-		const HouseholderQR<matrix_type> householderQR() const {
+		Matrix inverse() const {
+			return colPivHouseholderQr().inverse();
+		}
+
+		const HouseholderQR<matrix_type> householderQr() const {
 			return HouseholderQR<matrix_type>(*this);
 		}
-		const ColPivHouseholderQR<matrix_type> colPivHouseholderQR() const {
+		const ColPivHouseholderQR<matrix_type> colPivHouseholderQr() const {
 			return ColPivHouseholderQR<matrix_type>(*this);
 		}
 

include/rbdl/SimpleMath/SimpleMathQR.h

 			}
 			Dynamic::Matrix<value_type> inverse() const {
 				assert (mIsFactorized);
-				assert (0 && !"Not properly tested!");
 
 				VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
 				MatrixXXd result (mQ.cols(), mQ.cols());

include/rbdl/SimpleMath/compileassert.h

File contents unchanged.

include/rbdl/rbdl.h

 
 #include "rbdl/Contacts.h"
 
-/** \page api_version_checking_page API Version Checking
+/** \page api_version_checking_page API Changes
  * @{
  *
- * This documentation was created for API version 2.0.0.
+ * This documentation was created for API version 2.2.0.
  *
  * Here is a list of changes introduced by the different versions and what
  * adjustements have to be made to migrate.

include/rbdl/rbdl_config.h.cmake

 #ifndef _RBDLCONFIG_H
 #define _RBDLCONFIG_H
 
-#define RBDL_API_VERSION 0x020100
+#define RBDL_API_VERSION 0x020200
 
 #cmakedefine RBDL_USE_SIMPLE_MATH
 #cmakedefine RBDL_ENABLE_LOGGING

include/rbdl/rbdl_eigenmath.h

 		}
 };
 
+class RBDL_DLLAPI Vector4_t : public Eigen::Vector4d
+{
+	public:
+		typedef Eigen::Vector4d Base;
+
+		template<typename OtherDerived>
+			Vector4_t(const Eigen::MatrixBase<OtherDerived>& other)
+			: Eigen::Vector4d(other)
+			{}
+
+		template<typename OtherDerived>
+			Vector4_t& operator=(const Eigen::MatrixBase<OtherDerived>& other)
+			{
+				this->Base::operator=(other);
+				return *this;
+			}
+
+		EIGEN_STRONG_INLINE Vector4_t()
+		{}
+
+		EIGEN_STRONG_INLINE Vector4_t(
+				const double& v0, const double& v1, const double& v2, const double& v3
+				)
+		{
+			Base::_check_template_params();
+
+			(*this) << v0, v1, v2, v3;
+		}
+
+		void set(const double& v0, const double& v1, const double& v2, const double& v3)
+		{
+			Base::_check_template_params();
+
+			(*this) << v0, v1, v2, v3;
+		}
+};
+
 class RBDL_DLLAPI SpatialVector_t : public Eigen::Matrix<double, 6, 1>
 {
 	public:

include/rbdl/rbdl_math.h

 
 	typedef SimpleMath::Fixed::Matrix<double, 3,1> Vector3_t;
 	typedef SimpleMath::Fixed::Matrix<double, 3,3> Matrix3_t;
+	typedef SimpleMath::Fixed::Matrix<double, 4,1> Vector4_t;
 
 	typedef SimpleMath::Fixed::Matrix<double, 6,1> SpatialVector_t;
 	typedef SimpleMath::Fixed::Matrix<double, 6,6> SpatialMatrix_t;
 
+	typedef SimpleMath::Fixed::Matrix<double, 6,3> Matrix63_t;
+
 	typedef SimpleMath::Dynamic::Matrix<double> MatrixN_t;
 	typedef SimpleMath::Dynamic::Matrix<double> VectorN_t;
 
 
 	#include <Eigen/Dense>
 	#include <Eigen/StdVector>
+	#include <Eigen/QR>
 
 	#include "rbdl/rbdl_eigenmath.h"
 
+	typedef Eigen::Matrix<double, 6, 3> Matrix63_t;
+
 	typedef Eigen::VectorXd VectorN_t;
 	typedef Eigen::MatrixXd MatrixN_t;
 #endif
 /** \brief Math types such as vectors and matrices and utility functions. */
 namespace Math {
 	typedef Vector3_t Vector3d;
+	typedef Vector4_t Vector4d;
 	typedef Matrix3_t Matrix3d;
 	typedef SpatialVector_t SpatialVector;
 	typedef SpatialMatrix_t SpatialMatrix;
+	typedef Matrix63_t Matrix63;
 	typedef VectorN_t VectorNd;
 	typedef MatrixN_t MatrixNd;
 } /* Math */
 
 } /* RigidBodyDynamics */
 
+#include "rbdl/Quaternion.h"
 #include "rbdl/SpatialAlgebraOperators.h"
 
 // If we use Eigen3 we have to create specializations of the STL
 #ifndef RBDL_USE_SIMPLE_MATH
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialVector)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialMatrix)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix63)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialTransform)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialRigidBodyInertia)
 #endif

include/rbdl/rbdl_mathutils.h

  */
 RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler);
 
+RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) {
+	double s, c;
+	s = sin (xrot);
+	c = cos (xrot);
+	return Matrix3d (
+				1., 0., 0.,
+				0., c, s,
+				0., -s, c
+			);
+}
+
+RBDL_DLLAPI inline Matrix3d roty (const double &yrot) {
+	double s, c;
+	s = sin (yrot);
+	c = cos (yrot);
+	return Matrix3d (
+				c, 0., -s,
+				0., 1., 0.,
+				s, 0., c
+			);
+}
+
+RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) {
+	double s, c;
+	s = sin (zrot);
+	c = cos (zrot);
+	return Matrix3d (
+				c, s, 0.,
+				-s, c, 0.,
+				0., 0., 1.
+			);
+}
+
+RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) {
+	double s, c;
+	s = sin (x);
+	c = cos (x);
+	return Matrix3d (
+				0., 0., 0.,
+				0., -s * xdot, c * xdot,
+				0., -c * xdot,-s * xdot 
+			);
+}
+
+RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) {
+	double s, c;
+	s = sin (y);
+	c = cos (y);
+	return Matrix3d (
+				-s * ydot, 0., - c * ydot,
+				0., 0., 0.,
+				c * ydot, 0., - s * ydot 
+			);
+}
+
+RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) {
+	double s, c;
+	s = sin (z);
+	c = cos (z);
+	return Matrix3d (
+				-s * zdot, c * zdot, 0.,
+				-c * zdot, -s * zdot, 0.,
+				0., 0., 0.
+			);
+}
+
+RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) {
+	double sy = sin(zyx_angles[1]);
+	double cy = cos(zyx_angles[1]);
+	double sx = sin(zyx_angles[2]);
+	double cx = cos(zyx_angles[2]);
+
+	return Vector3d (
+			zyx_angle_rates[2] - sy * zyx_angle_rates[0],
+			cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0],
+			-sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0]
+			);
+}
+
+RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates) {
+	Matrix3d RzT = rotz(zyx_angles[0]).transpose();
+	Matrix3d RyT = roty(zyx_angles[1]).transpose();
+	Matrix3d RxT = rotx(zyx_angles[2]).transpose();
+
+	return Vector3d (
+			Vector3d (0., 0., zyx_rates[0])
+			+ RzT * Vector3d (0., zyx_rates[1], 0.)
+			+ RzT * RyT * Vector3d (zyx_rates[2], 0., 0.)
+			);
+}
+
+RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) {
+	double sy = sin(zyx_angles[1]);
+	double cy = cos(zyx_angles[1]);
+	double sx = sin(zyx_angles[2]);
+	double cx = cos(zyx_angles[2]);
+	double xdot = zyx_angle_rates[2];
+	double ydot = zyx_angle_rates[1];
+	double zdot = zyx_angle_rates[0];
+	double xddot = zyx_angle_rates_dot[2];
+	double yddot = zyx_angle_rates_dot[1];
+	double zddot = zyx_angle_rates_dot[0];
+
+	return Vector3d (
+			xddot - (cy * ydot * zdot + sy * zddot),
+			-sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * ( - sy * ydot * zdot + cy * zddot),
+			-cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * ( - sy * ydot * zdot + cy * zddot)
+			);
+}
+
 } /* Math */
 
 } /* RigidBodyDynamics */
 		const Vector3d &body_point,
 		const Vector3d &world_normal,
 		const char *constraint_name,
-		double acceleration) {
+		double normal_acceleration) {
 	assert (bound == false);
 
 	std::string name_str;
 	point.push_back (body_point);
 	normal.push_back (world_normal);
 
-	unsigned int n_constr = constraint_acceleration.size() + 1;
+	unsigned int n_constr = acceleration.size() + 1;
 
-	constraint_acceleration.conservativeResize (n_constr);
-	constraint_acceleration[n_constr - 1] = acceleration;
+	acceleration.conservativeResize (n_constr);
+	acceleration[n_constr - 1] = normal_acceleration;
 
-	constraint_force.conservativeResize (n_constr);
-	constraint_force[n_constr - 1] = 0.;
+	force.conservativeResize (n_constr);
+	force[n_constr - 1] = 0.;
 
-	constraint_impulse.conservativeResize (n_constr);
-	constraint_impulse[n_constr - 1] = 0.;
+	impulse.conservativeResize (n_constr);
+	impulse[n_constr - 1] = 0.;
+
+	v_plus.conservativeResize (n_constr);
+	v_plus[n_constr - 1] = 0.;
+
+	d_multdof3_U = std::vector<Math::Matrix63> (n_constr, Math::Matrix63::Zero());
+	d_multdof3_Dinv = std::vector<Math::Matrix3d> (n_constr, Math::Matrix3d::Zero());
+	d_multdof3_u = std::vector<Math::Vector3d> (n_constr, Math::Vector3d::Zero());
 
 	return n_constr - 1;
 }
 }
 
 void ConstraintSet::clear() {
-	constraint_acceleration.setZero();
-	constraint_force.setZero();
-	constraint_impulse.setZero();
+	acceleration.setZero();
+	force.setZero();
+	impulse.setZero();
 
 	H.setZero();
 	C.setZero();
 		// Only alow contact normals along the coordinate axes
 		unsigned int axis_index = 0;
 
-		if (CS.normal[i] == Vector3d(1., 0., 0.))
-			axis_index = 0;
-		else if (CS.normal[i] == Vector3d(0., 1., 0.))
-			axis_index = 1;
-		else if (CS.normal[i] == Vector3d(0., 0., 1.))
-			axis_index = 2;
-		else
-			assert (0 && "Invalid contact normal axis!");
-
 		// only compute the matrix Gi if actually needed
 		if (prev_body_id != CS.body[i] || prev_body_point != CS.point[i]) {
 			CalcPointJacobian (model, Q, CS.body[i], CS.point[i], Gi, false);
 		}
 
 		for (j = 0; j < model.dof_count; j++) {
-			CS.G(i,j) = Gi(axis_index, j);
+			Vector3d gaxis (Gi(0,j), Gi(1,j), Gi(2,j));
+			CS.G(i,j) = gaxis.transpose() * CS.normal[i];
+//			CS.G(i,j) = Gi(axis_index, j);
 		}
 	}
 
 	
 		// we also substract ContactData[i].acceleration such that the contact
 		// point will have the desired acceleration
-		CS.gamma[i] = gamma_i[axis_index] - CS.constraint_acceleration[i];
+		CS.gamma[i] = gamma_i[axis_index] - CS.acceleration[i];
 	}
 	
 	// Build the system
 
 	// Copy back contact forces
 	for (i = 0; i < CS.size(); i++) {
-		CS.constraint_force[i] = -CS.x[model.dof_count + i];
+		CS.force[i] = -CS.x[model.dof_count + i];
 	}
 }
 
 		// Only alow contact normals along the coordinate axes
 		unsigned int axis_index = 0;
 
-		if (CS.normal[i] == Vector3d(1., 0., 0.))
-			axis_index = 0;
-		else if (CS.normal[i] == Vector3d(0., 1., 0.))
-			axis_index = 1;
-		else if (CS.normal[i] == Vector3d(0., 0., 1.))
-			axis_index = 2;
-		else
-			assert (0 && "Invalid contact normal axis!");
-
 		// only compute the matrix Gi if actually needed
 		if (prev_body_id != CS.body[i] || prev_body_point != CS.point[i]) {
 			CalcPointJacobian (model, Q, CS.body[i], CS.point[i], Gi, false);
 		}
 
 		for (j = 0; j < model.dof_count; j++) {
-			CS.G(i,j) = Gi(axis_index, j);
+			Vector3d g_block (Gi(0,j), Gi(1, j), Gi(2,j));
+			/// \TODO use Gi.block<3,0> notation (SimpleMath does not give proper results for that currently.
+			CS.G(i,j) = CS.normal[i].transpose() * g_block;
 		}
 	}
 
 
 	// Build the system: Copy -gamma
 	for (i = 0; i < CS.size(); i++) {
-		CS.b[i + model.dof_count] = CS.constraint_acceleration[i];
+		CS.b[i + model.dof_count] = CS.v_plus[i];
 	}
 
 #ifndef RBDL_USE_SIMPLE_MATH
 
 	// Copy back contact impulses
 	for (i = 0; i < CS.size(); i++) {
-		CS.constraint_impulse[i] = -CS.x[model.dof_count + i];
+		CS.impulse[i] = -CS.x[model.dof_count + i];
 	}
 }
 
 	}
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
-		CS.d_U[i] = CS.d_IA[i] * model.S[i];
-		CS.d_d[i] = model.S[i].dot(model.U[i]);
-		CS.d_u[i] = Tau[i - 1] - model.S[i].dot(CS.d_pA[i]);
+		unsigned int q_index = model.mJoints[i].q_index;
 
-		unsigned int lambda = model.lambda[i];
-		if (lambda != 0) {
-			SpatialMatrix Ia = CS.d_IA[i] - CS.d_U[i] * (CS.d_U[i] / CS.d_d[i]).transpose();
-			SpatialVector pa = CS.d_pA[i] + Ia * model.c[i] + CS.d_U[i] * CS.d_u[i] / CS.d_d[i];
+		if (model.mJoints[i].mDoFCount == 3) {
+			CS.d_multdof3_U[i] = CS.d_IA[i] * model.multdof3_S[i];
+#ifdef EIGEN_CORE_H
+			CS.d_multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * CS.d_multdof3_U[i]).inverse().eval();
+#else
+			CS.d_multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * CS.d_multdof3_U[i]).inverse();
+#endif
+			Vector3d tau_temp (Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]);
+
+			CS.d_multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * CS.d_pA[i];
+
+//			LOG << "multdof3_u[" << i << "] = " << model.multdof3_u[i].transpose() << std::endl;
+			unsigned int lambda = model.lambda[i];
+			if (lambda != 0) {
+				SpatialMatrix Ia = CS.d_IA[i] - CS.d_multdof3_U[i] * CS.d_multdof3_Dinv[i] * CS.d_multdof3_U[i].transpose();
+				SpatialVector pa = CS.d_pA[i] + Ia * model.c[i] + CS.d_multdof3_U[i] * CS.d_multdof3_Dinv[i] * model.multdof3_u[i];
+#ifdef EIGEN_CORE_H
+				CS.d_IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				CS.d_pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
+#else
+				CS.d_IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				CS.d_pA[lambda] += model.X_lambda[i].applyTranspose(pa);
+#endif
+				LOG << "pA[" << lambda << "] = " << CS.d_pA[lambda].transpose() << std::endl;
+			}
+		} else {
+			CS.d_U[i] = CS.d_IA[i] * model.S[i];
+			CS.d_d[i] = model.S[i].dot(model.U[i]);
+			CS.d_u[i] = Tau[i - 1] - model.S[i].dot(CS.d_pA[i]);
+
+			unsigned int lambda = model.lambda[i];
+			if (lambda != 0) {
+				SpatialMatrix Ia = CS.d_IA[i] - CS.d_U[i] * (CS.d_U[i] / CS.d_d[i]).transpose();
+				SpatialVector pa = CS.d_pA[i] + Ia * model.c[i] + CS.d_U[i] * CS.d_u[i] / CS.d_d[i];
 
 #ifdef EIGEN_CORE_H
-			CS.d_IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
-			CS.d_pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
+				CS.d_IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				CS.d_pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
 #else
-			CS.d_IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
-			CS.d_pA[lambda] += model.X_lambda[i].applyTranspose(pa);
+				CS.d_IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				CS.d_pA[lambda] += model.X_lambda[i].applyTranspose(pa);
 #endif
+			}
 		}
 	}
-
 	/*
 	for (i = 0; i < model.mBodies.size(); i++) {
 		LOG << "i = " << i << ": d_IA[i] " << std::endl << d_IA[i] << std::endl;
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
 	for (i = 1; i < model.mBodies.size(); i++) {
+		unsigned int q_index = model.mJoints[i].q_index;
 		unsigned int lambda = model.lambda[i];
 		SpatialTransform X_lambda = model.X_lambda[i];
 
 			CS.d_a[i] = X_lambda.apply(CS.d_a[lambda]) + model.c[i];
 		}
 
-		QDDot[i - 1] = (CS.d_u[i] - model.U[i].dot(CS.d_a[i])) / model.d[i];
-		LOG << "QDDot_t[" << i - 1 << "] = " << QDDot[i - 1] << std::endl;
-		CS.d_a[i] = CS.d_a[i] + model.S[i] * QDDot[i - 1];
-		LOG << "d_a[i] - a[i] = " << (CS.d_a[i] - X_lambda.apply(model.a[i])).transpose() << std::endl;
+		if (model.mJoints[i].mDoFCount == 3) {
+			Vector3d qdd_temp = CS.d_multdof3_Dinv[i] * (CS.d_multdof3_u[i] - CS.d_multdof3_U[i].transpose() * model.a[i]);
+			QDDot[q_index] = qdd_temp[0];
+			QDDot[q_index + 1] = qdd_temp[1];
+			QDDot[q_index + 2] = qdd_temp[2];
+			CS.d_a[i] = CS.d_a[i] + model.multdof3_S[i] * qdd_temp;
+		} else {
+			QDDot[q_index] = (CS.d_u[i] - CS.d_U[i].dot(CS.d_a[i])) / CS.d_d[i];
+			CS.d_a[i] = CS.d_a[i] + model.S[i] * QDDot[q_index];
+		}
 	}
 }
 
 	}
 
 	for (unsigned int i = body_id; i > 0; i--) {
+		unsigned int q_index = model.mJoints[i].q_index;
+
 		if (i == body_id) {
 			CS.d_pA[i] = -model.X_base[i].applyAdjoint(f_t[i]);
 		}
 
-		CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]);
+		if (model.mJoints[i].mDoFCount == 3) {
+			CS.d_multdof3_u[i] = - model.multdof3_S[i].transpose() * (CS.d_pA[i]);
 
-		unsigned int lambda = model.lambda[i];
-		if (lambda != 0) {
-			CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].applyTranspose (CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]);
+			unsigned int lambda = model.lambda[i];
+			if (lambda != 0) {
+				CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].applyTranspose (CS.d_pA[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * CS.d_multdof3_u[i]);
+			}
+		} else {
+			CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]);
+
+			unsigned int lambda = model.lambda[i];
+			if (lambda != 0) {
+				CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].applyTranspose (CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]);
+			}
 		}
 	}
 
 	CS.d_a[0] = model.a[0];
 
 	for (unsigned int i = 1; i < model.mBodies.size(); i++) {
+		unsigned int q_index = model.mJoints[i].q_index;
 		unsigned int lambda = model.lambda[i];
 
 		SpatialVector Xa = model.X_lambda[i].apply(CS.d_a[lambda]);
-		QDDot_t[i - 1] = (CS.d_u[i] - model.U[i].dot(Xa) ) / model.d[i];
-		CS.d_a[i] = Xa + model.S[i] * QDDot_t[i - 1];
 
+		if (model.mJoints[i].mDoFCount == 3) {
+			Vector3d qdd_temp = model.multdof3_Dinv[i] * (CS.d_multdof3_u[i] - model.multdof3_U[i].transpose() * Xa);
+			QDDot_t[q_index] = qdd_temp[0];
+			QDDot_t[q_index + 1] = qdd_temp[1];
+			QDDot_t[q_index + 2] = qdd_temp[2];
+			model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
+			CS.d_a[i] = Xa + model.multdof3_S[i] * qdd_temp;
+		} else {
+			QDDot_t[q_index] = (CS.d_u[i] - model.U[i].dot(Xa) ) / model.d[i];
+			CS.d_a[i] = Xa + model.S[i] * QDDot_t[q_index];
+		}
+	
 		LOG << "QDDot_t[" << i - 1 << "] = " << QDDot_t[i - 1] << std::endl;
 		LOG << "d_a[i] = " << CS.d_a[i].transpose() << std::endl;
 	}
 	assert (CS.point_accel_0.size() == CS.size());
 	assert (CS.K.rows() == CS.size());
 	assert (CS.K.cols() == CS.size());
-	assert (CS.constraint_force.size() == CS.size());
+	assert (CS.force.size() == CS.size());
 	assert (CS.a.size() == CS.size());
 
 	Vector3d point_accel_t;
 		unsigned int body_id = CS.body[ci];
 		Vector3d point = CS.point[ci];
 		Vector3d normal = CS.normal[ci];
-		double acceleration = CS.constraint_acceleration[ci];
+		double acceleration = CS.acceleration[ci];
 
 		LOG << "body_id = " << body_id << std::endl;
 		LOG << "point = " << point << std::endl;
 #ifndef RBDL_USE_SIMPLE_MATH
 	switch (CS.linear_solver) {
 		case (LinearSolverPartialPivLU) :
-			CS.constraint_force = CS.K.partialPivLu().solve(CS.a);
+			CS.force = CS.K.partialPivLu().solve(CS.a);
 			break;
 		case (LinearSolverColPivHouseholderQR) :
-			CS.constraint_force = CS.K.colPivHouseholderQr().solve(CS.a);
+			CS.force = CS.K.colPivHouseholderQr().solve(CS.a);
 			break;
 		default:
 			LOG << "Error: Invalid linear solver: " << CS.linear_solver << std::endl;
 			break;
 	}
 #else
-	bool solve_successful = LinSolveGaussElimPivot (CS.K, CS.a, CS.constraint_force);
+	bool solve_successful = LinSolveGaussElimPivot (CS.K, CS.a, CS.force);
 	assert (solve_successful);
 #endif
 
-	LOG << "f = " << CS.constraint_force.transpose() << std::endl;
+	LOG << "f = " << CS.force.transpose() << std::endl;
 
 	for (ci = 0; ci < CS.size(); ci++) {
 		unsigned int body_id = CS.body[ci];
 
-		CS.f_ext_constraints[body_id] -= CS.f_t[ci] * CS.constraint_force[ci]; 
+		CS.f_ext_constraints[body_id] -= CS.f_t[ci] * CS.force[ci]; 
 		LOG << "f_ext[" << body_id << "] = " << CS.f_ext_constraints[body_id].transpose() << std::endl;
 	}
 
 	model.v[0].setZero();
 
 	for (i = 1; i < model.mBodies.size(); i++) {
+		unsigned int q_index = model.mJoints[i].q_index;
 		SpatialTransform X_J;
 		SpatialVector v_J;
 		SpatialVector c_J;
 		unsigned int lambda = model.lambda[i];
 
-		jcalc (model, i, X_J, model.S[i], v_J, c_J, Q[i - 1], QDot[i - 1]);
-		LOG << "X_T (" << i << "):" << std::endl << model.X_T[i] << std::endl;
+		jcalc (model, i, X_J, v_J, c_J, Q, QDot);
 
 		model.X_lambda[i] = X_J * model.X_T[i];
 
 
 	LOG << "--- first loop ---" << std::endl;
 
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "X_base[" << i << "] = " << model.X_base[i] << std::endl;
-	}
+	for (i = model.mBodies.size() - 1; i > 0; i--) {
+		unsigned int q_index = model.mJoints[i].q_index;
 
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "X_lambda[" << i << "] = " << model.X_lambda[i] << std::endl;
-	}
+		if (model.mJoints[i].mDoFCount == 3) {
+			model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
+#ifdef EIGEN_CORE_H
+			model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval();
+#else
+			model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse();
+#endif
+			Vector3d tau_temp (Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]);
 
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "Xup[" << i << "] = " << model.X_lambda[i] << std::endl;
-	}
+			model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i];
 
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "v[" << i << "]   = " << model.v[i].transpose() << std::endl;
-	}
+//			LOG << "multdof3_u[" << i << "] = " << model.multdof3_u[i].transpose() << std::endl;
+			unsigned int lambda = model.lambda[i];
+			if (lambda != 0) {
+				SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose();
+				SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i];
+#ifdef EIGEN_CORE_H
+				model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
+#else
+				model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
+#endif
+				LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl;
+			}
+		} else {
+			model.U[i] = model.IA[i] * model.S[i];
+			model.d[i] = model.S[i].dot(model.U[i]);
+			model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
+//			LOG << "u[" << i << "] = " << model.u[i] << std::endl;
 
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "IA[" << i << "]  = " << model.IA[i] << std::endl;
-	}
-
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "pA[" << i << "]  = " << model.pA[i].transpose() << std::endl;
-	}
-
-	LOG << std::endl;
-
-	for (i = model.mBodies.size() - 1; i > 0; i--) {
-		model.U[i] = model.IA[i] * model.S[i];
-		model.d[i] = model.S[i].dot(model.U[i]);
-		model.u[i] = Tau[i - 1] - model.S[i].dot(model.pA[i]);
-
-		unsigned int lambda = model.lambda[i];
-		if (lambda != 0) {
-			SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
-			SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i];
+			unsigned int lambda = model.lambda[i];
+			if (lambda != 0) {
+				SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
+				SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i];
 #ifdef EIGEN_CORE_H
-			model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
-			model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
+				model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
 #else
-			model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
-			model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
+				model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+				model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
 #endif
+				LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl;
+			}
 		}
 	}
 
 //	ClearLogOutput();
 
-	LOG << "--- second loop ---" << std::endl;
-
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "U[" << i << "]   = " << model.U[i].transpose() << std::endl;
-	}
-
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "d[" << i << "]   = " << model.d[i] << std::endl;
-	}
-
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "u[" << i << "]   = " << model.u[i] << std::endl;
-	}
-
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "IA[" << i << "]  = " << model.IA[i] << std::endl;
-	}
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "pA[" << i << "]  = " << model.pA[i].transpose() << std::endl;
-	}
-
-	LOG << std::endl << "--- third loop ---" << std::endl;
-
-	LOG << "spatial gravity = " << spatial_gravity.transpose() << std::endl;
-
 	model.a[0] = spatial_gravity * -1.;
 
 	for (i = 1; i < model.mBodies.size(); i++) {
+		unsigned int q_index = model.mJoints[i].q_index;
 		unsigned int lambda = model.lambda[i];
 		SpatialTransform X_lambda = model.X_lambda[i];
 
 		model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
+		LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
 
-		QDDot[i - 1] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
-		model.a[i] = model.a[i] + model.S[i] * QDDot[i - 1];
-	}
-
-	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "c[" << i << "] = " << model.c[i].transpose() << std::endl;
-	}
-
-	LOG << std::endl;
-
-	for (i = 0; i < model.mBodies.size(); i++) {
-		LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl;
+		if (model.mJoints[i].mDoFCount == 3) {
+			Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
+			QDDot[q_index] = qdd_temp[0];
+			QDDot[q_index + 1] = qdd_temp[1];
+			QDDot[q_index + 2] = qdd_temp[2];
+			model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
+		} else {
+			QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
+			model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
+		}
 	}
 
 	LOG << "QDDot = " << QDDot.transpose() << std::endl;
 	model.a[0] = spatial_gravity * -1.;
 
 	for (i = 1; i < model.mBodies.size(); i++) {
+		unsigned int q_index = model.mJoints[i].q_index;
 		SpatialTransform X_J;
 		SpatialVector v_J;
 		SpatialVector c_J;
 		unsigned int lambda = model.lambda[i];
 
-		jcalc (model, i, X_J, model.S[i], v_J, c_J, Q[i - 1], QDot[i - 1]);
+		jcalc (model, i, X_J, v_J, c_J, Q, QDot);
 
 		model.X_lambda[i] = X_J * model.X_T[i];
 
 		if (lambda == 0) {
 			model.X_base[i] = model.X_lambda[i];
 			model.v[i] = v_J;
-			model.a[i] = model.X_base[i].apply(spatial_gravity * -1.) + model.S[i] * QDDot[i - 1];
+			model.a[i] = model.X_base[i].apply(spatial_gravity * -1.);
+			
+			if (model.mJoints[i].mDoFCount == 3) {
+				Vector3d omegadot_temp (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]);
+				model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp;
+			} else {
+				model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
+			}	
+
 		}	else {
 			model.X_base[i] = model.X_lambda[i] * model.X_base.at(lambda);
 			model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + v_J;
 			model.c[i] = c_J + crossm(model.v[i],v_J);
-			model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.S[i] * QDDot[i - 1] + model.c[i];
+			model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
+
+			if (model.mJoints[i].mDoFCount == 3) {
+				Vector3d omegadot_temp (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]);
+				model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp;
+			} else {
+				model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
+			}	
 		}
 
 		model.f[i] = model.mBodies[i].mSpatialInertia * model.a[i] + crossf(model.v[i],model.mBodies[i].mSpatialInertia * model.v[i]);
 	}
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
-		Tau[i - 1] = model.S[i].dot(model.f[i]);
+		unsigned int q_index = model.mJoints[i].q_index;
+		unsigned int lambda = model.lambda[i];
 
-		unsigned int lambda = model.lambda[i];
+		if (model.mJoints[i].mDoFCount == 3) {
+			Vector3d tau_temp = model.multdof3_S[i].transpose() * model.f[i];
+			Tau[q_index] = tau_temp[0];
+			Tau[q_index+1] = tau_temp[1];
+			Tau[q_index+2] = tau_temp[2];
+		} else {
+			Tau[q_index] = model.S[i].dot(model.f[i]);
+		}
 
 		if (lambda != 0) {
 			model.f[lambda] = model.f[lambda] + model.X_lambda[i].applyTranspose(model.f[i]);
 
 	assert (H.rows() == model.dof_count && H.cols() == model.dof_count);
 
-	if (update_kinematics) {
-		for (unsigned int i = 1; i < model.mBodies.size(); i++) {
-			if (model.mJoints[i].mJointType == JointTypeRevolute) {
-				model.X_lambda[i] = Xrot (Q[i-1], Vector3d (
-							model.mJoints[i].mJointAxes[0][0],
-							model.mJoints[i].mJointAxes[0][1],
-							model.mJoints[i].mJointAxes[0][2]
-							)) * model.X_T[i] ;
-			} else if (model.mJoints[i].mJointType == JointTypePrismatic) {
-				model.X_lambda[i] = Xtrans ( Vector3d (
-							model.mJoints[i].mJointAxes[0][3] * Q[i-1],
-							model.mJoints[i].mJointAxes[0][4] * Q[i-1],
-							model.mJoints[i].mJointAxes[0][5] * Q[i-1]
-							)
-						) * model.X_T[i];
-			}
-		}
-	}
+	if (update_kinematics)
+		UpdateKinematicsCustom (model, &Q, NULL, NULL);
 
-//	H.setZero();
+	H.setZero();
 
 	unsigned int i;
-	unsigned int dof_i = model.dof_count;
 
-//	for (i = 1; i < model.mBodies.size(); i++) {
-//		model.Ic[i].createFromMatrix(model.mBodies[i].mSpatialInertia);
-//	}
+	for (i = 1; i < model.mBodies.size(); i++) {
+		model.Ic[i].createFromMatrix(model.mBodies[i].mSpatialInertia);
+	}
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 		unsigned int lambda = model.lambda[i];
 			model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].apply(model.Ic[i]);
 		}
 
-		dof_i = i - 1;
+		unsigned int dof_index_i = model.mJoints[i].q_index;
 
-		SpatialVector F = model.Ic[i] * model.S[i];
-		H(dof_i, dof_i) = model.S[i].dot(F);
+		if (model.mJoints[i].mDoFCount == 3) {
+			Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i];
+			Matrix3d H_temp = model.multdof3_S[i].transpose() * F_63;
 
-		unsigned int j = i;
-		unsigned int dof_j = dof_i;
+			H.block<3,3>(dof_index_i, dof_index_i) = H_temp;
 
-		while (model.lambda[j] != 0) {
-			F = model.X_lambda[j].applyTranspose(F);
-			j = model.lambda[j];
+			unsigned int j = i;
+			unsigned int dof_index_j = dof_index_i;
 
-			dof_j = j - 1;
+			while (model.lambda[j] != 0) {
+				F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63);
+				j = model.lambda[j];
+				dof_index_j = model.mJoints[j].q_index;
 
-			H(dof_i,dof_j) = F.dot(model.S[j]);
-			H(dof_j,dof_i) = H(dof_i,dof_j);
+				if (model.mJoints[j].mDoFCount == 3) {
+					Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]);
+
+					H.block<3,3>(dof_index_i,dof_index_j) = H_temp2;
+					H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
+				} else {
+					Vector3d H_temp2 = F_63.transpose() * (model.S[j]);
+
+					H.block<3,1>(dof_index_i,dof_index_j) = H_temp2;
+					H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
+				}
+			}
+		} else {
+			SpatialVector F = model.Ic[i] * model.S[i];
+			H(dof_index_i, dof_index_i) = model.S[i].dot(F);
+
+			unsigned int j = i;
+			unsigned int dof_index_j = dof_index_i;
+
+			while (model.lambda[j] != 0) {
+				F = model.X_lambda[j].applyTranspose(F);
+				j = model.lambda[j];
+				dof_index_j = model.mJoints[j].q_index;
+
+				if (model.mJoints[j].mDoFCount == 3) {
+					Vector3d H_temp2 = (F.transpose() * model.multdof3_S[j]).transpose();
+
+					H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose();
+ 					H.block<3,1>(dof_index_j,dof_index_i) = H_temp2;
+				} else {
+					H(dof_index_i,dof_index_j) = F.dot(model.S[j]);
+					H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j);
+				}
+			}
 		}
 	}
 }
 
 RBDL_DLLAPI
 void jcalc (
-		const Model &model,
-		const unsigned int &joint_id,
+		Model &model,
+		unsigned int joint_id,
 		SpatialTransform &XJ,
-		SpatialVector &S,
 		SpatialVector &v_J,
 		SpatialVector &c_J,
-		const double &q,
-		const double &qdot
+		const VectorNd &q,
+		const VectorNd &qdot
 		) {
 	// exception if we calculate it for the root body
 	assert (joint_id > 0);
 
-	// Set the joint axis
-	S = model.mJoints[joint_id].mJointAxes[0];
+	if (model.mJoints[joint_id].mDoFCount == 1) {
+		if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
+			XJ = Xrot (q[model.mJoints[joint_id].q_index], Vector3d (
+						model.mJoints[joint_id].mJointAxes[0][0],
+						model.mJoints[joint_id].mJointAxes[0][1],
+						model.mJoints[joint_id].mJointAxes[0][2]
+						));
 
-	// the velocity dependent spatial acceleration is != 0 only for rhenomic
-	// constraints (see RBDA, p. 55)
-	c_J.setZero();
+		} else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) {
+			XJ = Xtrans ( Vector3d (
+						model.mJoints[joint_id].mJointAxes[0][3] * q[model.mJoints[joint_id].q_index],
+						model.mJoints[joint_id].mJointAxes[0][4] * q[model.mJoints[joint_id].q_index],
+						model.mJoints[joint_id].mJointAxes[0][5] * q[model.mJoints[joint_id].q_index]
+						)
+					);
+		}
 
-	if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
-		XJ = Xrot (q, Vector3d (
-					model.mJoints[joint_id].mJointAxes[0][0],
-					model.mJoints[joint_id].mJointAxes[0][1],
-					model.mJoints[joint_id].mJointAxes[0][2]
-					));
-	} else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) {
-		XJ = Xtrans ( Vector3d (
-					model.mJoints[joint_id].mJointAxes[0][3] * q,
-					model.mJoints[joint_id].mJointAxes[0][4] * q,
-					model.mJoints[joint_id].mJointAxes[0][5] * q
-					)
-				);
+		// Set the joint axis
+		model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
+
+		// the velocity dependent spatial acceleration is != 0 only for rhenomic
+		// constraints (see RBDA, p. 55)
+		c_J.setZero();
+
+		v_J = model.S[joint_id] * qdot[model.mJoints[joint_id].q_index];
+	} else if (model.mJoints[joint_id].mDoFCount == JointTypeSpherical) {
+		XJ = SpatialTransform ( model.GetQuaternion (joint_id, q).toMatrix(), Vector3d (0., 0., 0.));
+
+		model.multdof3_S[joint_id].setZero();
+
+		model.multdof3_S[joint_id](0,0) = 1.;
+		model.multdof3_S[joint_id](1,1) = 1.;
+		model.multdof3_S[joint_id](2,2) = 1.;
+
+		Vector3d omega (qdot[model.mJoints[joint_id].q_index],
+				qdot[model.mJoints[joint_id].q_index+1],
+				qdot[model.mJoints[joint_id].q_index+2]);
+
+		v_J = SpatialVector (
+				omega[0], omega[1], omega[2],
+				0., 0., 0.);
+
+		c_J.setZero();
 	} else {
 		// Only revolute joints supported so far
 		assert (0);
 	}
-
-	v_J = S * qdot;
 }
 
 }

src/Kinematics.cc

 	//model.a[0] = spatial_gravity;
 
 	for (i = 1; i < model.mBodies.size(); i++) {
+		unsigned int q_index = model.mJoints[i].q_index;
+
 		SpatialTransform X_J;
 		SpatialVector v_J;
 		SpatialVector c_J;
 		Joint joint = model.mJoints[i];
 		unsigned int lambda = model.lambda[i];
 
-		jcalc (model, i, X_J, model.S[i], v_J, c_J, Q[i - 1], QDot[i - 1]);
+		jcalc (model, i, X_J, v_J, c_J, Q, QDot);
 
 		model.X_lambda[i] = X_J * model.X_T[i];
 
 		}
 		
 		model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
-		model.a[i] = model.a[i] + model.S[i] * QDDot[i - 1];
+
+		if (model.mJoints[i].mDoFCount == 3) {
+			Vector3d omegadot_temp (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]);
+			model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp;
+		} else {
+			model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
+		}	
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
 		LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl;
 	}
-
 }
 
 RBDL_DLLAPI
 		const VectorNd *QDDot
 		) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
-
+	
 	unsigned int i;
 
 	if (Q) {
 		for (i = 1; i < model.mBodies.size(); i++) {
+			unsigned int q_index = model.mJoints[i].q_index;
 			SpatialVector v_J;
 			SpatialVector c_J;
 			SpatialTransform X_J;
 			Joint joint = model.mJoints[i];
 			unsigned int lambda = model.lambda[i];
 
-			jcalc (model, i, X_J, model.S[i], v_J, c_J, (*Q)[i - 1], 0.);
+			VectorNd QDot_zero (VectorNd::Zero (model.q_size));
+
+			jcalc (model, i, X_J, v_J, c_J, (*Q), QDot_zero);
 
 			model.X_lambda[i] = X_J * model.X_T[i];
 
 
 	if (QDot) {
 		for (i = 1; i < model.mBodies.size(); i++) {
+			unsigned int q_index = model.mJoints[i].q_index;
 			SpatialVector v_J;