Commits

Martin Felis committed e6ad9d5 Merge

merged default into dev

Comments (0)

Files changed (34)

 SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # Options
-OPTION (BUILD_STATIC "Build the statically linked library" ON)
+OPTION (BUILD_STATIC "Build the statically linked library" OFF)
 OPTION (BUILD_TESTS "Build the test executables" OFF)
 OPTION (RBDL_ENABLE_LOGGING "Enable logging (warning: major impact on performance!)" OFF)
 OPTION (RBDL_USE_SIMPLE_MATH "Use slow math instead of the fast Eigen3 library (faster compilation)" OFF)
 SET ( RBDL_SOURCES 
 	src/rbdl_version.cc
 	src/rbdl_mathutils.cc
+	src/rbdl_utils.cc
 	src/Logging.cc
 	src/Joint.cc
 	src/Model.cc
 
 IF (BUILD_ADDON_LUAMODEL)
   ADD_SUBDIRECTORY ( addons/luamodel )
-	TARGET_LINK_LIBRARIES ( rbdl
-		rbdl_luamodel
-		)
 ENDIF (BUILD_ADDON_LUAMODEL)
 
+IF (BUILD_TESTS)
+  ADD_SUBDIRECTORY ( tests )
+ENDIF (BUILD_TESTS)
+
 IF (BUILD_STATIC)
   ADD_LIBRARY ( rbdl-static STATIC ${RBDL_SOURCES} )
   SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib")
   SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES OUTPUT_NAME "rbdl")
 
-	TARGET_LINK_LIBRARIES ( rbdl
-		rbdl_luamodel-static
-		)
+	IF (BUILD_ADDON_LUAMODEL)
+		TARGET_LINK_LIBRARIES ( rbdl-static
+			rbdl_luamodel-static
+			)
+	ENDIF (BUILD_ADDON_LUAMODEL)
 
 	INSTALL (TARGETS rbdl-static
 	  LIBRARY DESTINATION lib
 	)
 ENDIF (BUILD_STATIC)
 
-IF (BUILD_TESTS)
-  ADD_SUBDIRECTORY ( tests )
-ENDIF (BUILD_TESTS)
-
 # Installing
 INSTALL (TARGETS rbdl
 	LIBRARY DESTINATION lib

addons/benchmark/CMakeLists.txt

 # Options
 SET ( BENCHMARK_SOURCES 
 	model_generator.cc
+	Human36Model.cc
 	benchmark.cc
 	)
 

addons/benchmark/Human36Model.cc

+#include "Human36Model.h"
+
+#include "rbdl.h"
+
+using namespace RigidBodyDynamics;
+using namespace RigidBodyDynamics::Math;
+
+enum SegmentName {
+	SegmentPelvis = 0,
+	SegmentThigh,
+	SegmentShank,
+	SegmentFoot,
+	SegmentMiddleTrunk,
+	SegmentUpperTrunk,
+	SegmentUpperArm,
+	SegmentLowerArm,
+	SegmentHand,
+	SegmentHead,
+	SegmentNameLast
+};
+
+double SegmentLengths[SegmentNameLast] = {
+	0.1457,
+	0.4222,
+	0.4403,
+	0.1037,
+	0.2155,
+	0.2421,
+	0.2817,
+	0.2689,
+	0.0862,
+	0.2429
+};
+
+double SegmentMass[SegmentNameLast] = {
+	0.8154,
+	10.3368,
+	3.1609,
+	1.001,
+	16.33,
+	15.96,
+	1.9783,
+	1.1826,
+	0.4453,
+	5.0662
+};
+
+double SegmentCOM[SegmentNameLast][3] = {
+	{ 0., 0.,  0.0891},
+	{ 0., 0., -0.1729},
+	{ 0., 0., -0.1963},
+	{ 0.1254, 0., -0.0516},
+	{ 0., 0.,  0.1185},
+	{ 0., 0.,  0.1195},
+	{ 0., 0., -0.1626},
+	{ 0., 0., -0.1230},
+	{ 0., 0., -0.0680},
+	{ 0., 0.,  1.1214}
+};
+
+double SegmentRadiiOfGyration[SegmentNameLast][3] = {
+	{ 0.0897, 0.0855, 0.0803},
+	{ 0.1389, 0.0629, 0.1389},
+	{ 0.1123, 0.0454, 0.1096},
+	{ 0.0267, 0.0129, 0.0254},
+	{ 0.0970, 0.1009, 0.0825},
+ 	{ 0.1273, 0.1172, 0.0807},
+	{ 0.0803, 0.0758, 0.0445},
+	{ 0.0742, 0.0713, 0.0325},
+	{ 0.0541, 0.0442, 0.0346},
+	{ 0.0736, 0.0634, 0.0765}
+};
+
+Body create_body (SegmentName segment) {
+	return Body (
+			SegmentMass[segment],
+			Vector3d (
+				SegmentCOM[segment][0],
+				SegmentCOM[segment][1],
+				SegmentCOM[segment][2]
+				),
+			Vector3d (
+				SegmentRadiiOfGyration[segment][0],
+				SegmentRadiiOfGyration[segment][1],
+				SegmentRadiiOfGyration[segment][2]
+				)
+			);
+}
+
+void generate_human36model (RigidBodyDynamics::Model *model) {
+	Body pelvis_body = create_body (SegmentPelvis);
+	Body thigh_body = create_body (SegmentThigh);
+	Body shank_body = create_body (SegmentShank);
+	Body foot_body = create_body (SegmentFoot);
+	Body middle_trunk_body = create_body (SegmentMiddleTrunk);
+	Body upper_trunk_body = create_body (SegmentUpperTrunk);
+	Body upperarm_body = create_body (SegmentUpperArm);
+	Body lowerarm_body = create_body (SegmentLowerArm);
+	Body hand_body = create_body (SegmentHand);
+	Body head_body = create_body (SegmentHead);
+
+	Joint free_flyer (
+		SpatialVector (0., 0., 0., 1., 0., 0.),
+		SpatialVector (0., 0., 0., 0., 1., 0.),
+		SpatialVector (0., 0., 0., 0., 0., 1.),
+		SpatialVector (0., 1., 0., 0., 0., 0.),
+		SpatialVector (0., 0., 1., 0., 0., 0.),
+		SpatialVector (1., 0., 0., 0., 0., 0.)
+		);
+
+	Joint rot_yzx (
+			SpatialVector (0., 1., 0., 0., 0., 0.),
+			SpatialVector (0., 0., 1., 0., 0., 0.),
+			SpatialVector (1., 0., 0., 0., 0., 0.)
+			);
+
+	Joint rot_yz (
+			SpatialVector (0., 1., 0., 0., 0., 0.),
+			SpatialVector (0., 0., 1., 0., 0., 0.)
+			);
+
+	Joint rot_y (
+			SpatialVector (0., 1., 0., 0., 0., 0.)
+			);
+
+	Joint fixed (JointTypeFixed);
+
+	model->gravity = Vector3d (0., 0., -9.81);
+
+	unsigned int pelvis_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis");
+
+	// right leg
+	model->AddBody (pelvis_id, Xtrans(Vector3d(0., -0.0872, 0.)), rot_yzx, thigh_body, "thigh_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");
+
+	// left leg
+	model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0.0872, 0.)), rot_yzx, thigh_body, "thigh_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");
+
+	// trunk
+	model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yzx, middle_trunk_body, "middletrunk");
+	unsigned int uppertrunk_id = model->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");
+
+	// right arm
+	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "upperarm_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");
+
+	// left arm
+	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0.,  0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "upperarm_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");
+
+	// head	
+	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "head");
+}

addons/benchmark/Human36Model.h

+#ifndef _HUMAN36MODEL_H
+#define _HUMAN36MODEL_H
+
+namespace RigidBodyDynamics {
+	class Model;
+}
+
+void generate_human36model (RigidBodyDynamics::Model *model);
+
+/* _HUMAN36MODEL_H */
+#endif

addons/benchmark/benchmark.cc

 
 #include "rbdl.h"
 #include "model_generator.h"
+#include "Human36Model.h"
 #include "SampleData.h"
 #include "Timer.h"
 
 
 int benchmark_sample_count = 1000;
 int benchmark_model_max_depth = 5;
+
 bool benchmark_run_fd_aba = true;
 bool benchmark_run_fd_lagrangian = true;
 bool benchmark_run_id_rnea = true;
 bool benchmark_run_crba = true;
+bool benchmark_run_contacts = false;
+
+enum ContactsBenchmark {
+	ContactsBenchmarkLagrangian = 0,
+	ContactsBenchmarkKokkevis
+};
 
 double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) {
 	SampleData sample_data;
 				sample_data.q_data[i],
 				sample_data.qdot_data[i],
 				sample_data.tau_data[i],
-				sample_data.qddot_data[i],
-				LinearSolverPartialPivLU);
+				sample_data.qddot_data[i]
+);
 	}
 
 	double duration = timer_stop (&tinfo);
 	SampleData sample_data;
 	sample_data.fill_random_data(model->dof_count, sample_count);
 
+	Math::MatrixNd H = Math::MatrixNd(model->dof_count, model->dof_count);
+
 	TimerInfo tinfo;
 	timer_start (&tinfo);
 
-	Math::MatrixNd H = Math::MatrixNd(model->dof_count, model->dof_count);
-
 	for (int i = 0; i < sample_count; i++) {
 		CompositeRigidBodyAlgorithm (*model, sample_data.q_data[i], H, true);
 	}
 	return duration;
 }
 
+double run_contacts_lagrangian_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) {
+	SampleData sample_data;
+	sample_data.fill_random_data(model->dof_count, sample_count);
+
+	TimerInfo tinfo;
+	timer_start (&tinfo);
+
+	for (int i = 0; i < sample_count; i++) {
+		ForwardDynamicsContactsLagrangian (*model, sample_data.q_data[i], sample_data.qdot_data[i], sample_data.tau_data[i], *constraint_set, sample_data.qddot_data[i]); 
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	return duration;
+}
+
+double run_contacts_kokkevis_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) {
+	SampleData sample_data;
+	sample_data.fill_random_data(model->dof_count, sample_count);
+
+	TimerInfo tinfo;
+	timer_start (&tinfo);
+
+	for (int i = 0; i < sample_count; i++) {
+		ForwardDynamicsContacts (*model, sample_data.q_data[i], sample_data.qdot_data[i], sample_data.tau_data[i], *constraint_set, sample_data.qddot_data[i]); 
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	return duration;
+}
+
+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
+	unsigned int foot_r = model->GetBodyId ("foot_r");
+	unsigned int foot_l = model->GetBodyId ("foot_l");
+	unsigned int hand_r = model->GetBodyId ("hand_r");
+	unsigned int hand_l = model->GetBodyId ("hand_l");
+
+	ConstraintSet one_body_one_constraint;
+	ConstraintSet two_bodies_one_constraint;
+	ConstraintSet four_bodies_one_constraint;
+
+	ConstraintSet one_body_four_constraints;
+	ConstraintSet two_bodies_four_constraints;
+	ConstraintSet four_bodies_four_constraints;
+
+	// one_body_one
+	one_body_one_constraint.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	one_body_one_constraint.Bind (*model);
+
+	// two_bodies_one
+	two_bodies_one_constraint.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_one_constraint.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_one_constraint.Bind (*model);
+
+	// four_bodies_one
+	four_bodies_one_constraint.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.Bind (*model);
+
+	// one_body_four
+	one_body_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	one_body_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	one_body_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	one_body_four_constraints.AddConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	one_body_four_constraints.Bind (*model);	
+
+	// two_bodies_four
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+
+	two_bodies_four_constraints.Bind (*model);
+
+	// four_bodies_four
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+
+	four_bodies_four_constraints.Bind (*model);
+
+	cout << "= #DOF: " << setw(3) << model->dof_count << endl;
+	cout << "= #samples: " << sample_count << endl;
+	cout << "= No constraints (Articulated Body Algorithm):" << endl;
+	run_forward_dynamics_ABA_benchmark (model, sample_count);
+	cout << "= Constraints:" << endl;
+	double duration;
+
+	// one body one
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &one_body_one_constraint, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &one_body_one_constraint, sample_count);
+	}
+
+	cout << "ConstraintSet: 1 Body 1 Constraint   : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	// two_bodies_one
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &two_bodies_one_constraint, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &two_bodies_one_constraint, sample_count);
+	}
+
+	cout << "ConstraintSet: 2 Bodies 1 Constraint : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+
+	// four_bodies_one
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &four_bodies_one_constraint, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &four_bodies_one_constraint, sample_count);
+	}
+
+	cout << "ConstraintSet: 4 Bodies 1 Constraint : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	// one_body_four
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &one_body_four_constraints, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &one_body_four_constraints, sample_count);
+	}
+
+	cout << "ConstraintSet: 1 Body 4 Constraints  : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	// two_bodies_four
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &two_bodies_four_constraints, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &two_bodies_four_constraints, sample_count);
+	}
+
+	cout << "ConstraintSet: 2 Bodies 4 Constraints: "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+
+	// four_bodies_four
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &four_bodies_four_constraints, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &four_bodies_four_constraints, sample_count);
+	}
+
+	cout << "ConstraintSet: 4 Bodies 4 Constraints: "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	delete model;
+
+	return duration;
+}
+
 void print_usage () {
 	cout << "Usage: benchmark [--count|-c <sample_count>] [--depth|-d <depth>]" << endl;
 	cout << "Simple benchmark tool for the Rigid Body Dynamics Library." << endl;
 	cout << "  --no-crba                   : disables benchmark for joint space inertia" << endl;
 	cout << "                                matrix computation using the composite rigid." << endl;
 	cout << "                                body algorithm." << endl;
+	cout << "  --only-contacts | -C        : only runs contact model benchmarks." << endl;
+	cout << "  --help | -h                 : prints this help." << endl;
+}
+
+void disable_all_benchmarks () {
+	benchmark_run_fd_aba = false;
+	benchmark_run_fd_lagrangian = false;
+	benchmark_run_id_rnea = false;
+	benchmark_run_crba = false;
+	benchmark_run_contacts = false;
 }
 
 void parse_args (int argc, char* argv[]) {
 			benchmark_run_id_rnea = false;
 		} else if (arg == "--no-crba" ) {
 			benchmark_run_crba = false;
+		} else if (arg == "--only-contacts" || arg == "-C") {
+			disable_all_benchmarks();
+			benchmark_run_contacts = true;
 		} else {
 			print_usage();
 			cerr << "Invalid argument '" << arg << "'." << endl;
 
 	Model *model = NULL;
 
+	model = new Model();
+	model->Init();
+	generate_human36model (model);
+	cout << "Human dofs = " << model->dof_count << endl;
+	delete model;
+
 	RigidBodyDynamics::rbdl_print_version();
 	cout << endl;
 
 		cout << endl;
 	}
 
+	if (benchmark_run_contacts) {
+		cout << "= Contacts: ForwardDynamicsContactsLagrangian" << endl;
+		contacts_benchmark (benchmark_sample_count, ContactsBenchmarkLagrangian);
+
+		cout << "= Contacts: ForwardDynamicsContactsKokkevis" << endl;
+		contacts_benchmark (benchmark_sample_count, ContactsBenchmarkKokkevis);
+	}
+
+
+
 	return 0;
 }

addons/luamodel/luamodel.cc

 	}
 	body_name = get_string (L, path + ".name");
 
-	if (!value_exists (L, path + ".parent_frame") != 0.) {
-		cerr << "Error: could not find required value '" << path << ".parent_frame' for body '" << body_name << "'." << endl;
+	if (!value_exists (L, path + ".parent") != 0.) {
+		cerr << "Error: could not find required value '" << path << ".parent' for body '" << body_name << "'." << endl;
 		return false;
 	}
 
-	string parent_frame = get_string (L, path + ".parent_frame");
+	string parent_frame = get_string (L, path + ".parent");
 
 	StringIntMap::iterator parent_iter = body_table_id_map.find (parent_frame);
 	if (parent_iter == body_table_id_map.end()) {
 
 	if (verbose) {
 		cout << "frame name = " << body_name << endl;
-		cout << "  parent_frame = "  << parent_frame << endl;
+		cout << "  parent = "  << parent_frame << endl;
 		cout << "  parent_id = " << parent_id << endl;
 	}
 

addons/luamodel/luamodel.h

    * \par name (required, type: string):
    *     Name of the body that is being added. This name must be unique.
    * 
-   * \par parent_frame (required, type: string):
+   * \par parent (required, type: string):
    *     If the value is "ROOT" the parent frame of this body is assumed to be
    *     the base coordinate system, otherwise it must be the exact same string
    *     as was used for the "name"-field of the parent frame.

addons/luamodel/rbdl_luamodel_util.cc

 #include "rbdl.h"
+#include "rbdl_utils.h"
 #include "luamodel.h"
 
 #include <iostream>
 
 using namespace RigidBodyDynamics::Math;
 
-string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) {
-	if (model.mBodies[body_id].mMass == 0.) {
-		// this seems to be a virtual body that was added by a multi dof joint
-		unsigned int child_index = 0;
-
-		// if there is not a unique child we do not know what to do...
-		if (model.mu[body_id].size() != 1)
-			return "";
-
-		unsigned int child_id = model.mu[body_id][0];
-
-		return get_body_name (model, model.mu[body_id][0]);
-	}
-
-	return model.GetBodyName(body_id);
-}
-
-string get_dof_name (const SpatialVector &joint_dof) {
-	if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) 
-		return "RX";
-	else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.))
-		return "RY";
-	else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.))
-		return "RZ";
-	else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.))
-		return "TX";
-	else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.))
-		return "TY";
-	else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.))
-		return "TZ";
-
-	ostringstream dof_stream(ostringstream::out);
-	dof_stream << "custom (" << joint_dof.transpose() << ")";
-	return dof_stream.str();
-}
-
-void print_dof_overview (const RigidBodyDynamics::Model &model) {
-	cout << "Degree of freedom overview:" << endl;
-	for (unsigned int i = 1; i < model.mBodies.size(); i++) {
-		cout << setfill(' ') << setw(3) << i - 1 << ": " << get_body_name (model, i) << "_" << get_dof_name (model.S[i]) << endl;
-	}
-}
-
-void print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) {
-	for (int j = 0; j < indent; j++)
-		cout << "  ";
-
-	cout << get_body_name (model, body_index);
-
-	if (body_index == 0) {
-		cout << endl;
-		print_hierarchy (model, 1, 1);
-		return;
-	} 
-
-	// print the dofs
-	cout << " [ ";
-
-	while (model.mBodies[body_index].mMass == 0.) {
-		if (model.mu[body_index].size() != 1) {
-			cerr << endl << "Error: Cannot determine multi-dof joint as massless body with id " << body_index << " has more than 1 child." << endl;
-			abort();
-		}
-
-		cout << get_dof_name(model.S[body_index]) << ", ";
-
-		body_index = model.mu[body_index][0];
-	}
-	cout << get_dof_name(model.S[body_index]);
-
-	cout << " ]" << endl;
-
-	// print fixed children
-	for (unsigned int fbody_index = 0; fbody_index < model.mFixedBodies.size(); fbody_index++) {
-		if (model.mFixedBodies[fbody_index].mMovableParent == body_index) {
-			for (int j = 0; j < indent; j++)
-				cout << "  ";
-
-			cout << model.GetBodyName(model.fixed_body_discriminator + fbody_index) << " [fixed]" << endl;
-		}
-	}
-
-	unsigned int child_index = 0;
-	for (child_index = 0; child_index < model.mu[body_index].size(); child_index++) {
-		print_hierarchy (model, model.mu[body_index][child_index], indent + 1);
-	}
-}
-
 void usage (const char* argv_0) {
 	cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <model.lua>" << endl;
 	cerr << "  -v | --verbose            enable additional output" << endl;
 
 	cout << "Model loading successful!" << endl;
 
-	if (dof_overview)
-		print_dof_overview(model);
-	if (model_hierarchy)
-		print_hierarchy(model);
+	if (dof_overview) {
+		cout << "Degree of freedom overview:" << endl;
+		cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model);
+	}
+
+	if (model_hierarchy) {
+		cout << "Model Hierarchy:" << endl;
+		cout << RigidBodyDynamics::Utils::GetModelHierarchy (model);
+	}
 
 	return 0;
 }

addons/luamodel/samplemodel.lua

 	frames = {
 		{
 			name = "pelvis",
-			parent_frame = "BASE",
+			parent = "ROOT",
 			body = bodies.pelvis,
 			joint = joints.freeflyer,
 		},
 		{
 			name = "thigh_right",
-			parent_frame = "pelvis",
+			parent = "pelvis",
 			body = bodies.thigh_right,
 			joint = joints.spherical_zyx,
 		},
 		{
+			name = "shank_right",
+			parent = "thigh_right",
+			body = bodies.thigh_right,
+			joint = joints.rotational_y
+		},
+		{
 			name = "foot_right",
-			parent_frame = "thigh_right",
+			parent = "shank_right",
 			body = bodies.thigh_right,
 			joint = joints.fixed
 		},
 		{
-			name = "foot_left",
-			parent_frame = "thigh_right",
-			body = bodies.thigh_right,
-		},
-		{
 			name = "thigh_left",
-			parent_frame = "pelvis",
+			parent = "pelvis",
 			body = bodies.thigh_left,
 			joint = joints.spherical_zyx
 		},
 		{
 			name = "shank_left",
-			parent_frame = "thigh_left",
+			parent = "thigh_left",
 			body = bodies.thigh_left,
 			joint = joints.rotational_y
 		},
 		{
 			name = "foot_left",
-			parent_frame = "thigh_left",
+			parent = "shank_left",
 			body = bodies.thigh_left,
 			joint = joints.fixed
 		},

addons/urdfreader/CMake/FindURDF.cmake

+# Try to find shared object of the urdf library
+
+SET (URDF_FOUND FALSE)
+
+FIND_PROGRAM(ROSPACK rospack)
+IF(NOT ROSPACK)
+	MESSAGE(FATAL_ERROR "failed to find the rospack binary. Is ROS installed?")
+ENDIF()
+
+EXEC_PROGRAM("${ROSPACK} find ${PKG}" OUTPUT_VARIABLE ${PKG}_ROS_PREFIX)
+IF(NOT ${PKG}_ROS_PREFIX)
+  MESSAGE(FATAL_ERROR "Failed to detect ${PKG}.")
+ENDIF()
+
+EXEC_PROGRAM("${ROSPACK} find urdf" OUTPUT_VARIABLE URDF_FULL_PATH)
+
+FIND_PATH (URDF_INCLUDE_DIR urdf/model.h ${URDF_FULL_PATH}/include)
+FIND_LIBRARY (URDF_LIBRARY urdf ${URDF_FULL_PATH}/lib)
+
+IF (URDF_INCLUDE_DIR AND URDF_LIBRARY)
+	SET (URDF_FOUND TRUE)
+ENDIF (URDF_INCLUDE_DIR AND URDF_LIBRARY)
+
+IF (URDF_FOUND)
+   IF (NOT URDF_FIND_QUIETLY)
+      MESSAGE(STATUS "Found URDF: ${URDF_LIBRARY}")
+   ENDIF (NOT URDF_FIND_QUIETLY)
+ELSE (URDF_FOUND)
+   IF (URDF_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find URDF")
+   ENDIF (URDF_FIND_REQUIRED)
+ENDIF (URDF_FOUND)
+
+MARK_AS_ADVANCED (
+	URDF_INCLUDE_DIR
+	URDF_LIBRARY
+	)

addons/urdfreader/CMake/ros.cmake

   ENDIF()
 
   SET(${PREFIX}_FOUND 1)
-  EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=cflags ${PKG}"
+  EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=cflags -q ${PKG}"
     OUTPUT_VARIABLE "${PREFIX}_CFLAGS")
-  EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=lflags ${PKG}"
+  EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=lflags -q ${PKG}"
     OUTPUT_VARIABLE "${PREFIX}_LIBS")
 
   # Add flags to package pkg-config file.
   SET(CFLAGS "${CFLAGS} ${${PREFIX}_CFLAGS}")
   SET(LDFLAGS "${LDFLAGS} ${${PREFIX}_LIBS}")
 
+	# MESSAGE (STATUS "Linkerflags for ${TARGET}: ${LDFLAGS}")
+
+	# Explicitly link against the shared object file
+	EXEC_PROGRAM("${ROSPACK} export find ${PKG}"
+		OUTPUT_VARIABLE "${PKG_FULL_PATH}")
+
+	#	SET (LDFLAGS "${LDFLAGS} ${PKG_FULL_PATH}/lib/lib${PKG}.so")
+
   # Update the flags.
   SET_TARGET_PROPERTIES(${TARGET}
     PROPERTIES COMPILE_FLAGS "${CFLAGS}" LINK_FLAGS "${LDFLAGS}")

addons/urdfreader/CMakeLists.txt

 INCLUDE ( CMake/ros.cmake)
 INCLUDE ( CMake/pkg-config.cmake)
 
-
 ADD_ROSPACK_DEPENDENCY ("urdf")
 ADD_ROSPACK_DEPENDENCY ("urdf_interface")
 ADD_ROSPACK_DEPENDENCY ("urdf_parser")
 
+# We additionally link against urdf explicitly
+FIND_PACKAGE (URDF REQUIRED)
+INCLUDE_DIRECTORIES (${URDF_INCLUDE_DIR})
+
 INCLUDE_DIRECTORIES ( 
 	${CMAKE_CURRENT_BINARY_DIR}/include/rbdl
-	${CMAKE_CURRENT_SOURCE_DIR}/tinyxml/
 )
 
 SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
-		LINKER_LANGUAGE CXX
-	)
+	LINKER_LANGUAGE CXX
+)
 
 # Perform the proper linking
 SET (CMAKE_SKIP_BUILD_RPATH FALSE)
 
 ADD_LIBRARY ( rbdl_urdfreader SHARED ${URDFREADER_SOURCES} )
 
+ADD_EXECUTABLE (rbdl_urdfreader_util rbdl_urdfreader_util.cc)
+
 ROSPACK_USE_DEPENDENCY (rbdl_urdfreader urdf)
 ROSPACK_USE_DEPENDENCY (rbdl_urdfreader urdf_interface)
 
-ADD_DEFINITIONS (-DTIXML_USE_STL)
+ROSPACK_USE_DEPENDENCY (rbdl_urdfreader_util urdf)
+ROSPACK_USE_DEPENDENCY (rbdl_urdfreader_util urdf_interface)
 
 IF (BUILD_STATIC)
-  ADD_LIBRARY ( rbdl_urdfreader-static STATIC ${RBDL_SOURCES} )
+	ADD_LIBRARY ( rbdl_urdfreader-static STATIC ${URDFREADER_SOURCES} )
+
+	ROSPACK_USE_DEPENDENCY (rbdl_urdfreader-static urdf)
+	ROSPACK_USE_DEPENDENCY (rbdl_urdfreader-static urdf_interface)
+
   SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES PREFIX "lib")
   SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES OUTPUT_NAME "rbdl_urdfreader")
 
 	)
 ENDIF (BUILD_STATIC)
 
-ADD_EXECUTABLE (rbdl_urdf_test rbdl_urdf_test.cc)
-
 TARGET_LINK_LIBRARIES (rbdl_urdfreader
 	rbdl
+	${URDF_LIBRARY}
 	)
 
-TARGET_LINK_LIBRARIES (rbdl_urdf_test
+TARGET_LINK_LIBRARIES (rbdl_urdfreader_util
 	rbdl_urdfreader
 	)
 

addons/urdfreader/README

-urdfreader - load models from (URDF Unified Robot Description Format) files
-Copyright (c) 2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
-
-REQUIREMENTS
-
-This addon depends on ROS, the Robot Operating System (http://www.ros.org)
-and uses its urdf_parser and urdf_interface to load and access the model
-data in the files. You therefore need to have ROS and the package
-"robot-model" installed on your system to be able to compile this addon.
-
-See http://www.ros.org/wiki/ROS/Installation for more details on how to
-install ROS.
-
-LICENSING
-
-This code is published under the zlib license, however some parts of the
-CMake scripts are taken from other projects and are licensed under
-different terms.
-
-Full license text:
-
--------
-urdfreader - load models from URDF (Unified Robot Description Format) files
-Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
-
-This software is provided 'as-is', without any express or implied
-warranty. In no event will the authors be held liable for any damages
-arising from the use of this software.
-
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it
-freely, subject to the following restrictions:
-
-   1. The origin of this software must not be misrepresented; you must not
-   claim that you wrote the original software. If you use this software
-   in a product, an acknowledgment in the product documentation would be
-   appreciated but is not required.
-
-   2. Altered source versions must be plainly marked as such, and must not be
-   misrepresented as being the original software.
-
-   3. This notice may not be removed or altered from any source
-   distribution.

addons/urdfreader/README.md

+urdfreader - load models from (URDF Unified Robot Description Format) files
+Copyright (c) 2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+
+Requirements
+============
+
+This addon depends on ROS, the Robot Operating System (http://www.ros.org)
+and uses its urdf_parser and urdf_interface to load and access the model
+data in the files. You therefore need to have ROS and the package
+"robot-model" installed on your system to be able to compile this addon. So
+far only the ROS release "Fuerte" can be used with this code.
+
+See http://www.ros.org/wiki/ROS/Installation for more details on how to
+install ROS.
+
+Warning
+=======
+
+This code is not properly tested as I do not have a proper urdf robot
+model. If anyone has one and also some reference values that should come
+out for the dynamics computations, please let me know.
+
+Licensing
+=========
+
+This code is published under the zlib license, however some parts of the
+CMake scripts are taken from other projects and are licensed under
+different terms.
+
+Full license text:
+
+-------
+urdfreader - load models from URDF (Unified Robot Description Format) files
+Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+   1. The origin of this software must not be misrepresented; you must not
+   claim that you wrote the original software. If you use this software
+   in a product, an acknowledgment in the product documentation would be
+   appreciated but is not required.
+
+   2. Altered source versions must be plainly marked as such, and must not be
+   misrepresented as being the original software.
+
+   3. This notice may not be removed or altered from any source
+   distribution.

addons/urdfreader/rbdl_urdf_test.cc

-#include "rbdl.h"
-#include "rbdl_urdfreader.h"
-
-#include <iostream>
-
-using namespace std;
-
-int main (int argc, char *argv[]) {
-	if (argc != 2) {
-		cerr << "Usage: " << argv[0] << " <robot.urdf>" << endl;
-		return -1;
-	}
-
-	RigidBodyDynamics::Model model;
-	if (!RigidBodyDynamics::Addons::read_urdf_model(argv[1], &model, true)) {
-		cerr << "Loading of urdf model failed!" << endl;
-		return -1;
-	}
-
-	return 0;
-}

addons/urdfreader/rbdl_urdfreader.cc

 
 namespace Addons {
 
+using namespace Math;
+
 typedef boost::shared_ptr<urdf::Link> LinkPtr;
 typedef boost::shared_ptr<urdf::Joint> JointPtr;
 
 typedef map<string, LinkPtr > URDFLinkMap;
 typedef map<string, JointPtr > URDFJointMap;
 
-bool construct_model (Model* rbdl_model, urdf::Model *urdf_model) {
+bool construct_model (Model* rbdl_model, urdf::Model *urdf_model, bool verbose) {
 	boost::shared_ptr<urdf::Link> urdf_root_link;
 
 	URDFLinkMap link_map;
 	URDFJointMap joint_map;
 	joint_map = urdf_model->joints_;
 
+	vector<string> joint_names;
+
 	stack<LinkPtr > link_stack;
 	stack<int> joint_index_stack;
 
 			link_stack.push (link_map[cur_joint->child_link_name]);
 			joint_index_stack.push(0);
 
-			for (int i = 1; i < joint_index_stack.size() - 1; i++) {
-				cout << "  ";
+			if (verbose) {
+				for (int i = 1; i < joint_index_stack.size() - 1; i++) {
+					cout << "  ";
+				}
+				cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << " type = " << cur_joint->type << endl;
 			}
-			cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << " type = " << cur_joint->type << endl;
+
+			joint_names.push_back(cur_joint->name);
 		} else {
 			link_stack.pop();
 			joint_index_stack.pop();
 		}
 	}
 
-	/*
-
 	unsigned int j;
 	for (j = 0; j < joint_names.size(); j++) {
-		URDFJoint urdf_joint = joints[joint_names[j]];
-		URDFLink urdf_parent = links[urdf_joint.parent];
-		URDFLink urdf_child = links[urdf_joint.child];
+		JointPtr urdf_joint = joint_map[joint_names[j]];
+		LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name];
+		LinkPtr urdf_child = link_map[urdf_joint->child_link_name];
 
 		// determine where to add the current joint and child body
 		unsigned int rbdl_parent_id = 0;
 		
-		if (urdf_parent.name != "base_joint" && model->mBodies.size() != 1)
-			rbdl_parent_id = model->GetBodyId (urdf_parent.name.c_str());
+		if (urdf_parent->name != "base_joint" && rbdl_model->mBodies.size() != 1)
+			rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str());
 
 //		cout << "joint: " << urdf_joint.name << "\tparent = " << urdf_parent.name << " child = " << urdf_child.name << " parent_id = " << rbdl_parent_id << endl;
 
 		// create the joint
 		Joint rbdl_joint;
-		if (urdf_joint.type == URDFJointTypeRevolute || urdf_joint.type == URDFJointTypeContinuous) {
-			rbdl_joint = Joint (SpatialVector (urdf_joint.axis[0], urdf_joint.axis[1], urdf_joint.axis[2], 0., 0., 0.));
-		} else if (urdf_joint.type == URDFJointTypePrismatic) {
-			rbdl_joint = Joint (SpatialVector (0., 0., 0., urdf_joint.axis[0], urdf_joint.axis[1], urdf_joint.axis[2]));
-		} else if (urdf_joint.type == URDFJointTypeFixed) {
-			// todo: add fixed joint support to rbdl
-			cerr << "Error while processing joint '" << urdf_joint.name << "': fixed joints not yet supported!" << endl;
-			return false;
-		} else if (urdf_joint.type == URDFJointTypeFloating) {
+		if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS) {
+			rbdl_joint = Joint (SpatialVector (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z, 0., 0., 0.));
+		} else if (urdf_joint->type == urdf::Joint::PRISMATIC) {
+			rbdl_joint = Joint (SpatialVector (0., 0., 0., urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
+		} else if (urdf_joint->type == urdf::Joint::FIXED) {
+			rbdl_joint = Joint (JointTypeFixed);
+		} else if (urdf_joint->type == urdf::Joint::FLOATING) {
 			// todo: what order of DoF should be used?
 			rbdl_joint = Joint (
 					SpatialVector (0., 0., 0., 1., 0., 0.),
 					SpatialVector (1., 0., 0., 0., 0., 0.),
 					SpatialVector (0., 1., 0., 0., 0., 0.),
 					SpatialVector (0., 0., 1., 0., 0., 0.));
-		} else if (urdf_joint.type == URDFJointTypePlanar) {
+		} else if (urdf_joint->type == urdf::Joint::PLANAR) {
 			// todo: which two directions should be used that are perpendicular
 			// to the specified axis?
-			cerr << "Error while processing joint '" << urdf_joint.name << "': planar joints not yet supported!" << endl;
+			cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl;
 			return false;
 		}
 
 		// compute the joint transformation
-		SpatialTransform rbdl_joint_frame = Xrot (urdf_joint.origin_rot_r, Vector3d (1., 0., 0.))
-				* Xrot (urdf_joint.origin_rot_p, Vector3d (0., 1., 0.))
-				* Xrot (urdf_joint.origin_rot_y, Vector3d (0., 0., 1.))
+		Vector3d joint_rpy;
+		Vector3d joint_translation;
+		urdf_joint->parent_to_joint_origin_transform.rotation.getRPY (joint_rpy[0], joint_rpy[1], joint_rpy[2]);
+		joint_translation.set (
+				urdf_joint->parent_to_joint_origin_transform.position.x,
+				urdf_joint->parent_to_joint_origin_transform.position.y,
+				urdf_joint->parent_to_joint_origin_transform.position.z
+				);
+		SpatialTransform rbdl_joint_frame = 
+					Xrot (joint_rpy[0], Vector3d (1., 0., 0.))
+				* Xrot (joint_rpy[1], Vector3d (0., 1., 0.))
+				* Xrot (joint_rpy[2], Vector3d (0., 0., 1.))
 				* Xtrans (Vector3d (
-							urdf_joint.origin_x,
-							urdf_joint.origin_y,
-							urdf_joint.origin_z
+							joint_translation
 							));
 
 		// assemble the body
-		Vector3d body_com (urdf_child.origin_x, urdf_child.origin_y, urdf_child.origin_z);
-		Vector3d body_rpy (urdf_child.origin_rot_r, urdf_child.origin_rot_p, urdf_child.origin_rot_y);
+		Vector3d link_inertial_position;
+		Vector3d link_inertial_rpy;
+		Matrix3d link_inertial_inertia = Matrix3d::Zero();
+		double link_inertial_mass;
 
-		if (body_rpy != Vector3d (0., 0., 0.)) {
-			cerr << "Error while processing joint '" << urdf_joint.name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl;
-			return false;
+		// but only if we actually have inertial data
+		if (urdf_child->inertial) {
+			double link_inertial_mass = urdf_child->inertial->mass;
+
+			link_inertial_position.set (
+					urdf_child->inertial->origin.position.x,
+					urdf_child->inertial->origin.position.y,
+					urdf_child->inertial->origin.position.z
+					);
+			urdf_child->inertial->origin.rotation.getRPY (link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]);
+
+			link_inertial_inertia(0,0) = urdf_child->inertial->ixx;
+			link_inertial_inertia(0,1) = urdf_child->inertial->ixy;
+			link_inertial_inertia(0,2) = urdf_child->inertial->ixz;
+
+			link_inertial_inertia(1,0) = urdf_child->inertial->ixy;
+			link_inertial_inertia(1,1) = urdf_child->inertial->iyy;
+			link_inertial_inertia(1,2) = urdf_child->inertial->iyz;
+
+			link_inertial_inertia(2,0) = urdf_child->inertial->ixz;
+			link_inertial_inertia(2,1) = urdf_child->inertial->iyz;
+			link_inertial_inertia(2,2) = urdf_child->inertial->izz;
+
+			if (link_inertial_rpy != Vector3d (0., 0., 0.)) {
+				cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl;
+				return false;
+			}
 		}
 
-		Matrix3d inertia (
-			urdf_child.ixx, urdf_child.ixy, urdf_child.ixz,
-			urdf_child.ixy, urdf_child.iyy, urdf_child.iyz,
-			urdf_child.ixz, urdf_child.iyz, urdf_child.izz
-			);
-		Body rbdl_body = Body (urdf_child.mass, body_com, inertia);
+		Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia);
 
-		model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child.name);
+		if (verbose) {
+			cout << "+ Adding Body " << endl;
+			cout << "  parent_id  : " << rbdl_parent_id << endl;
+			cout << "  joint_frame: " << rbdl_joint_frame << endl;
+			cout << "  joint dofs : " << rbdl_joint.mDoFCount << endl;
+			for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
+				cout << "    " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl;
+			}
+			cout << "  body inertia: " << endl << rbdl_body.mSpatialInertia << endl;
+			cout << "  body_name  : " << urdf_child->name << endl;
+		}
+
+		rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name);
 	}
-	*/
 
-	return false;
+	return true;
 }
 
 bool read_urdf_model (const char* filename, Model* model, bool verbose) {
 	assert (model);
 
-	model->Init();
+	urdf::Model urdf_model;
 
-	urdf::Model urdf_model;
+	cerr << "Warning: this code (RigidBodyDynamics::Addons::" << __func__ << "()) is not properly tested as" << endl;
+	cerr << "         I do not have a proper urdf model that I can use to validate the model loading." << endl;
+	cerr << "         Please use with care." << endl;
 
 	bool urdf_result = urdf_model.initFile (filename);
 	if (!urdf_result) {
 		cerr << "Error opening urdf file" << endl;
 	}
 
-	if (!construct_model (model, &urdf_model)) {
+	if (!construct_model (model, &urdf_model, verbose)) {
 		cerr << "Error constructing model from urdf file." << endl;
 		return false;
 	}
 
+	model->gravity.set (0., 0., -9.81);
+
 	return true;
 }
 

addons/urdfreader/rbdl_urdfreader_util.cc

+#include "rbdl.h"
+#include "rbdl_utils.h"
+#include "rbdl_urdfreader.h"
+
+#include <iostream>
+
+using namespace std;
+
+bool verbose = false;
+string filename = "";
+
+void usage (const char* argv_0) {
+	cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <robot.urdf>" << endl;
+	cerr << "  -v | --verbose            enable additional output" << endl;
+	cerr << "  -d | --dof-overview       print an overview of the degress of freedom" << endl;
+	cerr << "  -m | --model-hierarchy    print the hierarchy of the model" << endl;
+	cerr << "  -h | --help               print this help" << endl;
+	exit (1);
+}
+
+
+int main (int argc, char *argv[]) {
+	if (argc < 2 || argc > 4) {
+		usage(argv[0]);
+	}
+
+	bool verbose = false;
+	bool dof_overview = false;
+	bool model_hierarchy = false;
+
+	string filename = argv[1];
+
+	for (int i = 1; i < argc; i++) {
+		if (string(argv[i]) == "-v" || string (argv[i]) == "--verbose")
+			verbose = true;
+		else if (string(argv[i]) == "-d" || string (argv[i]) == "--dof-overview")
+			dof_overview = true;
+		else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy")
+			model_hierarchy = true;
+		else if (string(argv[i]) == "-h" || string (argv[i]) == "--help")
+			usage(argv[0]);
+		else
+			filename = argv[i];
+	}
+
+	RigidBodyDynamics::Model model;
+	model.Init();
+
+	if (!RigidBodyDynamics::Addons::read_urdf_model(filename.c_str(), &model, verbose)) {
+		cerr << "Loading of urdf model failed!" << endl;
+		return -1;
+	}
+
+	cout << "Model loading successful!" << endl;
+
+	if (dof_overview) {
+		cout << "Degree of freedom overview:" << endl;
+		cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model);
+	}
+
+	if (model_hierarchy) {
+		cout << "Model Hierarchy:" << endl;
+		cout << RigidBodyDynamics::Utils::GetModelHierarchy (model);
+	}
+
+	return 0;
+}

examples/luamodel/CMakeLists.txt

 
 # And link the library against the executable
 TARGET_LINK_LIBRARIES ( example_luamodel
-	${RBDL_LIBRARY}
+	${RBDL_LIBRARIES}
 	)

examples/luamodel/FindRBDL.cmake

 # Sets the variables
 #   RBDL_FOUND
 #   RBDL_INCLUDE_DIR
-#   RBDL_LIBRARY
+#   RBDL_LIBRARIES
 
 SET (RBDL_FOUND FALSE)
 
 	$ENV{RBDL_LIBRARY_PATH}
 	)
 
+FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY NAMES rbdl_luamodel	PATHS
+	/usr/lib
+	/usr/local/lib
+	$ENV{HOME}/local/lib
+	$ENV{RBDL_PATH}
+	$ENV{RBDL_LIBRARY_PATH}
+	)
+
 IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
 	SET (RBDL_FOUND TRUE)
 ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
 
+IF (RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+	SET (RBDL_LIBRARIES ${RBDL_LIBRARY} ${RBDL_LUAMODEL_LIBRARY})
+ELSE (RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+	SET (RBDL_LIBRARIES ${RBDL_LIBRARY})
+ENDIF(RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+
 IF (RBDL_FOUND)
    IF (NOT RBDL_FIND_QUIETLY)
       MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
 
 MARK_AS_ADVANCED (
 	RBDL_INCLUDE_DIR
-	RBDL_LIBRARY
+	RBDL_LIBRARIES
 	)

examples/luamodel/samplemodel.lua

 	frames = {
 		{
 			name = "pelvis",
-			parent_frame = "ROOT",
+			parent = "ROOT",
 			body = bodies.pelvis,
 			joint = joints.freeflyer,
 		},
 		{
 			name = "thigh_right",
-			parent_frame = "pelvis",
+			parent = "pelvis",
 			body = bodies.thigh_right,
 			joint = joints.spherical_zyx,
-			joint_transform = {
+			joint_frame = {
 				r = {0.0, -0.15, 0.},
 				E = {
 					{1., 0., 0.},
 		},
 		{
 			name = "shank_right",
-			parent_frame = "thigh_right",
+			parent = "thigh_right",
 			body = bodies.thigh_right,
 			joint = joints.rotational_y,
-			joint_transform = {
+			joint_frame = {
 				r = {0.0, 0., -0.42},
 			},
 		},
 		{
 			name = "foot_right",
-			parent_frame = "shank_right",
+			parent = "shank_right",
 			body = bodies.thigh_right,
-			joint_transform = {
+			joint_frame = {
 				r = {0.0, 0., -0.41}
 			},
 		},
 		{
 			name = "thigh_left",
-			parent_frame = "pelvis",
+			parent = "pelvis",
 			body = bodies.thigh_left,
 			joint = joints.spherical_zyx,
-			joint_transform = {
+			joint_frame = {
 				r = {0.0, 0.15, 0.},
 				E = {
 					{1., 0., 0.},
 		},
 		{
 			name = "shank_left",
-			parent_frame = "thigh_left",
+			parent = "thigh_left",
 			body = bodies.thigh_left,
 			joint = joints.rotational_y,
-			joint_transform = {
+			joint_frame = {
 				r = {0.0, 0., -0.42},
 			},
 		},
 		{
 			name = "foot_left",
-			parent_frame = "shank_left",
+			parent = "shank_left",
 			body = bodies.thigh_left,
-			joint_transform = {
+			joint_frame = {
 				r = {0.0, 0., -0.41},
 			},
 		},

examples/simple/FindRBDL.cmake

 # Sets the variables
 #   RBDL_FOUND
 #   RBDL_INCLUDE_DIR
-#   RBDL_LIBRARY
+#   RBDL_LIBRARIES
 
 SET (RBDL_FOUND FALSE)
 
 	$ENV{RBDL_LIBRARY_PATH}
 	)
 
+FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY NAMES rbdl_luamodel	PATHS
+	/usr/lib
+	/usr/local/lib
+	$ENV{HOME}/local/lib
+	$ENV{RBDL_PATH}
+	$ENV{RBDL_LIBRARY_PATH}
+	)
+
 IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
 	SET (RBDL_FOUND TRUE)
 ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
 
+IF (RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+	SET (RBDL_LIBRARIES ${RBDL_LIBRARY} ${RBDL_LUAMODEL_LIBRARY})
+ELSE (RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+	SET (RBDL_LIBRARIES ${RBDL_LIBRARY})
+ENDIF(RBDL_LIBRARY AND RBDL_LUAMODEL_LIBRARY)
+
 IF (RBDL_FOUND)
    IF (NOT RBDL_FIND_QUIETLY)
       MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
 
 MARK_AS_ADVANCED (
 	RBDL_INCLUDE_DIR
-	RBDL_LIBRARY
+	RBDL_LIBRARIES
 	)

examples/simple/example.cc

 
 	std::cout << QDDot.transpose() << std::endl;
 
-	QDot(0) = 1;
-	QDDot(0) = 1;
-	std::cout << "QDot  : " << QDot.transpose() << std::endl;
-	std::cout << "QDDot  : " << QDDot.transpose() << std::endl;
-	Vector3d accel = CalcPointAcceleration ( *model, Q, QDot, QDDot, body_a_id, Vector3d(1,0,0), true );
-	Vector3d velo = CalcPointVelocity ( *model, Q, QDot, body_a_id, Vector3d(1,0,0), false );
-	std::cout << "A_velo : " << velo.transpose() << std::endl;
-	std::cout << "A_accel: " << accel.transpose() << std::endl;
-
 	delete model;
 
  	return 0;
 	 * 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;
 
 
 	static FixedBody CreateFromBody (const Body& body) {
 		FixedBody fbody;
+
 		fbody.mMass = body.mMass;
 		fbody.mCenterOfMass = body.mCenterOfMass;
 		fbody.mSpatialInertia = body.mSpatialInertia;
+
+		return fbody;
 	}
 };
 
 		CS.d_IA[i] = model.mBodies[i].mSpatialInertia;
 
 		if (CS.f_ext_constraints[i] != SpatialVectorZero) {
-			CS.d_pA[i] -= spatial_adjoint(model.X_base[i].toMatrix()) * (CS.f_ext_constraints)[i];
+			CS.d_pA[i] -= model.X_base[i].applyAdjoint ((CS.f_ext_constraints)[i]);
 //			LOG << "f_t (local)[" << i << "] = " << spatial_adjoint(model.X_base[i]) * (*f_ext)[i] << std::endl;
 		}
 //		LOG << "i = " << i << " d_p[i] = " << d_p[i].transpose() << std::endl;
 		if (lambda != 0) {
 			SpatialMatrix Ia = CS.d_IA[i] - CS.d_U[i] * (CS.d_U[i] / CS.d_d[i]).transpose();
 			SpatialVector pa = CS.d_pA[i] + Ia * model.c[i] + CS.d_U[i] * CS.d_u[i] / CS.d_d[i];
-			SpatialTransform X_lambda = model.X_lambda[i];
 
-			// note: X_lambda.inverse().spatial_adjoint() = X_lambda.transpose()
-			CS.d_IA[lambda] = CS.d_IA[lambda] + X_lambda.toMatrixTranspose() * Ia * X_lambda.toMatrix();
-			CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].toMatrixTranspose() * pa;
+#ifdef EIGEN_CORE_H
+			CS.d_IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+			CS.d_pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
+#else
+			CS.d_IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+			CS.d_pA[lambda] += model.X_lambda[i].applyTranspose(pa);
+#endif
 		}
 	}
 
 
 	for (unsigned int i = body_id; i > 0; i--) {
 		if (i == body_id) {
-			CS.d_pA[i] = -spatial_adjoint(model.X_base[i].toMatrix()) * f_t[i];
+			CS.d_pA[i] = -model.X_base[i].applyAdjoint(f_t[i]);
 		}
 
 		CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]);
 
 		unsigned int lambda = model.lambda[i];
 		if (lambda != 0) {
-			CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].toMatrixTranspose() * (CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]);
+			CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].applyTranspose (CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]);
 		}
 	}
 
 
 		unsigned int lambda = model.lambda[i];
 		if (lambda != 0) {
-			SpatialTransform X_lambda = model.X_lambda[i];
-
 			SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
 			SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i];
-
-			// note: X_lambda.inverse().spatial_adjoint() = X_lambda.transpose()
-			model.IA[lambda] = model.IA[lambda] + X_lambda.toMatrixTranspose() * Ia * X_lambda.toMatrix();
-			model.pA[lambda] = model.pA[lambda] + X_lambda.toMatrixTranspose() * pa;
+#ifdef EIGEN_CORE_H
+			model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+			model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
+#else
+			model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
+			model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
+#endif
 		}
 	}
 
 struct Model;
 
 /** \brief General types of joints
- *
- * \todo add proper fixed joint handling
  */
 enum JointType {
 	JointTypeUndefined = 0,
 		return *this;
 	}
 	~Joint() {
-		if (mDoFCount) {
+		if (mJointAxes) {
 			assert (mJointAxes);
 			delete[] mJointAxes;
 			mJointAxes = NULL;
+			mDoFCount = 0;
 		}
 	}
 
 
 		if (joint_type == JointTypeRevolute) {
 			// make sure we have a unit axis
-			assert (joint_axis.squaredNorm() == 1.);
-
-			assert ( joint_axis == Math::Vector3d (1., 0., 0.)
-					|| joint_axis == Math::Vector3d (0., 1., 0.)
-					|| joint_axis == Math::Vector3d (0., 0., 1.));
-
 			mJointAxes[0].set (
 					joint_axis[0],
 					joint_axis[1], 
 	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;
-			return false;
 		}
 
 		bool axis_rotational = false;
 		bool axis_translational = false;
 
 		Math::Vector3d rotation (axis[0], axis[1], axis[2]);
-		if (fabs(rotation.norm() - 1.0) < 1.0e-8)
+		Math::Vector3d translation (axis[3], axis[4], axis[5]);
+
+		if (fabs(translation.norm()) < 1.0e-8)
 			axis_rotational = true;
 
-		Math::Vector3d translation (axis[3], axis[4], axis[5]);
-		if (fabs(translation.norm() - 1.0) < 1.0e-8)
+		if (fabs(rotation.norm()) < 1.0e-8)
 			axis_translational = true;
 
 		return axis_rotational || axis_translational;
 	lambda.push_back(0.);
 	mu.push_back(std::vector<unsigned int>());
 	dof_count = 0;
+	previously_added_body_id = 0;
 
 	gravity = Vector3d (0., -9.81, 0.);
 
 		const Joint &joint,
 		const Body &body,
 		std::string body_name) {
-	if (parent_id >= model.fixed_body_discriminator) {
-		std::cerr << "Error: cannot attach Body with a fixed joint to another fixed body!" << std::endl;
-		assert (0);
-		abort();
-	}
 	FixedBody fbody = FixedBody::CreateFromBody (body);
 	fbody.mMovableParent = parent_id;
 	fbody.mParentTransform = joint_frame;
 
+	if (model.IsFixedBodyId(parent_id)) {
+		FixedBody fixed_parent = model.mFixedBodies[parent_id - model.fixed_body_discriminator];
+
+		fbody.mMovableParent = fixed_parent.mMovableParent;
+		fbody.mParentTransform = fixed_parent.mParentTransform * joint_frame;
+	}
+
 	// merge the two bodies
-	Body parent_body = model.mBodies[parent_id];
-	parent_body.Join (joint_frame, body);
-	model.mBodies[parent_id] = parent_body;
+	Body parent_body = model.mBodies[fbody.mMovableParent];
+	parent_body.Join (fbody.mParentTransform, body);
+	model.mBodies[fbody.mMovableParent] = parent_body;
 
 	model.mFixedBodies.push_back (fbody);
 
 		const Body &body,
 		std::string body_name) {
 	// Here we emulate multi DoF joints by simply adding nullbodies. This
-	// may be changed in the future, but so far it is reasonably fast.
+	// allows us to use fixed size elements for S,v,a, etc. which is very
+	// fast in Eigen.
 	unsigned int joint_count;
 	if (joint.mJointType == JointType1DoF)
 		joint_count = 1;
 	unsigned int null_parent = parent_id;
 	SpatialTransform joint_frame_transform;
 
-	for (unsigned int j = 0; j < joint_count; j++) {
-		Joint single_dof_joint;
+	Joint single_dof_joint;
+	unsigned int j;
 
+	// Here we add multiple virtual bodies that have no mass or inertia for
+	// which each is attached to the model with a single degree of freedom
+	// joint.
+	for (j = 0; j < joint_count; j++) {
 		Vector3d rotation (
 				joint.mJointAxes[j][0],
 				joint.mJointAxes[j][1],
 		else
 			joint_frame_transform = SpatialTransform();
 
-		if (j == joint_count - 1)
+		if (j == joint_count - 1) 
 			// if we are at the last we must add the real body
-			return model.AddBody (null_parent, joint_frame_transform, single_dof_joint, body, body_name);
+			break;
 		else
 			// otherwise we just add an intermediate body
 			null_parent = model.AddBody (null_parent, joint_frame_transform, single_dof_joint, null_body);
 	}
+
+	return model.AddBody (null_parent, joint_frame_transform, single_dof_joint, body, body_name);
 }
 
 unsigned int Model::AddBody (const unsigned int parent_id,
 		const Body &body,
 		std::string body_name) {
 	if (joint.mJointType == JointTypeFixed) {
-		return AddBodyFixedJoint (*this, parent_id, joint_frame, joint, body, body_name);
+		previously_added_body_id = AddBodyFixedJoint (*this, parent_id, joint_frame, joint, body, body_name);
+		return previously_added_body_id;
 	}
 	else if (joint.mJointType != JointTypePrismatic 
 			&& joint.mJointType != JointTypeRevolute) {
-		return AddBodyMultiDofJoint (*this, parent_id, joint_frame, joint, body, body_name);
+		previously_added_body_id = AddBodyMultiDofJoint (*this, parent_id, joint_frame, joint, body, body_name);
+		return previously_added_body_id;
 	}
 
 	assert (lambda.size() > 0);
 		assert (0);
 		abort();
 	}
+	
+	previously_added_body_id = mBodies.size() - 1;
 
-	return mBodies.size() - 1;
+	return previously_added_body_id;
 }
 
 unsigned int Model::AppendBody (
 		const Body &body,
 		std::string body_name 
 		) {
-	unsigned int prev_body_id = mBodies.size() - 1;
-	return Model::AddBody (prev_body_id, joint_frame,
+	return Model::AddBody (previously_added_body_id, joint_frame,
 			joint, body, body_name);
 }
 
 	 * velocity (qdot), acceleration (qddot), and force (tau) vector.
 	 */
 	unsigned int dof_count;
+	/// \brief Id of the previously added body, required for Model::AppendBody()
+	unsigned int previously_added_body_id;
 
 	/// \brief the cartesian vector of the gravity
 	Math::Vector3d gravity;

src/SimpleMath/SimpleMathDynamic.h

 		}
 
 		~Matrix() {
-			if (nrows * ncols > 0 && mData != NULL)
+			if (nrows * ncols > 0 || mData != NULL)
 				delete[] mData;
 
 			nrows = 0;

src/SpatialAlgebraOperators.h

 		m (mass), h (com * mass), I (inertia)
 	{ }
 
-	inline Matrix3d VectorCrossMatrix (const Vector3d &vector) {
+	inline Matrix3d VectorCrossMatrix (const Vector3d &vector) const {
 		return Matrix3d (
 				0., -vector[2], vector[1],
 				vector[2], 0., -vector[0],
 		Vector3d mv_upper (mv[0], mv[1], mv[2]);
 		Vector3d mv_lower (mv[3], mv[4], mv[5]);
 
-		Vector3d res_upper = I * mv_upper + Vector3d (
-				mv_lower[2] * h[1] - mv_lower[1] * h[2],
-				mv_lower[0] * h[2] - mv_lower[2] * h[0],
-				mv_lower[1] * h[0] - mv_lower[0] * h[1]
-				);
-		Vector3d res_lower = m* mv_lower - Vector3d(
-				mv_upper[2] * h[1] - mv_upper[1] * h[2],
-				mv_upper[0] * h[2] - mv_upper[2] * h[0],
-				mv_upper[1] * h[0] - mv_upper[0] * h[1]
-				);
-
+		Vector3d res_upper = I * Vector3d (mv[0], mv[1], mv[2]) + h.cross(mv_lower);
+		Vector3d res_lower = m * mv_lower - h.cross (mv_upper);
+			
 		return SpatialVector (
 				res_upper[0], res_upper[1], res_upper[2],
 				res_lower[0], res_lower[1], res_lower[2]
 		I = Ic.block<3,3>(0,0);
 	}
 
-	SpatialMatrix toMatrix() {
+	SpatialMatrix toMatrix() const {
 		SpatialMatrix result;
 		result.block<3,3>(0,0) = I;
 		result.block<3,3>(0,3) = VectorCrossMatrix(h);
 	 *
 	 * \returns (E^T * n + rx * E^T * f, E^T * f)
 	 */
+	SpatialRigidBodyInertia apply (const SpatialRigidBodyInertia &rbi) {
+		return SpatialRigidBodyInertia (
+				rbi.m,
+				E.transpose() * (rbi.h / rbi.m) + r,
+				E.transpose() * rbi.I * E
+				- VectorCrossMatrix (r) * VectorCrossMatrix(E.transpose() * rbi.h)
+				- VectorCrossMatrix (E.transpose() * (rbi.h) + r * rbi.m) * VectorCrossMatrix (r)
+				);
+	}
+
 	SpatialVector applyTranspose (const SpatialVector &f_sp) {
 		Vector3d E_T_f (
 				E(0,0) * f_sp[3] + E(1,0) * f_sp[4] + E(2,0) * f_sp[5],
 				);
 	}
 
-	SpatialRigidBodyInertia apply (const SpatialRigidBodyInertia &rbi) {
-		return SpatialRigidBodyInertia (
-				rbi.m,
-				E.transpose() * (rbi.h / rbi.m) + r,
-				E.transpose() * rbi.I * E
-				- VectorCrossMatrix (r) * VectorCrossMatrix(E.transpose() * rbi.h)
-				- VectorCrossMatrix (E.transpose() * (rbi.h) + r * rbi.m) * VectorCrossMatrix (r)
+	SpatialVector applyAdjoint (const SpatialVector &f_sp) {
+		Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Vector3d (f_sp[3], f_sp[4], f_sp[5])));
+//		Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Eigen::Map<Vector3d> (&(f_sp[3]))));
+
+		return SpatialVector (
+				En_rxf[0],
+				En_rxf[1],
+				En_rxf[2],
+				E(0,0) * f_sp[3] + E(0,1) * f_sp[4] + E(0,2) * f_sp[5],
+				E(1,0) * f_sp[3] + E(1,1) * f_sp[4] + E(1,2) * f_sp[5],
+				E(2,0) * f_sp[3] + E(2,1) * f_sp[4] + E(2,2) * f_sp[5]
 				);
 	}
 
 	Vector3d r;
 };
 
+inline std::ostream& operator<<(std::ostream& output, const SpatialRigidBodyInertia &rbi) {
+	output << "rbi.m = " << rbi.m << std::endl;
+	output << "rbi.h = " << rbi.h.transpose();
+	output << "rbi.I = " << std::endl << rbi.I << std::endl;
+	return output;
+}
+
 inline std::ostream& operator<<(std::ostream& output, const SpatialTransform &X) {
 	output << "X.E = " << std::endl << X.E << std::endl;
 	output << "X.r = " << X.r.transpose();
+#include "rbdl_utils.h"
+
+#include "rbdl_math.h"
+#include "Model.h"
+
+#include <sstream>
+#include <iomanip>
+
+namespace RigidBodyDynamics {
+
+namespace Utils {
+
+using namespace std;
+using namespace Math;
+
+string get_dof_name (const SpatialVector &joint_dof) {
+	if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) 
+		return "RX";
+	else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.))
+		return "RY";
+	else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.))
+		return "RZ";
+	else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.))
+		return "TX";
+	else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.))
+		return "TY";
+	else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.))
+		return "TZ";
+
+	ostringstream dof_stream(ostringstream::out);
+	dof_stream << "custom (" << joint_dof.transpose() << ")";
+	return dof_stream.str();
+}
+
+string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) {
+	if (model.mBodies[body_id].mMass == 0.) {
+		// this seems to be a virtual body that was added by a multi dof joint
+		unsigned int child_index = 0;
+
+		// if there is not a unique child we do not know what to do...
+		if (model.mu[body_id].size() != 1)
+			return "";
+
+		unsigned int child_id = model.mu[body_id][0];
+
+		return get_body_name (model, model.mu[body_id][0]);
+	}
+
+	return model.GetBodyName(body_id);
+}
+
+std::string GetModelDOFOverview (const Model &model) {
+	stringstream result ("");
+
+	for (unsigned int i = 1; i < model.mBodies.size(); i++) {
+		result << setfill(' ') << setw(3) << i - 1 << ": " << get_body_name(model, i) << "_" << get_dof_name (model.S[i]) << endl;
+	}
+
+	return result.str();
+}
+
+std::string print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) {
+	stringstream result ("");
+
+	for (int j = 0; j < indent; j++)
+		result << "  ";
+
+	result << get_body_name (model, body_index);
+
+	if (body_index == 0) {
+		result << endl;
+		result << print_hierarchy (model, 1, 1);
+		return result.str();
+	}