Anonymous avatar Anonymous committed 9470ff7 Draft

Changed include structure, moved Eigen from src/ to third/, added RBDL_USE_OWN_EIGEN to have option to use own Eigen install, otherwise use /usr/include/eigen3. Updated style in CMakeLists.txt to match Martin's.

Comments (2)

  1. Eric Cousineau repo owner

    Hi Martin,

    I just wanted to change all of the header includes of RBDL to be prefixed with "rbdl/" to effectively 'namespace' the files so that they do not conflict with other packages. I also moved Eigen to the third/ directory and have an option (disabled by default) to use that version of Eigen versus using an existing installation. A coworker and I are using this updated fork and it has been working for us thus far. We had both had problems with trying to build / use RBDL with the existing include file setup. Apparently the compiler does not (always?) search for files relatively to headers, just source files. I had tried doing this earlier with my other fork (that I need to delete), but ended up screwing up the branches haha. Does this change seem appropriate to you?

    Thank you for the help! - Eric

Files changed (653)

 Debug/*
 Release/*
 RelWithDebInfo/*
-build/*
+build*/*
 
 .*.swp
 CMakeFiles/*
 examples/luamodel/example_luamodel
 examples/luamodel/CMakeCache.txt
 
+*~
+.cproject
+.project
+*.orig
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
 PROJECT (RBDL)
 
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
-
 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
 SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
 SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
+# Up the optimization level to -O4
+set(CMAKE_CXX_FLAGS_RELEASE "-O4 -DNDEBUG")
+set(CMAKE_C_FLAGS_RELEASE "-O4 -DNDEBUG")
+
 # Options
 OPTION (BUILD_STATIC "Build the statically linked library" OFF)
 OPTION (BUILD_TESTS "Build the test executables" OFF)
 OPTION (BUILD_ADDON_URDFREADER "Build the (experimental) urdf reader" OFF)
 OPTION (BUILD_ADDON_BENCHMARK "Build the benchmarking tool" OFF)
 OPTION (BUILD_ADDON_LUAMODEL "Build the lua model reader" OFF)
+OPTION (RBDL_USE_OWN_EIGEN "Compile with and install RBDL's version of Eigen. Otherwise use system's installation" OFF)
+
+#TODO Use FindPackage
+IF (NOT RBDL_USE_SIMPLE_MATH)
+  SET (RBDL_EIGEN_INCLUDE /usr/include/eigen3)
+  IF (RBDL_USE_OWN_EIGEN)
+    MESSAGE ("Using own version of Eigen3")
+    SET (RBDL_EIGEN_INCLUDE ${PROJECT_SOURCE_DIR}/third)
+  ENDIF (RBDL_USE_OWN_EIGEN)
+  INCLUDE_DIRECTORIES ("${RBDL_EIGEN_INCLUDE}")
+ENDIF (NOT RBDL_USE_SIMPLE_MATH)
 
 SET ( RBDL_SOURCES 
 	src/rbdl_version.cc
 	src/Dynamics.cc
 	src/Contacts.cc
 	)
-
+	
 ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} )
 
 IF (BUILD_ADDON_URDFREADER)
 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 )
 
-IF (NOT RBDL_USE_SIMPLE_MATH)
+IF (RBDL_USE_OWN_EIGEN AND NOT RBDL_USE_SIMPLE_MATH)
 	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src/Eigen"
 		DESTINATION include/rbdl
 	)
-ELSE (NOT RBDL_USE_SIMPLE_MATH)
+ELSEIF (RBDL_USE_SIMPLE_MATH)
 	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src/SimpleMath"
 		DESTINATION include/rbdl
 	)
-ENDIF (NOT RBDL_USE_SIMPLE_MATH)
+ENDIF (RBDL_USE_OWN_EIGEN AND NOT 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;
+	}