Commits

Martin Felis committed e0f64ac Merge

merged new API into default

Comments (0)

Files changed (389)

CMake/FindEigen3.cmake

+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)

CMake/FindUnitTest++.cmake

-# - Try to find UnitTest++
-#
-#
-
-SET (UNITTEST++_FOUND FALSE)
-
-FIND_PATH (UNITTEST++_INCLUDE_DIR UnitTest++.h
-	/usr/include/unittest++ 
-	/usr/local/include/unittest++ 
-	/opt/local/include/unittest++
-	$ENV{UNITTESTXX_PATH}/src 
-	$ENV{UNITTESTXX_INCLUDE_PATH}
-	)
-
-FIND_LIBRARY (UNITTEST++_LIBRARY NAMES UnitTest++ PATHS 
-	/usr/lib 
-	/usr/local/lib 
-	/opt/local/lib 
-	$ENV{UNITTESTXX_PATH} 
-	ENV{UNITTESTXX_LIBRARY_PATH}
-	)
-
-IF (UNITTEST++_INCLUDE_DIR AND UNITTEST++_LIBRARY)
-	SET (UNITTEST++_FOUND TRUE)
-ENDIF (UNITTEST++_INCLUDE_DIR AND UNITTEST++_LIBRARY)
-
-IF (UNITTEST++_FOUND)
-   IF (NOT UnitTest++_FIND_QUIETLY)
-      MESSAGE(STATUS "Found UnitTest++: ${UNITTEST++_LIBRARY}")
-   ENDIF (NOT UnitTest++_FIND_QUIETLY)
-ELSE (UNITTEST++_FOUND)
-   IF (UnitTest++_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find UnitTest++")
-   ENDIF (UnitTest++_FIND_REQUIRED)
-ENDIF (UNITTEST++_FOUND)
-
-MARK_AS_ADVANCED (
-	UNITTEST++_INCLUDE_DIR
-	UNITTEST++_LIBRARY
-	)
 LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake )
 
 INCLUDE_DIRECTORIES ( 
-	${CMAKE_CURRENT_BINARY_DIR}/src
-	src
+	${CMAKE_CURRENT_SOURCE_DIR}/include
+	${CMAKE_CURRENT_BINARY_DIR}/include
 )
 
 SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
 		LINKER_LANGUAGE CXX
 	)
 
+# Find and use the system's Eigen3 library
+FIND_PACKAGE (Eigen3 3.0.0)
+
+IF (NOT EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH)
+	MESSAGE (WARNING "Could not find Eigen3 on your system. Install it or use the slower SimpleMath library by enabling RBDL_USE_SIMPLE_MATH.")
+ENDIF (NOT EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH)
+
+IF (EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH)
+	INCLUDE_DIRECTORIES (${EIGEN3_INCLUDE_DIR})
+ENDIF (EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH)
+
 # Perform the proper linking
 SET (CMAKE_SKIP_BUILD_RPATH FALSE)
 SET (CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
 	src/rbdl_version.cc
 	src/rbdl_mathutils.cc
 	src/rbdl_utils.cc
+	src/Contacts.cc
+	src/Dynamics.cc
 	src/Logging.cc
 	src/Joint.cc
 	src/Model.cc
 	src/Kinematics.cc
-	src/Dynamics.cc
-	src/Contacts.cc
 	)
 
 ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} )
 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)
-	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src/Eigen"
+# Setup of SimpleMath install settings
+IF (RBDL_USE_SIMPLE_MATH)
+	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/SimpleMath"
 		DESTINATION include/rbdl
 	)
-ELSE (NOT RBDL_USE_SIMPLE_MATH)
-	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src/SimpleMath"
-		DESTINATION include/rbdl
-	)
-ENDIF (NOT RBDL_USE_SIMPLE_MATH)
+ENDIF (RBDL_USE_SIMPLE_MATH)
+
 
 Recent Changes
 ==============
- * 20 February 2013: removed too specialized RigidBodyDynamics::Body constructor (API version 1.1.0)
- *  29 January 2013: added code for api_version_checking. Current API version is 1.0.0.
- *  29 January 2013: added code for api_version_checking
- *  18 June 2012: added support of luamodel_introduction
- *  01 June 2012: added support of joint_models_fixed
- *  14 May 2012: fixed Body constructor as reported by Maxime Reis
- *  04 April 2012: added benchmark tool for CRBA
- *  01 March 2012: added multi degree of freedom joint_models
- *  06 Februry 2012: restructured constraint handling using RigidBodyDynamics::ConstraintSet
- *  24 January 2012: implemented compact and fast representation of RigidBodyDynamics::Math::SpatialTransform
+   * 18 July 2013: API version 2.0.0: removed Eigen3 sources, removed Model::Init(), inverted sign of contact forces/impulses
+   * 20 February 2013: removed too specialized RigidBodyDynamics::Body constructor (API version 1.1.0)
+   * 29 January 2013: added code for api_version_checking. Current API version is 1.0.0.
+   * 11 January 2013: removed Eigen3 sources and relying on an already installed Eigen3 library. Optionally RBDL can be used with the included but slower SimpleMath library.
+   * 18 June 2012: added support of luamodel_introduction
+   * 01 June 2012: added support of joint_models_fixed
+   * 14 May 2012: fixed Body constructor as reported by Maxime Reis
+   * 04 April 2012: added benchmark tool for CRBA
+   * 01 March 2012: added multi degree of freedom joint_models
+   * 06 Februry 2012: restructured constraint handling using RigidBodyDynamics::ConstraintSet
+   * 24 January 2012: implemented compact and fast representation of RigidBodyDynamics::Math::SpatialTransform 
 
 Documentation
 =============
 Building and Installation
 =========================
 
-The RBDL is built using CMake (http://www.cmake.org). To compile the
-library in a separate directory in Release mode use:
+The RBDL is built using CMake
+([http://www.cmake.org](http://www.cmake.org)). To compile the library in
+a separate directory in Release mode use:
 
     mkdir build
     cd build/
-    cmake ../ -D CMAKE_BUILD_TYPE=Release
+    cmake -D CMAKE_BUILD_TYPE=Release ../ 
     make
 
+For optimal performance it is highly recommended to install the Eigen3
+linear algebra library from
+[http://eigen.tuxfamily.org]([http://eigen.tuxfamily.org]). RBDL also
+comes with a simple, albeit much slower math library (SimpleMath) that can
+be used by enabling `RBDL_USE_SIMPLE_MATH`, i.e.:
+
+    cmake -D RBDL_USE_SIMPLE_MATH=TRUE ../
+
 Licensing
 =========
 
     
        3. This notice may not be removed or altered from any source
        distribution.
-
-Please note that this library also comes with the Eigen3 library which is
-primarily licensed under the MPL2 license. More information about these
-can be found at:
-	
- * [http://www.mozilla.org/MPL/2.0/](http://www.mozilla.org/MPL/2.0/)
- * [http://www.mozilla.org/MPL/2.0/FAQ.html](http://www.mozilla.org/MPL/2.0/FAQ.html)
- * [http://eigen.tuxfamily.org]([http://eigen.tuxfamily.org])

addons/benchmark/Human36Model.cc

 #include "Human36Model.h"
 
-#include "rbdl.h"
+#include "rbdl/rbdl.h"
 
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;

addons/benchmark/benchmark.cc

 #include <iomanip>
 #include <sstream>
 
-#include "rbdl.h"
+#include "rbdl/rbdl.h"
 #include "model_generator.h"
 #include "Human36Model.h"
 #include "SampleData.h"
 double contacts_benchmark (int sample_count, ContactsBenchmark contacts_benchmark) {
 	// initialize the human model
 	Model *model = new Model();
-	model->Init();
 	generate_human36model(model);
 
 	// initialize the constraint sets
 	Model *model = NULL;
 
 	model = new Model();
-	model->Init();
 	generate_human36model (model);
 	cout << "Human dofs = " << model->dof_count << endl;
 	delete model;
 		cout << "= Forward Dynamics: ABA =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
-			model->Init();
 			model->gravity = Vector3d (0., -9.81, 0.);
 
 			generate_planar_tree (model, depth);
 		cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
-			model->Init();
 			model->gravity = Vector3d (0., -9.81, 0.);
 
 			generate_planar_tree (model, depth);
 		cout << "= Inverse Dynamics: RNEA =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
-			model->Init();
 			model->gravity = Vector3d (0., -9.81, 0.);
 
 			generate_planar_tree (model, depth);
 		cout << "= Joint Space Inertia Matrix: CRBA =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
-			model->Init();
 			model->gravity = Vector3d (0., -9.81, 0.);
 
 			generate_planar_tree (model, depth);

addons/benchmark/model_generator.cc

 #include "model_generator.h"
 
-#include "rbdl.h"
+#include "rbdl/rbdl.h"
 
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;

addons/luamodel/CMakeLists.txt

File contents unchanged.

addons/luamodel/luamodel.cc

-#include "rbdl.h"
+#include "rbdl/rbdl.h"
 #include "luamodel.h"
 
 #include <iostream>

addons/luamodel/rbdl_luamodel_util.cc

-#include "rbdl.h"
-#include "rbdl_utils.h"
+#include "rbdl/rbdl.h"
+#include "rbdl/rbdl_utils.h"
 #include "luamodel.h"
 
 #include <iostream>
 	}
 
 	RigidBodyDynamics::Model model;
-	model.Init();
 
 	if (!RigidBodyDynamics::Addons::LuaModelReadFromFile(filename.c_str(), &model, verbose)) {
 		cerr << "Loading of lua model failed!" << endl;
  *
  * \section recent_changes Recent Changes :
  * <ul>
- * <li> 20. February 2013: removed too specialized RigidBodyDynamics::Body constructor (API version 1.1.0)
+ * <li> 20. February 2013: removed too specialized RigidBodyDynamics::Body constructor (API version 1.1.0)</li>
  * <li> 29. January 2013: added code for \ref api_version_checking. Current is 1.0.0.</li>
  * <li> 18. June 2012: added support of \ref luamodel_introduction</li>
  * <li> 01. June 2012: added support of \ref joint_models_fixed</li>
 - removed Model::Init():
   All initialization is now done in the default constructor in Model(). To
 	be compatible with the new API simply remove any calls to Model::Init().
+- removed Eigen3 sources:
+  Eigen3 is no more included in RBDL instead it uses the Eigen3 library
+  that is installed on the system. If you want to use RBDL with Eigen3
+  you have to install it on your system yourself.
+- inverted sign of contact forces/impulses:
+  ConstraintSet::constraint_force and ConstraintSet::constraint_impulse are
+	now the forces or impulses that are acting on the constrained body by the
+	constraint.
 
 1.0.0 -> 1.1.0
 - removed constructor Body (mass, com, length, gyration_radii)

examples/luamodel/FindRBDL.cmake

 
 SET (RBDL_FOUND FALSE)
 
-FIND_PATH (RBDL_INCLUDE_DIR rbdl.h
+FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h
 	/usr/include
-	/usr/include/rbdl
 	/usr/local/include
-	/usr/local/include/rbdl
 	$ENV{HOME}/local/include
-	$ENV{HOME}/local/include/rbdl
 	$ENV{RBDL_PATH}/src
 	$ENV{RBDL_PATH}/include
-	$ENV{RBDL_PATH}/include/rbdl
 	$ENV{RBDL_INCLUDE_PATH}
 	)
 FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl	PATHS
 IF (RBDL_FOUND)
    IF (NOT RBDL_FIND_QUIETLY)
       MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
+
+			IF (RBDL_LUAMODEL_LIBRARY)
+				MESSAGE(STATUS "Found RBDL Addon Luamodel: ${RBDL_LUAMODEL_LIBRARY}")
+			ENDIF (RBDL_LUAMODEL_LIBRARY)
    ENDIF (NOT RBDL_FIND_QUIETLY)
 ELSE (RBDL_FOUND)
    IF (RBDL_FIND_REQUIRED)
 MARK_AS_ADVANCED (
 	RBDL_INCLUDE_DIR
 	RBDL_LIBRARIES
+	RBDL_LIBRARY	
+	RBDL_LUAMODEL_LIBRARY	
 	)

examples/luamodel/example_luamodel.cc

 
 #include <iostream>
 
-#include <rbdl.h>
+#include <rbdl/rbdl.h>
 
 #ifndef BUILD_ADDON_LUAMODEL
 	#error "Error: RBDL addon BUILD_LUAMODELS not activated."
 #endif
 
-#include <addons/luamodel/luamodel.h>
+#include <rbdl/addons/luamodel/luamodel.h>
 
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;
 	Model* model = NULL;
 
 	model = new Model();
-	model->Init();
 
-	if (!Addons::LuaModelReadFromFile ("./samplemodel.lua", model, false)) {
+	if (!Addons::LuaModelReadFromFile ("./samplemodel.lua", model, true)) {
 		std::cerr << "Error loading model ./samplemodel.lua" << std::endl;
 		abort();
 	}
 
  	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 
+	std::cout << Q.transpose() << std::endl;
 	std::cout << QDDot.transpose() << std::endl;
 
 	delete model;

examples/simple/FindRBDL.cmake

 
 SET (RBDL_FOUND FALSE)
 
-FIND_PATH (RBDL_INCLUDE_DIR rbdl.h
+FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h
 	/usr/include
-	/usr/include/rbdl
 	/usr/local/include
-	/usr/local/include/rbdl
 	$ENV{HOME}/local/include
-	$ENV{HOME}/local/include/rbdl
 	$ENV{RBDL_PATH}/src
 	$ENV{RBDL_PATH}/include
-	$ENV{RBDL_PATH}/include/rbdl
 	$ENV{RBDL_INCLUDE_PATH}
 	)
 FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl	PATHS
 IF (RBDL_FOUND)
    IF (NOT RBDL_FIND_QUIETLY)
       MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
+
+			IF (RBDL_LUAMODEL_LIBRARY)
+				MESSAGE(STATUS "Found RBDL Addon Luamodel: ${RBDL_LUAMODEL_LIBRARY}")
+			ENDIF (RBDL_LUAMODEL_LIBRARY)
    ENDIF (NOT RBDL_FIND_QUIETLY)
 ELSE (RBDL_FOUND)
    IF (RBDL_FIND_REQUIRED)
 MARK_AS_ADVANCED (
 	RBDL_INCLUDE_DIR
 	RBDL_LIBRARIES
+	RBDL_LIBRARY	
+	RBDL_LUAMODEL_LIBRARY	
 	)

examples/simple/example.cc

 
 #include <iostream>
 
-#include <rbdl.h>
+#include <rbdl/rbdl.h>
 
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;
 	Joint joint_a, joint_b, joint_c;
 
 	model = new Model();
-	model->Init();
 
 	model->gravity = Vector3d (0., -9.81, 0.);
 
+/*
+ * 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 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();
+
+			mInertia = Math::Matrix3d (
+					gyration_radii[0], 0., 0.,
+					0., gyration_radii[1], 0.,
+					0., 0., gyration_radii[2]
+					);
+
+			Math::Matrix3d pa (parallel_axis);
+			Math::Matrix3d mcc = mass * com_cross;
+			Math::Matrix3d mccT = mcc.transpose();
+
+			Math::Matrix3d inertia_O = mInertia + pa;
+
+			mSpatialInertia.set (
+					inertia_O(0,0), inertia_O(0,1), inertia_O(0,2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
+					inertia_O(1,0), inertia_O(1,1), inertia_O(1,2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
+					inertia_O(2,0), inertia_O(2,1), inertia_O(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 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 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 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.
+ *
+ * @{
+ */
+
+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 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;
+
+	// 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 */
+/*
+ * 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;
+