1. Martin Felis
  2. rbdl

Commits

Martin Felis  committed d58c448

Documented and cleaned up spherical joints, preparing release

  • Participants
  • Parent commits 4f2d7f1
  • Branches dev

Comments (0)

Files changed (11)

File README.md

View file
 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.

File doc/Mainpage.h

View file
  *
  * \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

File doc/api_changes.txt

View file
-2.1.0 -> next 
+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	

File include/rbdl/Body.h

View file
 	 *
 	 * \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.

File include/rbdl/Contacts.h

View file
 
 namespace RigidBodyDynamics {
 
-/** \defgroup contacts_group External Contacts
+/** \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
  * impulse values for each constraint when returning from one of the
  * contact functions.
  *
+*
  * @{
  */
 
  * \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

File include/rbdl/Joint.h

View file
 
 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 {
 /** \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() :
  * \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 (

File include/rbdl/Kinematics.h

View file
  * \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).
  *

File include/rbdl/Model.h

View file
  */
 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
 	 */
 	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()

File include/rbdl/Quaternion.h

View file
 		}
 
 		static Quaternion fromGLRotate (double angle, double x, double y, double z) {
-			double st = sinf (angle * M_PI / 360.f);
+			double st = std::sin (angle * M_PI / 360.f);
 			return Quaternion (
 						st * x,
 						st * y,
 
 		Quaternion slerp (double alpha, const Quaternion &quat) const {
 			// check whether one of the two has 0 length
-			double s = sqrt (squaredNorm() * quat.squaredNorm());
+			double s = std::sqrt (squaredNorm() * quat.squaredNorm());
 
 			// division by 0.f is unhealthy!
-			assert (s != 0.f);
+			assert (s != 0.);
 
 			double angle = acos (dot(quat) / s);
-			if (angle == 0.f || isnan(angle)) {
+			if (angle == 0. || isnan(angle)) {
 				return *this;
 			}
 			assert(!isnan(angle));
 
-			double d = 1.f / sinf (angle);
-			double p0 = sinf ((1.f - alpha) * angle);
-			double p1 = sinf (alpha * angle);
+			double d = 1. / std::sin (angle);
+			double p0 = std::sin ((1. - alpha) * angle);
+			double p1 = std::sin (alpha * angle);
 
-			if (dot (quat) < 0.f) {
+			if (dot (quat) < 0.) {
 				return Quaternion( ((*this) * p0 - quat * p1) * d);
 			}
 			return Quaternion( ((*this) * p0 + quat * p1) * d);
 		}
 
 		static Quaternion fromMatrix (const Matrix3d &mat) {
-			double w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
+			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.f),
-					(mat(2,0) - mat(0,2)) / (w * 4.f),
-					(mat(0,1) - mat(1,0)) / (w * 4.f),
+					(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);
 		}
 

File include/rbdl/rbdl.h

View file
 
 #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.

File include/rbdl/rbdl_config.h.cmake

View file
 #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