Commits

Martin Felis committed 8a84dc2 Merge

merged dev branch into stable

  • Participants
  • Parent commits bdb7617, e11797d

Comments (0)

Files changed (419)

CMake/FindUnitTest++.cmake

 
 SET (UNITTEST++_FOUND FALSE)
 
-FIND_PATH (UNITTEST++_INCLUDE_DIR UnitTest++.h /usr/include/unittest++ /usr/local/include/unittest++ $ENV{UNITTESTXX_PATH}/src $ENV{UNITTESTXX_INCLUDE_PATH})
+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 $ENV{UNITTESTXX_PATH} ENV{UNITTESTXX_LIBRARY_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)
-PROJECT (ABA)
+PROJECT (RBDL)
 
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake )
 
 INCLUDE_DIRECTORIES ( 
+	${CMAKE_CURRENT_BINARY_DIR}/include/rbdl
 	src/
+	include/rbdl
 )
 
 SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
 		LINKER_LANGUAGE CXX
 	)
 
-ADD_SUBDIRECTORY ( tests )
-ADD_SUBDIRECTORY ( gui )
+# Perform the proper linking
+SET (CMAKE_SKIP_BUILD_RPATH FALSE)
+SET (CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
-ADD_LIBRARY ( aba 
+# Options
+OPTION (BUILD_STATIC "Build the statically linked library" OFF)
+OPTION (BUILD_GUI "Build the (rudimentary) graphical user interface" 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/mathutils.cc
 	src/Logging.cc
 	src/Body.cc
 	src/Dynamics.cc
 	)
 
-OPTION (ENABLE_LOGGING "Enable logging (major impact on performance!)" OFF)
+ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} )
 
-IF ( ENABLE_LOGGING )
-	SET ( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_LOGGING" )
-ENDIF ( ENABLE_LOGGING )
+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")
+
+	INSTALL (TARGETS rbdl-static
+	  LIBRARY DESTINATION lib
+  	ARCHIVE DESTINATION lib
+	)
+ENDIF (BUILD_STATIC)
+
+IF (BUILD_TESTS)
+  ADD_SUBDIRECTORY ( tests )
+ENDIF (BUILD_TESTS)
+
+IF (BUILD_GUI)
+  ADD_SUBDIRECTORY ( gui )
+ENDIF (BUILD_GUI)
+
+# Installing
+INSTALL (TARGETS rbdl
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	)
+
+CONFIGURE_FILE (
+	"${CMAKE_CURRENT_SOURCE_DIR}/src/rbdlconfig.h.cmake" 
+	"${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdlconfig.h"
+	)
+
+FILE ( GLOB headers 
+	"${CMAKE_CURRENT_SOURCE_DIR}/src/*.h"
+	"${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdlconfig.h"
+	)
+
+INSTALL ( FILES ${headers} DESTINATION include/rbdl )
+
+IF (NOT RBDL_USE_SIMPLE_MATH)
+	INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src/Eigen"
+		DESTINATION include/rbdl
+	)
+ENDIF (NOT RBDL_USE_SIMPLE_MATH)
 # with spaces.
 
 INPUT                  = ./src \
+                         doc/example.h \
                          doc/Mainpage.h
 
 # This tag can be used to specify the character encoding of the source files 
 # excluded from the INPUT source files. This way you can easily exclude a 
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ./src/cml
+EXCLUDE                = ./src/cml \
+                         ./src/Eigen
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or 
 # directories that are symbolic links (a Unix filesystem feature) are excluded 
 # tagging system (see http://www.gnu.org/software/global/global.html). You 
 # will need version 4.8.6 or higher.
 
-USE_HTAGS              = YES
+USE_HTAGS              = NO
 
 # If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen 
 # will generate a verbatim copy of the header file for each class for 
 # This tag can be used to set the number of enum values (range [1..20]) 
 # that doxygen will group on one line in the generated HTML documentation.
 
-ENUM_VALUES_PER_LINE   = 4
+ENUM_VALUES_PER_LINE   = 3
 
 # The GENERATE_TREEVIEW tag is used to specify whether a tree-like index 
 # structure should be generated to display hierarchical information. 
 # JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). 
 # Windows users are probably better off using the HTML help feature.
 
-GENERATE_TREEVIEW      = NO
+GENERATE_TREEVIEW      = YES
 
 # By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, 
 # and Class Hierarchy pages using a tree view instead of an ordered list.
 # used to set the initial width (in pixels) of the frame in which the tree 
 # is shown.
 
-TREEVIEW_WIDTH         = 250
+TREEVIEW_WIDTH         = 248
 
 # Use this tag to change the font size of Latex formulas included 
 # as images in the HTML documentation. The default is 10. Note that 
+RBDL - Rigid Body Dynamics Library
+
+This code is mainly an C++ implementation of Roy Featherstone's Articulated
+Body Algorithm (ABA) written by Martin Felis. The code tightly follows the
+notation used in Featherstone's book "Rigid Body Dynamics Algorithm".
+
+The documentation is contained in the code and can be extracted with the
+tool doxygen (http://www.doxygen.org).
+
+To create the documentation simply run
+  doxygen Doxyfile
+which will generate the documentation in the subdirectory ./doc/html. The
+mainpage will then be located in ./doc/html/index.html.
 /** \file Mainpage.h 
- * \mainpage Main Page
+ * \mainpage Rigid Body Dynamics Library
  *
  * This is the documentation of a yet to be named rigid body simulation
- * code. So far the code supports the simulation of forward dynamics of
- * tree structured (i.e. no kinematic loops) rigid body models by using the
- * Articulated Body Algorithm (ABA) by Roy Featherstone. The code is
- * heavily inspired by the pseudo code of the book "Rigid Body Dynamics
- * Algorithms" of Featherstone.
+ * code. So far the code supports forward and inverse dynamics by using the
+ * Articulated Body Algorathm and the Newton-Euler algorithm, respectively.
+ * Additionally it also containes the Composite Rigid Body Algorithm that
+ * computes the joint space inertia matrix.
  *
- * The library uses the configurable math library (which can be found here:
- * <a href="http://www.cmldev.net">http://www.cmldev.net</a>).
+ * The code is written by <a
+ * href="mailto:martin.felis@iwr.uni-heidelberg.de">Martin Felis
+ * <martin.felis@iwr.uni-heidelberg.de></a> and heavily inspired by the
+ * pseudo code of the book "Rigid Body Dynamics Algorithms" of <a
+ * href="http://users.cecs.anu.edu.au/~roy/">Roy Featherstone</a>.
+ * 
+ * The library comes with version 3 of the the
+ * <a href="http://eigen.tuxfamily.org/">Eigen</a> math library. More
+ * information about it can be found here:
+ * <a href="http://eigen.tuxfamily.org/">http://eigen.tuxfamily.org/</a>).
  *
  * Documentation of the functions can be found at the documentation page of
  * the namespace RigidBodyDynamics.
  *
- * \section Example An Example
- * Here is a simple example how one can create a model and compute the
- * forward dynamics for it:
- *
- * \code
- *	#include <iostream>
- *	
- *	#include "Model.h"
- *	#include "Dynamics.h"
- *	
- *	#include "mathutils.h" // required for Xtrans
- *	
- *	int main (int argc, char* argv[]) {
- *		Model* model = NULL;
- *	
- *		unsigned int body_a_id, body_b_id, body_c_id;
- *		Body body_a, body_b, body_c;
- *		Joint joint_a, joint_b, joint_c;
- *	
- *		model = new Model();
- *		model->Init();
- *	
- *		body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.));
- *		joint_a = Joint(
- *			JointTypeRevolute,
- *			Vector3d (0., 0., 1.)
- *		);
- *		
- *		body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a);
- *		
- *		body_b = Body (1., Vector3d (0., 0.5, 0.), Vector3d (1., 1., 1.));
- *		joint_b = Joint (
- *			JointTypeRevolute,
- *			Vector3d (0., 0., 1.)
- *		);
- *		
- *		body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b);
- *		
- *		body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.));
- *		joint_c = Joint (
- *			JointTypeRevolute,
- *			Vector3d (0., 0., 1.)
- *		);
- *		
- *		body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c);
- *	
- *		cmlVector Q(3);
- *		cmlVector QDot(3);
- *		cmlVector QDDot(3);
- *		cmlVector Tau(3);
- *	
- *	 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
- *	
- *		std::cout << QDDot << std::endl;
- *	
- *	 	return 0;
- *	}
- * \endcode
- *
- * If the library itself is already created, one can compile this example
- * with:
- * \code
- * 	g++ example.cc -I<path to src folder> -laba -L<path to libaba.a> -o example
- * \endcode
+ * A simple example can be found \ref SimpleExample "here".
  *
  * \section ModelConstruction Construction of Models
  *
  * Adding bodies to the model is done by specifying the parent body by its
  * id, the transformation from the parent origin to the joint origin, the
  * joint specification as an object, and the body itself. These parameters
- * are then fed to the function Model::AddBody().
+ * are then fed to the function RigidBodyDynamics::Model::AddBody().
+ *
+  * \todo [low] incorporate GiNaC (http://www.ginac.de) to generate code
+ * \todo [low] serialization of the model?
+ *
+ * \subsection ToDo_done Done:
+ * <ul>
+ *   <li>[high] check impulse computation 2011-06-07</li>
+ *   <li>[med] add specification for the visualization to the model</li>
+ *   <li>[med] get replace std::vector<> vectors with proper math vectors in Model</li>
+ * </ul>
  */
+/** \file example.h 
+ * \page SimpleExample A simple example
+ *
+ * Here is a simple example how one can create a meaningless model and
+ * compute the forward dynamics for it:
+ * 
+ * \code
+ *	#include <iostream>
+ *
+ *	#include <rbdl.h>
+ *
+ *	using namespace RigidBodyDynamics;
+ *
+ *	int main (int argc, char* argv[]) {
+ *	Model* model = NULL;
+ *
+ *	unsigned int body_a_id, body_b_id, body_c_id;
+ *	Body body_a, body_b, body_c;
+ *	Joint joint_a, joint_b, joint_c;
+ *
+ *	model = new Model();
+ *	model->Init();
+ *
+ *	model->gravity = Vector3d (0., -9.81, 0.);
+ *
+ *	body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.));
+ *		joint_a = Joint(
+ *		JointTypeRevolute,
+ *		Vector3d (0., 0., 1.)
+ *	);
+ *	
+ *	body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a);
+ *	
+ *	body_b = Body (1., Vector3d (0., 0.5, 0.), Vector3d (1., 1., 1.));
+ *		joint_b = Joint (
+ *		JointTypeRevolute,
+ *		Vector3d (0., 0., 1.)
+ *	);
+ *	
+ *	body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b);
+ *	
+ *	body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.));
+ *		joint_c = Joint (
+ *		JointTypeRevolute,
+ *		Vector3d (0., 0., 1.)
+ *	);
+ *	
+ *	body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c);
+ *
+ *	VectorNd Q = VectorNd::Zero (model->dof_count);
+ *	VectorNd QDot = VectorNd::Zero (model->dof_count);
+ *	VectorNd Tau = VectorNd::Zero (model->dof_count);
+ *	VectorNd QDDot = VectorNd::Zero (model->dof_count);
+ *
+ * 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
+ *
+ *	std::cout << QDDot.transpose() << std::endl;
+ *
+ *	delete model;
+ *
+ * 	return 0;
+ *}
+ * \endcode
+ *
+ * If the library itself is already created, one can compile this example
+ * with:
+ * \code
+ * 	g++ example.cc -I<path to src folder> -lrbdl -L<path to librbdl.a> -o example
+ * \endcode
+ *
+ * Additionally there is a CMakeLists.txt, that can be used to automatically
+ * create the makefiles by using <a href="http://www.cmake.org">CMake</a>.
+ * It uses the script FindRBDL.cmake which can be used to find the library
+ * and include directory of the headers.
+ *
+ * The FindRBDL.cmake script can use the environment variables RBDL_PATH,
+ * RBDL_INCLUDE_PATH, and RBDL_LIBRARY_PATH to find the required files.
+ *
+ */

doc/notes/point_velocity_acceleration.tex

 To compute the acceleration of a point we have to compute the spatial
 acceleration of a body in base coordinates. This can be expressed as:
 \begin{equation}
-	{}^0\Spa{a}_i = {}^0\Spa{a}_{\lambda(i)}
+	\Spa{a}_i = {}^0\Spa{a}_{\lambda(i)}
 	+ {}^{0}\Spa{X}_{i} \Spa{a}_{Ji}
 \end{equation}
 for which $\Spa{a}_{Ji} = \Spa{S}_i \ddot{q}_i + \dot{\Spa{S}}_i \dot{q}_i$.
 The linear part of this vector is then the acceleration of the point in base
 coordinates.
 
-Looking again at the components of the rotational and translational part of
-the combined 6D vector reveals the following (after some re-ordering):
-
-\begin{equation}
-	{}^0\Nspa{a}_p = \Nspa{a}_i +
-	\Nspa{\dot{\omega}}_i
-	\times
-	{}^0\Nspa{r}_p
-	+
-	\Nspa{\omega}_i \times \Nspa{v}_p
-	\label{}
-\end{equation}
-
-We shall derive this equation with standard 3D notation. Assuming we have the
-velocity of point $\Vec{p}$ given as ${}^0\Vec{v}_p = {}^0\Vec{v}_i +
-\Vec{\omega}_i \times {}^0\Vec{r}_p$. Taking the derivative gives us:
-\begin{eqnarray}
-	\frac{d}{dt} {}^0 \Vec{v}_p & = & {}^0 \Vec{a}_i + \Vec{\dot{\omega}}_i \times
-	{}^0 \Vec{r}_p + \Vec{\omega}_i \times {}^0 \Vec{ v}_p
-	\label{}
-\end{eqnarray}
-as both $\Vec{\omega}$ and ${}^0\Vec{r}_p$ can change over time. The
-expression we get is exactly what we found for the translational part of our
-6D vector.
-
-
 \end{document}

example/CMakeLists.txt

+PROJECT (RBDLEXAMPLE CXX)
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+# We need to add the project source path to the CMake module path so that
+# the FindRBDL.cmake script can be found.
+LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} )
+
+# Search for the RBDL include directory and library
+FIND_PACKAGE (RBDL REQUIRED)
+
+# Add the include directory to the include paths
+INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} )
+
+# Create an executable
+ADD_EXECUTABLE (rbdlexample example.cc)
+
+# And link the library against the executable
+TARGET_LINK_LIBRARIES ( rbdlexample
+	${RBDL_LIBRARY}
+	)

example/FindRBDL.cmake

+# Searches for RBDL includes and library files
+#
+# Sets the variables
+#   RBDL_FOUND
+#   RBDL_INCLUDE_DIR
+#   RBDL_LIBRARY
+
+SET (RBDL_FOUND FALSE)
+
+FIND_PATH (RBDL_INCLUDE_DIR 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
+	/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_FOUND)
+   IF (NOT RBDL_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
+   ENDIF (NOT RBDL_FIND_QUIETLY)
+ELSE (RBDL_FOUND)
+   IF (RBDL_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RBDL")
+   ENDIF (RBDL_FIND_REQUIRED)
+ENDIF (RBDL_FOUND)
+
+MARK_AS_ADVANCED (
+	RBDL_INCLUDE_DIR
+	RBDL_LIBRARY
+	)

example/example.cc

 #include <iostream>
 
-#include "Model.h"
-#include "Dynamics.h"
+#include <rbdl.h>
 
-#include "mathutils.h" // required for Xtrans
+using namespace RigidBodyDynamics;
 
 int main (int argc, char* argv[]) {
 	Model* model = NULL;
 	model = new Model();
 	model->Init();
 
+	model->gravity = Vector3d (0., -9.81, 0.);
+
 	body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.));
 		joint_a = Joint(
 		JointTypeRevolute,
 	
 	body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c);
 
-	cmlVector Q(3);
-	cmlVector QDot(3);
-	cmlVector QDDot(3);
-	cmlVector Tau(3);
+	VectorNd Q = VectorNd::Zero (model->dof_count);
+	VectorNd QDot = VectorNd::Zero (model->dof_count);
+	VectorNd Tau = VectorNd::Zero (model->dof_count);
+	VectorNd QDDot = VectorNd::Zero (model->dof_count);
 
  	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 
-	std::cout << QDDot << std::endl;
+	std::cout << QDDot.transpose() << std::endl;
+
+	delete model;
 
  	return 0;
 }

gui/CMakeLists.txt

-PROJECT ( abagui )
+PROJECT ( rbdlgui )
 
 CMAKE_MINIMUM_REQUIRED (VERSION 2.6)
 
 
 	INCLUDE (${QT_USE_FILE})
 
-	SET ( abaMainWindow_UIS
-		ui/abaMainWindow.ui
+	SET ( rbdlMainWindow_UIS
+		ui/rbdlMainWindow.ui
 		)
 	
-	QT4_WRAP_UI (abaMainWindow_UIS_H ${abaMainWindow_UIS})
+	QT4_WRAP_UI (rbdlMainWindow_UIS_H ${rbdlMainWindow_UIS})
 
-	SET ( abaQtApp_SRCS
+	SET ( rbdlQtApp_SRCS
 		src/qtmain.cc
-		src/abaQtApp.cc
+		src/rbdlQtApp.cc
 		src/glwidget.cc
 		src/modeldrawing.cc
 		src/modelstate.cc
 		src/glprimitives.cc
 		)
 
-	QT4_WRAP_CPP ( abaQtApp_MOC_SRCS
-		src/abaQtApp.h
+	QT4_WRAP_CPP ( rbdlQtApp_MOC_SRCS
+		src/rbdlQtApp.h
 		src/glwidget.h
 		)
 
-	ADD_EXECUTABLE ( abagui
-		${abaQtApp_SRCS}
-		${abaQtApp_MOC_SRCS}
-		${abaMainWindow_UIS_H}
+	ADD_EXECUTABLE ( rbdlgui
+		${rbdlQtApp_SRCS}
+		${rbdlQtApp_MOC_SRCS}
+		${rbdlMainWindow_UIS_H}
 	)
 
  	INCLUDE_DIRECTORIES (${QT_INCLUDE_DIR})
 		INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" 
   )
 
-  TARGET_LINK_LIBRARIES ( abagui
+  TARGET_LINK_LIBRARIES ( rbdlgui
 		${QT_LIBRARIES}
 		${OPENGL_LIBRARIES}
-		aba	
+		rbdl	
   )
 
-	INSTALL (TARGETS abagui
+	INSTALL (TARGETS rbdlgui
 	RUNTIME DESTINATION bin
 	LIBRARY DESTINATION lib
 	)

gui/src/abaQtApp.cc

-#include <QtGui> 
-
-#include "glwidget.h" 
-#include "abaQtApp.h"
-
-#include <assert.h>
-#include <iostream>
-
-using namespace std;
-
-abaQtApp::abaQtApp(QWidget *parent)
-{
-	timer = new QTimer (this);
-
-	setupUi(this); // this sets up GUI
-
-	timer->setSingleShot(false);
-	timer->start(20);
-
-	// the timer is used to continously redraw the OpenGL widget
-	connect (timer, SIGNAL(timeout()), glWidget, SLOT(updateGL()));
-	
-	connect (actionQuit, SIGNAL( triggered() ), qApp, SLOT( quit() ));
-}

gui/src/abaQtApp.h

-#ifndef ABAMAINWINDOW_H
-#define ABAMAINWINDOW_H
-
-#include <QTimer>
-#include "ui_abaMainWindow.h"
-
-class abaQtApp : public QMainWindow, public Ui::abaMainWindow
-{
-    Q_OBJECT
- 
-public:
-    abaQtApp(QWidget *parent = 0);
-
-protected:
-		QTimer *timer;
-};
- 
-#endif

gui/src/glprimitives.cc

 	BufferIndexTorusPosition,
 	BufferIndexTorusIndex,
 	BufferIndexTorusNormal,
+	BufferIndexSpherePosition,
+	BufferIndexSphereNormal,
 	BufferIndexLast
 };
 
 	 0,-1, 0,   0,-1, 0,  0,-1, 0,  0,-1, 0, // v7-v4-v3-v2
 	 0, 0,-1,   0, 0,-1,  0, 0,-1,  0, 0,-1  // v4-v7-v6-v5
 
-};        
+};
+
+GLfloat sphere_x = 0.525731112119133606;
+GLfloat sphere_z = 0.850650808352039932;
+
+GLfloat sphere_vertices[60][3] = {
+   {sphere_x, 0.f, sphere_z  },  { 0.0, sphere_z, sphere_x }, 	{-sphere_x, 0.f, sphere_z },
+   { 0.0, sphere_z, sphere_x },  {-sphere_z, sphere_x, 0.f }, 	{-sphere_x, 0.f, sphere_z }, 
+   { 0.0, sphere_z, sphere_x },  {0.f, sphere_z, -sphere_x }, 	{-sphere_z, sphere_x, 0.f }, 
+   { sphere_z, sphere_x, 0.f },  {0.f, sphere_z, -sphere_x }, 	{ 0.0, sphere_z, sphere_x }, 
+   {sphere_x, 0.f, sphere_z  },  { sphere_z, sphere_x, 0.f }, 	{ 0.0, sphere_z, sphere_x }, 
+                                                              
+   {sphere_x, 0.f, sphere_z  },  {sphere_z, -sphere_x, 0.f }, 	{ sphere_z, sphere_x, 0.f },  
+   {sphere_z, -sphere_x, 0.f },  {sphere_x, 0.f, -sphere_z }, 	{ sphere_z, sphere_x, 0.f }, 
+   {sphere_z, sphere_x, 0.f  },  {sphere_x, 0.f, -sphere_z }, 	{0.f, sphere_z, -sphere_x }, 
+   {sphere_x, 0.f, -sphere_z },  {-sphere_x, 0.f, -sphere_z}, 	{0.f, sphere_z, -sphere_x }, 
+   {sphere_x, 0.f, -sphere_z },  {0.f, -sphere_z, -sphere_x}, 	{-sphere_x, 0.f, -sphere_z}, 
+                                                              
+   {sphere_x, 0.f, -sphere_z },  {sphere_z, -sphere_x, 0.f }, 	{0.f, -sphere_z, -sphere_x}, 
+   {sphere_z, -sphere_x, 0.f },  {0.f, -sphere_z, sphere_x }, 	{0.f, -sphere_z, -sphere_x}, 
+   {0.f, -sphere_z, sphere_x },  {-sphere_z, -sphere_x, 0.f}, 	{0.f, -sphere_z, -sphere_x}, 
+   {0.f, -sphere_z, sphere_x },  {-sphere_x, 0.f, sphere_z }, 	{-sphere_z, -sphere_x, 0.f}, 
+   {0.f, -sphere_z, sphere_x },  {sphere_x, 0.f, sphere_z  }, 	{-sphere_x, 0.f, sphere_z }, 
+                                                              
+   {sphere_z, -sphere_x, 0.f },  {sphere_x, 0.f, sphere_z  }, 	{0.f, -sphere_z, sphere_x }, 
+   {-sphere_z, -sphere_x, 0.f},  {-sphere_x, 0.f, sphere_z }, 	{-sphere_z, sphere_x, 0.f },
+   {-sphere_x, 0.f, -sphere_z},  {-sphere_z, -sphere_x, 0.f}, 	{-sphere_z, sphere_x, 0.f }, 
+   {0.f, sphere_z, -sphere_x },  {-sphere_x, 0.f, -sphere_z}, 	{-sphere_z, sphere_x, 0.f }, 
+   {-sphere_z, -sphere_x, 0.f},  {-sphere_x, 0.f, -sphere_z}, 	{0.f, -sphere_z, -sphere_x} 
+};
+
+GLfloat sphere_normals[60][3];
+
+GLsizeiptr sphere_element_count = 60;
+GLsizeiptr sphere_position_size = sphere_element_count * 3 * sizeof (GLfloat);
+GLsizeiptr sphere_normal_size   = sphere_element_count * 3 * sizeof (GLfloat);
+
 void check_opengl_error (const char* file, int line) {
 	GLenum gl_error = glGetError();
 	if (gl_error) {
 	torus_normal_size = torus_element_count * 3 * sizeof (GLfloat);
 	torus_normal_data = new GLfloat[torus_element_count * 3];
 
-	unsigned int i;
+	int i;
 	GLfloat rad_inc = static_cast<GLfloat> (M_PI * 2 / static_cast<GLfloat> (disc_slices));
 	for (i = 0; i < (torus_element_count - 2); i = i + 2) {
 		GLfloat rad_val = static_cast<GLfloat> (i) * rad_inc * 0.5f;
 	torus_normal_data = NULL;
 }
 
+void glprimitives_sphere_init () {
+	// Vertices
+	glBindBuffer (GL_ARRAY_BUFFER, BufferNames[BufferIndexSpherePosition]);
+	check_opengl_error (__FILE__, __LINE__);
+	glBufferData (GL_ARRAY_BUFFER, sphere_position_size, sphere_vertices, GL_STATIC_DRAW);
+	check_opengl_error (__FILE__, __LINE__);
+
+	// Normals
+	glBindBuffer (GL_ARRAY_BUFFER, BufferNames[BufferIndexSphereNormal]);
+	check_opengl_error (__FILE__, __LINE__);
+	glBufferData (GL_ARRAY_BUFFER, sphere_normal_size, sphere_vertices, GL_STATIC_DRAW);
+	check_opengl_error (__FILE__, __LINE__);
+}
+
 void glprimitives_init () {
 	// initialize VBO for the cube
 	glGenBuffers(BufferIndexLast, BufferNames);
 	glprimitives_cube_init();
 	glprimitives_disc_init();
 	glprimitives_torus_init();
+	glprimitives_sphere_init();
 }
 
 void glprimitives_destroy () {
 
 	glEnable(GL_RESCALE_NORMAL);
 
-	glDrawArrays (GL_QUADS, 0, 6 * 3 * 4);
+	glDrawArrays (GL_QUADS, 0, 6 * 4);
 
 	glDisable(GL_RESCALE_NORMAL);
 
 		glprimitives_disc();
 	glPopMatrix();
 }
+
+void glprimitives_sphere () {
+	static GLUquadric* quadric;
+
+	quadric = gluNewQuadric();
+
+	gluSphere (quadric, 1., 16, 16);
+	glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
+
+	glScalef (1.001f, 1.001f, 1.001f);
+	glColor3f (0., 0., 0.);
+	gluSphere (quadric, 1., 16, 16);
+	glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
+	
+	gluDeleteQuadric(quadric);
+
+	/*
+	glBindBuffer (GL_ARRAY_BUFFER, BufferNames[BufferIndexSpherePosition]);
+	check_opengl_error (__FILE__, __LINE__);
+	glVertexPointer (3, GL_FLOAT, 0, 0);
+	glBindBuffer (GL_ARRAY_BUFFER, BufferNames[BufferIndexSphereNormal]);
+	check_opengl_error (__FILE__, __LINE__);
+	glNormalPointer (GL_FLOAT, 0, 0);
+	check_opengl_error (__FILE__, __LINE__);
+
+	glEnable(GL_COLOR_MATERIAL);
+	glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
+	glEnableClientState (GL_VERTEX_ARRAY);
+	glEnableClientState (GL_NORMAL_ARRAY);
+
+	glEnable(GL_RESCALE_NORMAL);
+
+	glDrawArrays (GL_TRIANGLES, 0, sphere_element_count);
+//	glDrawElements (GL_TRIANGLES, 20 * 3, GL_UNSIGNED_INT, NULL);
+//	glDrawArrays (GL_QUAD_STRIP, 0, sphere_element_count);
+
+	glDisable(GL_RESCALE_NORMAL);
+
+	glDisableClientState (GL_VERTEX_ARRAY);
+	glDisableClientState (GL_NORMAL_ARRAY);
+	glDisableClientState (GL_COLOR_ARRAY);
+	glDisable(GL_COLOR_MATERIAL);
+
+	glBindBuffer (GL_ARRAY_BUFFER, 0);
+	*/
+}

gui/src/glprimitives.h

 void glprimitives_disc ();
 void glprimitives_torus ();
 
+void glprimitives_sphere ();
+
 #endif /* _GLPRIMITIVES_H */

gui/src/glwidget.cc

     : QGLWidget(parent)
 {
 	poi.setX(0.);
-	poi.setY(-0.4);
+	poi.setY(1.0);
 	poi.setZ(0.);
 
-	eye.setX(4.);
-	eye.setY(1.);
-	eye.setZ(4.);
+	eye.setX(6.);
+	eye.setY(3.);
+	eye.setZ(6.);
 
 	updateSphericalCoordinates();
 
 			up.x(), up.y(), up.z());
 }
 
+void GLWidget::drawGrid() {
+	float xmin, xmax, xstep, zmin, zmax, zstep;
+	int i, count;
+
+	xmin = -16;
+	xmax = 16;
+	zmin = -16;
+	zmax = 16;
+
+	count = 32;
+
+	xstep = fabs (xmin - xmax) / (float)count;
+	zstep = fabs (zmin - zmax) / (float)count;
+
+	glColor3f (0.6, 0.6, 0.6);
+	glBegin (GL_LINES);
+	for (i = 0; i <= count; i++) {
+		glVertex3f (i * xstep + xmin, 0., zmin);
+		glVertex3f (i * xstep + xmin, 0., zmax);
+		glVertex3f (xmin, 0, i * zstep + zmin);
+		glVertex3f (xmax, 0, i * zstep + zmin);
+	}
+	glEnd ();
+}
+
 void GLWidget::paintGL() {
 	update_timer();
 	glClearColor (0.3, 0.3, 0.3, 1.);
 
 	updateCamera();
 
+	GLfloat light_pos[4] = {20.0f, 20.0f, 20.0f, 1.0f};
+	glLightfv (GL_LIGHT0, GL_POSITION, light_pos);
+
 	if (update_simulation)
 		model_update (delta_time_sec);
 
-//	draw_model (model_get());
+	glEnable (GL_COLOR_MATERIAL);
+	glDisable(GL_LIGHTING);
+
+	drawGrid();
+	glDisable (GL_COLOR_MATERIAL);
+	glEnable(GL_LIGHTING);
+
+	draw_model (model_get());
 }
 
 void GLWidget::resizeGL(int width, int height)
 		theta = std::min(theta, static_cast<float>(M_PI * 0.99));
 	} else if (event->buttons().testFlag(Qt::MiddleButton)) {
 		// move
-		QVector3D eye_normalized (eye);
+		QVector3D eye_normalized (poi - eye);
 		eye_normalized.normalize();
-		QVector3D right = QVector3D::crossProduct (up, eye_normalized) * -1.;
-		poi += right * dx * 0.01 + up * dy * 0.01;
-		eye += right * dx * 0.01 + up * dy * 0.01;
+
+		QVector3D global_y (0., 1., 0.);
+		QVector3D right = QVector3D::crossProduct (up, eye_normalized);
+		poi += right * dx * 0.01 + global_y * dy * 0.01;
+		eye += right * dx * 0.01 + global_y * dy * 0.01;
 	} else if (event->buttons().testFlag(Qt::RightButton)) {
 		// zoom
 		r += 0.05 * dy;

gui/src/glwidget.h

 
 	protected:
 		void update_timer();
+		void drawGrid();
 
 		void initializeGL();
 		void paintGL();

gui/src/modeldrawing.cc

 #include "Model.h"
 #include "Joint.h"
 #include "Body.h"
+#include "Visualization.h"
 
 #include <sstream>
 
 #include "glprimitives.h"
 
 using namespace RigidBodyDynamics;
+using namespace SpatialAlgebra::Operators;
+
+typedef std::list<Visualization::Primitive> VisualizationPrimitiveList;
 
 void compute_body_center_and_dimensions (Model* model, unsigned int body_id, Vector3d &body_center, Vector3d &body_dimensions) {
-	int j;
+	unsigned int j;
 
 	// draw the body as a green box that extends from the origin to the
 	// next joint
 		// dimensions such that the boundaries reach to the joint origins
 		for (j = 0; j < model->mu.at(body_id).size(); j++) {
 			unsigned int child_id = model->mu[body_id][j];
-			Vector3d child_translation = model->X_T[child_id].get_translation();
+			Vector3d child_translation = get_translation(model->X_T[child_id]);
 
 			int k;
 			for (k = 0; k < 3; k++) {
 	glEnd();
 	glEnable(GL_LIGHTING);
 
+	glEnable (GL_DEPTH_TEST);
+
 	unsigned int i;
 	for (i = 1; i < model->q.size(); i++) {
 		Matrix3d rotation = model->GetBodyWorldOrientation(i);
 		Vector3d translation = model->GetBodyOrigin(i);
 
+		VisualizationPrimitiveList primitive_list = model->GetBodyVisualizationPrimitiveList(i);
+
 		std::ostringstream model_X;
 		model_X << model->X_base[i];
 
 		Vector3d target_direction (joint_axis[0], joint_axis[1], joint_axis[2]);
 
 		target_direction.normalize();
-		float rot_angle = acos (cml::dot(target_direction, Vector3d (0., 0., 1.)));
+		float rot_angle = acos (target_direction.dot(Vector3d (0., 0., 1.)));
 		rot_angle = rot_angle;
 		if (fabs(rot_angle) > 0.0001) {
-			Vector3d rotation_axis = cml::cross (target_direction, Vector3d (0., 0., 1.));
-			glRotatef (rot_angle * 180./M_PI, rotation_axis[0], rotation_axis[1], rotation_axis[2]);
+			Vector3d rotation_axis = target_direction.cross(Vector3d (0., 0., 1.));
+			glRotatef (rot_angle * 180./M_PI,
+					rotation_axis[0],
+					rotation_axis[1],
+					rotation_axis[2]
+					);
 		}
 		glColor3f (1., 0., 0.);
 		glScalef (0.07, 0.07, 0.2);
 		glEnd();
 		glEnable(GL_LIGHTING);
 
+		// Draw the body
+		if (primitive_list.size() > 0) {
+			VisualizationPrimitiveList::iterator iter = primitive_list.begin();
 
-		//! \todo the body box is not well centered!
-		// Draw the body
-		if (model->mBodies[i].mMass != 0) {
-			Vector3d body_center, body_dimensions;
-			compute_body_center_and_dimensions (model, i, body_center, body_dimensions);
+			while (iter != primitive_list.end()) {
+				Vector3d body_center, body_dimensions;
+				Visualization::Primitive *primitive = &(*iter);
 
-			glPushMatrix();
-			glTranslated (
-					body_center[0],
-					body_center[1],
-					body_center[2]
-					);
-			glScalef (
-					body_dimensions[0] * 0.5,
-					body_dimensions[1] * 0.5,
-					body_dimensions[2] * 0.5
-					);
+				if (primitive->type == Visualization::PrimitiveTypeBox) {
+					Vector3d box_size = primitive->max - primitive->min;
+					glPushMatrix();
+					glTranslated (
+							primitive->min[0] + box_size[0] * 0.5,
+							primitive->min[1] + box_size[1] * 0.5,
+							primitive->min[2] + box_size[2] * 0.5
+							);
+					glScaled (
+							box_size[0] * 0.5,
+							box_size[1] * 0.5,
+							box_size[2] * 0.5
+							);
 
-			glEnable(GL_COLOR_MATERIAL);
-			glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
-			glColor3f (0., 0.9, 0.1);
-			glprimitives_cube();
-			glColor3f (1.0f, 1.0f, 1.0f);
-			glDisable(GL_COLOR_MATERIAL);
+					glEnable(GL_COLOR_MATERIAL);
+					glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
+					glColor3f (
+							primitive->color[0],
+							primitive->color[1],
+							primitive->color[2]
+							);
+					glprimitives_cube();
+					glColor3f (1.0f, 1.0f, 1.0f);
+					glDisable(GL_COLOR_MATERIAL);
 
-			glPopMatrix();
+					glPopMatrix();
+				} else if (primitive->type == Visualization::PrimitiveTypeSphere) {
+					glPushMatrix();
+					glTranslated (
+							primitive->center[0],
+							primitive->center[1],
+							primitive->center[2]
+							);
+					glScaled (
+							primitive->radius,
+							primitive->radius,
+							primitive->radius
+							);
 
+					glEnable(GL_COLOR_MATERIAL);
+					glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
+					glColor3f (
+							primitive->color[0],
+							primitive->color[1],
+							primitive->color[2]
+							);
+					glprimitives_sphere();
+					glColor3f (1.0f, 1.0f, 1.0f);
+					glDisable(GL_COLOR_MATERIAL);
+
+					glPopMatrix();
+				}
+
+				iter ++;
+			}
 			// draw the COM as a small cube of red color (we ignore the depth
 			// buffer for better visibility 
 			glTranslated (
 			glprimitives_cube();
 			glColor3f (1.0f, 1.0f, 1.0f);
 			glDisable(GL_COLOR_MATERIAL);
+
 			glEnable (GL_DEPTH_TEST);
 		}
 		glPopMatrix();

gui/src/modelstate.cc

 #include "Contacts.h"
 #include "Dynamics.h"
 #include "Kinematics.h"
+#include "Visualization.h"
 
 using namespace std;
 using namespace RigidBodyDynamics;
 using namespace SpatialAlgebra;
+using namespace Experimental;
 
 static Model* model = NULL;
 
 Joint joint_base_rot_z, joint_base_rot_y, joint_base_rot_x,
 	joint_child_rot_z, joint_child_rot_y, joint_child_rot_x;
 
-cmlVector Q;
-cmlVector QDot;
-cmlVector QDDot;
-cmlVector Tau;
+VectorNd Q;
+VectorNd QDot;
+VectorNd QDDot;
+VectorNd Tau;
 
 unsigned int contact_body_id;
 Vector3d contact_point;
 
 std::vector<ContactInfo> contact_data;
 
-typedef cmlVector (rhs_func) (double, const cmlVector&);
+typedef VectorNd (rhs_func) (double, const VectorNd&);
 
-cmlVector rk45_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double error) {
-	cmlVector k1 (y0.size());
-	cmlVector k2 (y0.size());
-	cmlVector k3 (y0.size());
-	cmlVector k4 (y0.size());
-	cmlVector k5 (y0.size());
-	cmlVector k6 (y0.size());
-	cmlVector rk4 (y0.size());
-	cmlVector rk5 (y0.size());
+VectorNd rk45_integrator (double t0, double tf, VectorNd &y0, rhs_func func, double error) {
+	VectorNd k1 (y0.size());
+	VectorNd k2 (y0.size());
+	VectorNd k3 (y0.size());
+	VectorNd k4 (y0.size());
+	VectorNd k5 (y0.size());
+	VectorNd k6 (y0.size());
+	VectorNd rk4 (y0.size());
+	VectorNd rk5 (y0.size());
 
 	double s = 1.;
 	double t = t0;
 	double h0 = tf - t0;
 	double h = h0;
 	double h_min = 1.0e5;
-	cmlVector h_min_y (y0);
-	cmlVector h_min_rk5 (y0);
-	cmlVector y (y0);
+	VectorNd h_min_y (y0);
+	VectorNd h_min_rk5 (y0);
+	VectorNd y (y0);
 	int stepcount = 0;
 
 	while (t < tf) {
 		rk4 = y + 25./216. * k1 + 1408./2565. * k3 + 2197./4104. * k4 - 1./5. * k5;
 		rk5 = y + 16./135. * k1 + 6656./12825. * k3 + 28561./56430. * k4 - 9./50. * k5 + 2./55. * k6;
 
-		double error_est = cml::length(rk5 - rk4);
+		double error_est = (rk5 - rk4).norm();
 
 //		cout << "error estimate = " << scientific << error_est << endl;
 
 //	return y0 + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6.; 
 }
 
-cmlVector rk4_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double stepsize) {
-	cmlVector k1 (y0.size());
-	cmlVector k2 (y0.size());
-	cmlVector k3 (y0.size());
-	cmlVector k4 (y0.size());
-	cmlVector y (y0);
+VectorNd rk4_integrator (double t0, double tf, VectorNd &y0, rhs_func func, double stepsize) {
+	VectorNd k1 (y0.size());
+	VectorNd k2 (y0.size());
+	VectorNd k3 (y0.size());
+	VectorNd k4 (y0.size());
+	VectorNd y (y0);
 
 	double t = t0;
 	double h = stepsize;
 	return y;
 }
 
+VectorNd euler_integrator (double t0, double tf, VectorNd &y0, rhs_func func, double stepsize) {
+	VectorNd y (y0);
+
+	VectorNd ydot (y0);
+	ydot = func (tf, y);
+
+	y = y0 + (tf - t0) * ydot;
+
+	return y;
+}
+
 void model_init () {
 	model = new Model;
 	model->Init();
 
-	model->gravity.set (0., -2.81, 0.);
+	model->gravity = Vector3d (0., -9.81, 0.);
 
 	// base body
 	Body base (
 			Vector3d (1., 1., 1.)
 			);
 
-	model->SetFloatingBaseBody(base);
+	unsigned int base_body_id = model->SetFloatingBaseBody(base);
 
-	Q = cmlVector(model->dof_count);
-	QDot = cmlVector(model->dof_count);
-	QDDot = cmlVector(model->dof_count);
-	Tau = cmlVector(model->dof_count);
+	Q = VectorNd::Constant (model->dof_count, 0.);
+	QDot = VectorNd::Constant (model->dof_count, 0.);
+	QDDot = VectorNd::Constant (model->dof_count, 0.);
+	Tau = VectorNd::Constant (model->dof_count, 0.);
 
-	Q.zero();
-	QDot.zero();
-	QDDot.zero();
-	Tau.zero();
+	model->AddBodyVisualizationPrimitive (
+			base_body_id,
+			Visualization::PrimitiveSphere (
+				Vector3d (0.7, 0.9, 0.7),
+				Vector3d (0., 0., 0.),
+				1.	
+				)
+			);
 
-	contact_body_id = child_rot_x_id;
-	contact_point.set (0., -1., 0.);
-	contact_normal.set (0., 1., 0.);
+	model->AddBodyVisualizationPrimitive (
+			base_body_id,
+			Visualization::PrimitiveSphere (
+				Vector3d (0.7, 0.9, 0.7),
+				Vector3d (1., 0., 0.),
+				1.	
+				)
+			);
+
+
+	contact_body_id = base_body_id;
+	contact_point = Vector3d (0., -1., 0.);
+	contact_normal = Vector3d (0., 1., 0.);
 
 	Q[1] = 1.;
 
 	// at (0, 0, 0). There it should have a negative unit rotation around the
 	// Z-axis (i.e. rolling along the X axis). The spatial velocity of the
 	// body at the contact point is therefore (0, 0, -1, 0, 0, 0).
-	SpatialVector velocity_ground (0., 0., -1., -1., 0., 0.);
+	SpatialVector velocity_ground (0., 0., -1., 0., 0., 0.);
 
 	// This has now to be transformed to body coordinates.
 	SpatialVector velocity_body = Xtrans (Vector3d (0., 1., 0.)) * velocity_ground;
 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 }
 
-cmlVector rhs_contact (double t, const cmlVector &y) {
+VectorNd rhs_contact (double t, const VectorNd &y) {
 	unsigned int i;
 	unsigned int size = Q.size();
 
-	cmlVector q (size);
-	cmlVector qdot (size);
-	cmlVector qddot (size);
+	VectorNd q (size);
+	VectorNd qdot (size);
+	VectorNd qddot (size);
 
 	std::vector<ContactInfo> contact_data;
-	contact_point.set (Q[0], -1., Q[2]);
+	contact_point = model->CalcBaseToBodyCoordinates (contact_body_id, Vector3d (Q[0], 0., Q[2]));
+	
+	Vector3d contact_point_world;
+	contact_point_world = model->CalcBodyToBaseCoordinates(contact_body_id, contact_point);
 
-//	cout << "Q = " << Q << " CP = " << contact_point << endl;
+	cout << "Q = " << Q << "\tCP = " << contact_point_world;
 
 	contact_data.push_back(ContactInfo (contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
 	contact_data.push_back(ContactInfo (contact_body_id, contact_point, Vector3d (0., 1., 0.), 1.));
 
 	{
 		_NoLogging nolog;
-		ForwardDynamicsContacts (*model, q, qdot, Tau, contact_data, qddot);
+		ForwardDynamicsContactsLagrangian (*model, q, qdot, Tau, contact_data, qddot);
+	}
+	cout << "\tqdd = " << qddot ;
+
+	Vector3d contact_point_world_vel;
+	contact_point_world_vel = CalcPointVelocity (*model, q, qdot, contact_body_id, contact_point);
+	cout << "\tCPvel = " << contact_point_world_vel;
+
+	Vector3d contact_point_world_acc;
+	contact_point_world_acc = CalcPointAcceleration (*model, q, qdot, qddot, contact_body_id, contact_point);
+	cout << "\tCPacc = " << contact_point_world_acc;
+
+	cout << "\tforce = " << contact_data[0].force << ", " << contact_data[1].force << ", " << contact_data[2].force << endl;
+
+	VectorNd res (size * 2);
+	for (i = 0; i < size; i++) {
+		res[i] = qdot[i];
+		res[i + size] = qddot[i];
 	}
 
-	cmlVector res (size * 2);
+//	cout << "     y = " << y << "    " << "res = " << res << endl;
+
+	cout << endl;
+
+	return res;
+}
+
+VectorNd rhs_normal (double t, const VectorNd &y) {
+	unsigned int i;
+	unsigned int size = Q.size();
+
+	VectorNd q (size);
+	VectorNd qdot (size);
+	VectorNd qddot (size);
+
+	for (i = 0; i < size; i++) {
+		q[i] = y[i];
+		qdot[i] = y[i + size];
+	}
+
+	ForwardDynamics (*model, q, qdot, Tau, qddot);
+	contact_point = model->CalcBaseToBodyCoordinates (contact_body_id, Vector3d (Q[0], 0., Q[2]));
+
+	Vector3d contact_point_world;
+	contact_point_world = model->CalcBodyToBaseCoordinates(contact_body_id, contact_point);
+
+	cout << "Q = " << Q << "\tCP = " << contact_point_world;
+
+	Vector3d contact_point_world_vel;
+	contact_point_world_vel = CalcPointVelocity (*model, q, qdot, contact_body_id, contact_point);
+	cout << "\tCPvel = " << contact_point_world_vel;
+
+	Vector3d contact_point_world_acc;
+	contact_point_world_acc = CalcPointAcceleration (*model, q, qdot, qddot, contact_body_id, contact_point);
+	cout << "\tCPacc = " << contact_point_world_acc << endl;
+
+	assert (0);
+
+	VectorNd res (size * 2);
 	for (i = 0; i < size; i++) {
 		res[i] = qdot[i];
 		res[i + size] = qddot[i];
 	return res;
 }
 
-cmlVector rhs_normal (double t, const cmlVector &y) {
-	unsigned int i;
-	unsigned int size = Q.size();
-
-	cmlVector q (size);
-	cmlVector qdot (size);
-	cmlVector qddot (size);
-
-	for (i = 0; i < size; i++) {
-		q[i] = y[i];
-		qdot[i] = y[i + size];
-	}
-
-	ForwardDynamics (*model, q, qdot, Tau, qddot);
-
-	cmlVector res (size * 2);
-	for (i = 0; i < size; i++) {
-		res[i] = qdot[i];
-		res[i + size] = qddot[i];
-	}
-
-	return res;
-}
-
-void model_update_contact (double delta_time) {
+void model_update (double delta_time) {
 	unsigned int size = Q.size();
 	unsigned int i;
 	
-	cmlVector y (size * 2);
+	VectorNd y (size * 2);
 
 	for (i = 0; i < size; i++) {
 		y[i] = Q[i];
 		y[i + size] = QDot[i];
 	}
 
-	cmlVector ynew (size * 2);
-	ynew = rk4_integrator (0., delta_time, y, rhs_contact, delta_time);
-
-	for (i = 0; i < size; i++) {
-		Q[i] += ynew[i];
-		QDot[i] += ynew[i + size];
-	}
-
-	Vector3d point_accel;
-	CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point, point_accel);
-
-	Vector3d point_velocity;
-	CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, point_velocity);
-
-	Vector3d point_pos = model->GetBodyPointPosition (contact_body_id, contact_point);
-
-//	qDebug() << "accel =" << cml::dot(point_accel, contact_normal) 
-//		<< " point_accel =" << point_accel[0] << point_accel[1] << point_accel[2]
-//		<< " point_veloc =" << point_velocity[0] << point_velocity[1] << point_velocity[2];
-}
-
-void model_update (double delta_time) {
-	unsigned int size = Q.size();
-	unsigned int i;
-	
-	cmlVector y (size * 2);
-
-	for (i = 0; i < size; i++) {
-		y[i] = Q[i];
-		y[i + size] = QDot[i];
-	}
-
-	cmlVector ynew (size * 2);
+	VectorNd ynew (size * 2);
 //	delta_time = 0.02;
 //	ynew = rk45_integrator (0., delta_time, y, rhs_normal, 1.0e-3);
-	ynew = rk4_integrator (0., delta_time, y, rhs_contact, 5.0e-2);
+//	ynew = rk4_integrator (0., delta_time, y, rhs_contact, 5.0e-2);
+//	ynew = euler_integrator (0., delta_time, y, rhs_normal, 1.0e-3);
+	ynew = euler_integrator (0., delta_time, y, rhs_contact, 1.0e-3);
+
+//	cout << "        ynew = " << ynew << endl;
 
 	for (i = 0; i < size; i++) {
 		Q[i] = ynew[i];
 		QDot[i] = ynew[i + size];
 	}
-
-	cout << "y = " << ynew << endl;
 }
 
 Model* model_get() {

gui/src/qtmain.cc

 #include <QApplication>
 
-#include "abaQtApp.h"
+#include "rbdlQtApp.h"
 #include "glwidget.h"
 
 #include <iostream>
 int main(int argc, char *argv[])
 {
 	QApplication app(argc, argv);
-	abaQtApp *main_window = new abaQtApp;
+	rbdlQtApp *main_window = new rbdlQtApp;
 
 	main_window->show();
 	return app.exec();

gui/src/rbdlQtApp.cc

+#include <QtGui> 
+
+#include "glwidget.h" 
+#include "rbdlQtApp.h"
+
+#include <assert.h>
+#include <iostream>
+
+using namespace std;
+
+rbdlQtApp::rbdlQtApp(QWidget *parent)
+{
+	timer = new QTimer (this);
+
+	setupUi(this); // this sets up GUI
+
+	timer->setSingleShot(false);
+	timer->start(20);
+
+	// the timer is used to continously redraw the OpenGL widget
+	connect (timer, SIGNAL(timeout()), glWidget, SLOT(updateGL()));
+	
+	connect (actionQuit, SIGNAL( triggered() ), qApp, SLOT( quit() ));
+}

gui/src/rbdlQtApp.h

+#ifndef RBDLMAINWINDOW_H
+#define RBDLMAINWINDOW_H
+
+#include <QTimer>
+#include "ui_rbdlMainWindow.h"
+
+class rbdlQtApp : public QMainWindow, public Ui::rbdlMainWindow
+{
+    Q_OBJECT
+ 
+public:
+    rbdlQtApp(QWidget *parent = 0);
+
+protected:
+		QTimer *timer;
+};
+ 
+#endif

gui/ui/abaMainWindow.ui

-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>abaMainWindow</class>
- <widget class="QMainWindow" name="abaMainWindow">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>587</width>
-    <height>599</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>abaGUI</string>
-  </property>
-  <property name="statusTip">
-   <string/>
-  </property>
-  <widget class="QWidget" name="centralwidget">
-   <layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0">
-    <property name="margin">
-     <number>0</number>
-    </property>
-    <item>
-     <widget class="GLWidget" name="glWidget" native="true">
-      <property name="enabled">
-       <bool>true</bool>
-      </property>
-      <property name="minimumSize">
-       <size>
-        <width>300</width>
-        <height>300</height>
-       </size>
-      </property>
-     </widget>
-    </item>
-    <item>
-     <layout class="QVBoxLayout" name="verticalLayout" stretch=""/>
-    </item>
-   </layout>
-  </widget>
-  <widget class="QMenuBar" name="menubar">
-   <property name="geometry">
-    <rect>
-     <x>0</x>
-     <y>0</y>
-     <width>587</width>
-     <height>25</height>
-    </rect>
-   </property>
-   <widget class="QMenu" name="menuFile">
-    <property name="title">
-     <string>File</string>
-    </property>
-    <addaction name="actionQuit"/>
-   </widget>
-   <addaction name="menuFile"/>