Commits

Martin Felis  committed b1a0354 Draft Merge

trying to incorporate ginac to rbdl

  • Participants
  • Parent commits b80c204, 245d95f

Comments (0)

Files changed (62)

File CMakeLists.txt

-PROJECT (ABA)
+PROJECT (RBDL)
 
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 		LINKER_LANGUAGE CXX
 	)
 
+# 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_SUBDIRECTORY ( tests )
 ADD_SUBDIRECTORY ( gui )
 
-ADD_LIBRARY ( aba 
+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} )
+ADD_LIBRARY ( rbdl-static STATIC ${RBDL_SOURCES} )
+
+SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES OUTPUT_NAME "rbdl")
+SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib")
+
+OPTION (ENABLE_LOGGING "Enable logging (warning: major impact on performance!)" OFF)
 
 IF ( ENABLE_LOGGING )
 	SET ( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_LOGGING" )
 ENDIF ( ENABLE_LOGGING )
+
+INSTALL (TARGETS rbdl rbdl-static
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	)
+
+FILE ( GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/src/*.h" )
+INSTALL ( FILES ${headers} DESTINATION include/rbdl )
+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 doc/Mainpage.h

 /** \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.
+ * Articulated Body Algorithm (ABA) from Roy Featherstone. The code is
+ * written by Martin Felis <martin@silef.de> and heavily inspired by the
+ * pseudo code of the book "Rigid Body Dynamics Algorithms" of
+ * Featherstone.
  *
  * The library uses the configurable math library (which can be found here:
  * <a href="http://www.cmldev.net">http://www.cmldev.net</a>).
  * \code
  *	#include <iostream>
  *	
- *	#include "Model.h"
- *	#include "Dynamics.h"
- *	
- *	#include "mathutils.h" // required for Xtrans
+ *	#include "rbdl.h"
+ *
+ *	using namespace RigidBodyDynamics;
  *	
  *	int main (int argc, char* argv[]) {
  *		Model* model = NULL;
  *	
  *		model = new Model();
  *		model->Init();
+ *
+ *		model->gravity.set (0., -9.81, 0.);
  *	
  *		body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.));
  *		joint_a = Joint(
  *		cmlVector QDot(3);
  *		cmlVector QDDot(3);
  *		cmlVector Tau(3);
+ *
+ *		Q.zero();
+ *		QDot.zero();
+ *		QDDot.zero();
+ *		Tau.zero();
  *	
  *	 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
  *	
  *		std::cout << QDDot << 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> -laba -L<path to libaba.a> -o example
+ * 	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.
+ *
  * \section ModelConstruction Construction of Models
  *
  * The construction of models makes use of carefully designed constructors
  * 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().
+ *
+ * \todo [high] check impulse computation
+ * \todo [med] add specification for the visualization to the model
+ * \todo [med] get rid of the std::vector<> values in Model
+ * \todo [low] use cml for the SpatialAlgebra quantities
+ * \todo [low] incorporate GiNaC (http://www.ginac.de) to generate code
+ * \todo [low] serialization of the model?
  */

File 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}

File 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}
+	)

File example/FindRBDL.cmake

+# - Try to find RBDL
+#
+#
+
+SET (RBDL_FOUND FALSE)
+
+FIND_PATH (RBDL_INCLUDE_DIR rbdl.h
+	/usr/include/
+	/usr/include/rbdl/
+	/usr/local/include/
+	/usr/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{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
+	)

File 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.set (0., -9.81, 0.);
+
 	body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.));
 		joint_a = Joint(
 		JointTypeRevolute,
 	cmlVector QDDot(3);
 	cmlVector Tau(3);
 
+	Q.zero();
+	QDot.zero();
+	QDDot.zero();
+	Tau.zero();
+
  	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 
 	std::cout << QDDot << std::endl;
 
+	delete model;
+
  	return 0;
 }
 

File 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
 	)

File 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() ));
-}

File 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

File 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);
+	*/
+}

File gui/src/glprimitives.h

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

File 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);
 
+	glEnable (GL_COLOR_MATERIAL);
+	glDisable(GL_LIGHTING);
+
+	drawGrid();
+	glDisable (GL_COLOR_MATERIAL);
+	glEnable(GL_LIGHTING);
+
 	draw_model (model_get());
 }
 
 		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;

File gui/src/glwidget.h

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

File gui/src/modeldrawing.cc

 using namespace RigidBodyDynamics;
 
 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
 	glVertex3f (0., 0., 1.);
 	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);
-			
-			std::ostringstream model_X;
-			model_X << model->X_base[i];
+		Matrix3d rotation = model->GetBodyWorldOrientation(i);
+		Vector3d translation = model->GetBodyOrigin(i);
+		VisualizationPrimitive* body_visualization = model->GetBodyVisualizationPrimitive(i);
 
-			glPushMatrix();
-				GLfloat orientation[16];
-				int j,k;
-				for (j = 0; j < 4; j++) {
-					for (k = 0; k < 4; k++) {
-						if (j < 3 && k < 3)
-							orientation[j * 4 + k] = rotation(j,k);
-						else 
-							orientation[j * 4 + k] = 0.;
-					} 
-				}
-				orientation[3 * 4 + 3] = 1.;
+		std::ostringstream model_X;
+		model_X << model->X_base[i];
 
-				// draw an orientation system of the current body
-				glTranslated (translation[0], translation[1], translation[2]);
-				glMultMatrixf(orientation);
+		glPushMatrix();
+		GLfloat orientation[16];
+		int j,k;
+		for (j = 0; j < 4; j++) {
+			for (k = 0; k < 4; k++) {
+				if (j < 3 && k < 3)
+					orientation[j * 4 + k] = rotation(j,k);
+				else 
+					orientation[j * 4 + k] = 0.;
+			} 
+		}
+		orientation[3 * 4 + 3] = 1.;
 
-				// Draw the joint
-				/// \todo add visualizations for prismatic joints
+		// draw an orientation system of the current body
+		glTranslated (translation[0], translation[1], translation[2]);
+		glMultMatrixf(orientation);
+
+		// Draw the joint
+		/// \todo add visualizations for prismatic joints
+		glPushMatrix();
+		// align the orientation such that the Z-axis aligns with the joint axis
+		SpatialAlgebra::SpatialVector joint_axis (model->S.at(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.)));
+		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]
+					);
+		}
+		glColor3f (1., 0., 0.);
+		glScalef (0.07, 0.07, 0.2);
+		glprimitives_torus();
+		glPopMatrix ();
+
+		// Draw a small coordinate system
+		glDisable(GL_LIGHTING);
+		glBegin (GL_LINES);
+		glColor3f (0.8, 0.2, 0.2);
+		glVertex3f (0., 0., 0.);
+		glVertex3f (1., 0., 0.);
+		glColor3f (0.2, 0.8, 0.2);
+		glVertex3f (0., 0., 0.);
+		glVertex3f (0., 1., 0.);
+		glColor3f (0.2, 0.2, 0.8);
+		glVertex3f (0., 0., 0.);
+		glVertex3f (0., 0., 1.);
+		glEnd();
+		glEnable(GL_LIGHTING);
+
+		// Draw the body
+		if (body_visualization) {
+			Vector3d body_center, body_dimensions;
+
+			if (body_visualization->type == VisualizationPrimitiveBox) {
+				Vector3d box_size = body_visualization->max - body_visualization->min;
 				glPushMatrix();
-					// align the orientation such that the Z-axis aligns with the joint axis
-					SpatialAlgebra::SpatialVector joint_axis (model->S.at(i));
-					Vector3d target_direction (joint_axis[0], joint_axis[1], joint_axis[2]);
+				glTranslated (
+						body_visualization->min[0] + box_size[0] * 0.5,
+						body_visualization->min[1] + box_size[1] * 0.5,
+						body_visualization->min[2] + box_size[2] * 0.5
+						);
+				glScaled (
+						box_size[0] * 0.5,
+						box_size[1] * 0.5,
+						box_size[2] * 0.5
+						);
 
-					target_direction.normalize();
-					float rot_angle = acos (cml::dot(target_direction, 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]);
-					}
-					glColor3f (1., 0., 0.);
-					glScalef (0.07, 0.07, 0.2);
-					glprimitives_torus();
-				glPopMatrix ();
+				glEnable(GL_COLOR_MATERIAL);
+				glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
+				glColor3f (
+						body_visualization->color[0],
+						body_visualization->color[1],
+						body_visualization->color[2]
+						);
+				glprimitives_cube();
+				glColor3f (1.0f, 1.0f, 1.0f);
+				glDisable(GL_COLOR_MATERIAL);
 
-				// Draw a small coordinate system
-					glDisable(GL_LIGHTING);
-					glBegin (GL_LINES);
-					glColor3f (0.8, 0.2, 0.2);
-					glVertex3f (0., 0., 0.);
-					glVertex3f (1., 0., 0.);
-					glColor3f (0.2, 0.8, 0.2);
-					glVertex3f (0., 0., 0.);
-					glVertex3f (0., 1., 0.);
-					glColor3f (0.2, 0.2, 0.8);
-					glVertex3f (0., 0., 0.);
-					glVertex3f (0., 0., 1.);
-					glEnd();
-					glEnable(GL_LIGHTING);
+				glPopMatrix();
+			} else if (body_visualization->type == VisualizationPrimitiveSphere) {
+				glPushMatrix();
+				glTranslated (
+						body_visualization->center[0],
+						body_visualization->center[1],
+						body_visualization->center[2]
+						);
+				glScaled (
+						body_visualization->radius,
+						body_visualization->radius,
+						body_visualization->radius
+						);
 
+				glEnable(GL_COLOR_MATERIAL);
+				glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
+				glColor3f (
+						body_visualization->color[0],
+						body_visualization->color[1],
+						body_visualization->color[2]
+						);
+				glprimitives_sphere();
+				glColor3f (1.0f, 1.0f, 1.0f);
+				glDisable(GL_COLOR_MATERIAL);
 
-				//! \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);
+				glPopMatrix();
+			}
 
-					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
-							);
+			// draw the COM as a small cube of red color (we ignore the depth
+			// buffer for better visibility 
+			glTranslated (
+					model->mBodies[i].mCenterOfMass[0],
+					model->mBodies[i].mCenterOfMass[1],
+					model->mBodies[i].mCenterOfMass[2]
+					); 
 
-					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);
+			glDisable (GL_DEPTH_TEST);
+			glEnable(GL_COLOR_MATERIAL);
+			glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
+			glColor3f (0.9, 0., 0.1);
+			glScalef (0.025, 0.025, 0.025);
+			glprimitives_cube();
+			glColor3f (1.0f, 1.0f, 1.0f);
+			glDisable(GL_COLOR_MATERIAL);
 
-					glPopMatrix();
-
-					// draw the COM as a small cube of red color (we ignore the depth
-					// buffer for better visibility 
-					glTranslated (
-							model->mBodies[i].mCenterOfMass[0],
-							model->mBodies[i].mCenterOfMass[1],
-							model->mBodies[i].mCenterOfMass[2]
-							); 
-
-					glDisable (GL_DEPTH_TEST);
-					glEnable(GL_COLOR_MATERIAL);
-					glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
-					glColor3f (0.9, 0., 0.1);
-					glScalef (0.025, 0.025, 0.025);
-					glprimitives_cube();
-					glColor3f (1.0f, 1.0f, 1.0f);
-					glDisable(GL_COLOR_MATERIAL);
-					glEnable (GL_DEPTH_TEST);
-				}
-			glPopMatrix();
-			}
+			glEnable (GL_DEPTH_TEST);
+		}
+		glPopMatrix();
 	}
 }

File gui/src/modelstate.cc

 
 using namespace std;
 using namespace RigidBodyDynamics;
+using namespace SpatialAlgebra;
 
 static Model* model = NULL;
 
 Vector3d contact_point;
 Vector3d contact_normal;
 
+std::vector<ContactInfo> contact_data;
+
 typedef cmlVector (rhs_func) (double, const cmlVector&);
 
 cmlVector rk45_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double error) {
 	if (h > tf - t0)
 		h = tf - t0;
 
-	cout << "t0 = " << t0 << " tf = " << tf << " h = " << h << endl;
+//	cout << "t0 = " << t0 << " tf = " << tf << " h = " << h << endl;
 	while (t < tf) {
 		k1 = func (t, y);
 		k2 = func (t + 0.5 * h, y + h * 0.5 * k1);
 		}
 
 		stepcount ++;
-		cout << stepcount << " " << tf - t << endl;
+//		cout << stepcount << " " << tf - t << endl;
 	}
 
-	cout << "stepcount = " << stepcount << endl;
+//	cout << "stepcount = " << stepcount << endl;
+	return y;
+}
+
+cmlVector euler_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double stepsize) {
+	cmlVector y (y0);
+
+	cmlVector ydot (y0);
+	ydot = func (tf, y);
+
+	y = y0 + (tf - t0) * ydot;
+
 	return y;
 }
 
 	model = new Model;
 	model->Init();
 
-	model->gravity.set (0., -2.81, 0.);
+	model->gravity.set (0., 0., 0.);
 
-	/* Basically a model like this, where X are the Center of Masses
-	 * and the CoM of the last (3rd) body comes out of the Y=X=0 plane.
-	 *
-	 *                X
-	 *                *
-	 *              _/
-	 *            _/  (-Z)
-	 *      Z    /
-	 *      *---* 
-	 *      |
-	 *      |
-	 *  Z   |
-	 *  O---*
-	 *      Y
-	 */
-
-	/*
-	body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
-	joint_a = Joint(
-			JointTypeRevolute,
-			Vector3d (0., 0., 1.)
+	// base body
+	Body base (
+			1.,
+			Vector3d (0., 0., 0.),
+			Vector3d (1., 1., 1.)
 			);
 
-	body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a);
+	unsigned int base_body_id = model->SetFloatingBaseBody(base);
 
-	body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));
-	joint_b = Joint (
-			JointTypeRevolute,
-			Vector3d (0., 1., 0.)
-			);
-
-	body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b);
-
-	body_c = Body (1., Vector3d (0., 0., 1.), 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);
-	*/
-
-	// base body
-	base_rot_z = Body (
-			0.,
-			Vector3d (0., 0., 0.),
-			Vector3d (0., 0., 0.)
-			);
-	joint_base_rot_z = Joint (
-			JointTypeRevolute,
-			Vector3d (0., 0., 1.)
-			);
-	base_rot_z_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_z, base_rot_z);
-
-	base_rot_y = Body (
-			0.,
-			Vector3d (0., 0., 0.),
-			Vector3d (0., 0., 0.)
-			);
-	joint_base_rot_y = Joint (
-			JointTypeRevolute,
-			Vector3d (0., 1., 0.)
-			);
-	base_rot_y_id = model->AddBody (base_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_y, base_rot_y);
-
-	base_rot_x = Body (
-			1.,
-			Vector3d (0., 1., 0.),
-			Vector3d (1., 1., 1.)
-			);
-	joint_base_rot_x = Joint (
-			JointTypeRevolute,
-			Vector3d (1., 0., 0.)
-			);
-	base_rot_x_id = model->AddBody (base_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_x, base_rot_x);
-
-	// child body
-	child_rot_z = Body (
-			0.,
-			Vector3d (0., 0., 0.),
-			Vector3d (0., 0., 0.)
-			);
-	joint_child_rot_z = Joint (
-			JointTypeRevolute,
-			Vector3d (0., 0., 1.)
-			);
-	child_rot_z_id = model->AddBody (base_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_rot_z, child_rot_z);
-
-	child_rot_y = Body (
-			0.,
-			Vector3d (0., 0., 0.),
-			Vector3d (0., 0., 0.)
-			);
-	joint_child_rot_y = Joint (
-			JointTypeRevolute,
-			Vector3d (0., 1., 0.)
-			);
-	child_rot_y_id = model->AddBody (child_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_y, child_rot_y);
-
-	child_rot_x = Body (
-			1.,
-			Vector3d (0., 1., 0.),
-			Vector3d (1., 1., 1.)
-			);
-	joint_child_rot_x = Joint (
-			JointTypeRevolute,
-			Vector3d (1., 0., 0.)
-			);
-	child_rot_x_id = model->AddBody (child_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_x, child_rot_x);
-
-	Q = cmlVector(model->mBodies.size() - 1);
-	QDot = cmlVector(model->mBodies.size() - 1);
-	QDDot = cmlVector(model->mBodies.size() - 1);
-	Tau = cmlVector(model->mBodies.size() - 1);
+	Q = cmlVector(model->dof_count);
+	QDot = cmlVector(model->dof_count);
+	QDDot = cmlVector(model->dof_count);
+	Tau = cmlVector(model->dof_count);
 
 	Q.zero();
 	QDot.zero();
 	QDDot.zero();
 	Tau.zero();
 
-	Q[0] = 0.1;
-	Q[1] = 0.1;
+	model->SetBodyVisualizationSphere(
+			base_body_id,
+			Vector3d (0.7, 0.9, 0.7),
+			Vector3d (0., 0., 0.),
+			1.	
+			);
 
-	contact_body_id = child_rot_x_id;
-	contact_point.set (1., 1., 0.);
+	contact_body_id = base_body_id;
+	contact_point.set (0., -1., 0.);
 	contact_normal.set (0., 1., 0.);
 
+	Q[1] = 1.;
+
+	// We want the body to rotate around its contact point which is located
+	// 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., 0., 0., 0.);
+
+	// This has now to be transformed to body coordinates.
+	SpatialVector velocity_body = Xtrans (Vector3d (0., 1., 0.)) * velocity_ground;
+
+	// This has now to be shuffled such that it complies with the ordering of
+	// the DoF in the generalized velocity vector.
+	QDot[0] = velocity_body[3];
+	QDot[1] = velocity_body[4];
+	QDot[2] = velocity_body[5];
+	QDot[3] = velocity_body[2];
+	QDot[4] = velocity_body[1];
+	QDot[5] = velocity_body[0];
+
+	cout << "velocity_body = " << velocity_body << std::endl;
+	cout << "Q = " << Q << std::endl;
+	cout << "QDot = " << QDot << std::endl;
+
 	// we call model_update once to update the internal variables for the
 	// state, etc.
 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 	cmlVector qddot (size);
 
 	std::vector<ContactInfo> contact_data;
+	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);
 
-	contact_data.push_back(ContactInfo (contact_body_id, contact_point, Vector3d (1., 0., 0.)));
-	contact_data.push_back(ContactInfo (contact_body_id, contact_point, Vector3d (0., 1., 0.)));
-	contact_data.push_back(ContactInfo (contact_body_id, contact_point, Vector3d (0., 0., 1.)));
+	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.), 0.));
+	contact_data.push_back(ContactInfo (contact_body_id, contact_point, Vector3d (0., 0., 1.), 0.));
 
 	for (i = 0; i < size; i++) {
 		q[i] = y[i];
 		qdot[i] = y[i + size];
 	}
 
-	ForwardDynamicsContacts (*model, q, qdot, Tau, contact_data, qddot);
+	{
+		_NoLogging nolog;
+		ForwardDynamicsContacts (*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;
 
 	cmlVector res (size * 2);
 	for (i = 0; i < size; i++) {
 		res[i + size] = qddot[i];
 	}
 
+//	cout << "     y = " << y << "    " << "res = " << res << endl;
+
+	cout << endl;
+
 	return res;
 }
 
 	}
 
 	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;
 
 	cmlVector res (size * 2);
 	for (i = 0; i < size; i++) {
 	return res;
 }
 
-void model_update_contact (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);
-	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) {
-//	model_update_contact (delta_time);
-//	return;
-
 	unsigned int size = Q.size();
 	unsigned int i;
 	
 	cmlVector 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_normal, 5.0e-1);
+//	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-6);
+
+//	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() {

File 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();

File 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() ));
+}

File 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

File 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"/>
-  </widget>
-  <widget class="QStatusBar" name="statusbar"/>
-  <action name="actionQuit">
-   <property name="text">
-    <string>Quit</string>
-   </property>
-  </action>
-  <action name="actionDrawJoints">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Draw Joints</string>
-   </property>
-   <property name="shortcut">
-    <string>J</string>
-   </property>
-  </action>
-  <action name="actionDrawSkeleton">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Draw Skeleton</string>
-   </property>
-   <property name="shortcut">
-    <string>S</string>
-   </property>
-  </action>
-  <action name="actionDrawMesh">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Draw Mesh</string>
-   </property>
-   <property name="shortcut">
-    <string>M</string>
-   </property>
-  </action>
-  <action name="actionDrawPoints">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Draw Points</string>
-   </property>
-   <property name="shortcut">
-    <string>P</string>
-   </property>
-  </action>
-  <action name="actionRender_Imageseries">
-   <property name="enabled">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Render Imageseries</string>
-   </property>
-  </action>
- </widget>
- <customwidgets>
-  <customwidget>
-   <class>GLWidget</class>
-   <extends>QWidget</extends>
-   <header location="global">glwidget.h</header>
-   <container>1</container>
-  </customwidget>
- </customwidgets>
- <resources/>
- <connections/>
-</ui>

File gui/ui/rbdlMainWindow.ui

+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>rbdlMainWindow</class>
+ <widget class="QMainWindow" name="rbdlMainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>587</width>
+    <height>599</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>rbdlGUI</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"/>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+  <action name="actionQuit">
+   <property name="text">
+    <string>Quit</string>
+   </property>
+  </action>
+  <action name="actionDrawJoints">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Draw Joints</string>
+   </property>
+   <property name="shortcut">
+    <string>J</string>
+   </property>
+  </action>
+  <action name="actionDrawSkeleton">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Draw Skeleton</string>
+   </property>
+   <property name="shortcut">
+    <string>S</string>
+   </property>
+  </action>
+  <action name="actionDrawMesh">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Draw Mesh</string>
+   </property>
+   <property name="shortcut">
+    <string>M</string>
+   </property>
+  </action>
+  <action name="actionDrawPoints">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Draw Points</string>
+   </property>
+   <property name="shortcut">
+    <string>P</string>
+   </property>
+  </action>
+  <action name="actionRender_Imageseries">
+   <property name="enabled">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Render Imageseries</string>
+   </property>
+  </action>
+ </widget>
+ <customwidgets>
+  <customwidget>
+   <class>GLWidget</class>
+   <extends>QWidget</extends>
+   <header location="global">glwidget.h</header>
+   <container>1</container>
+  </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>

File spatial_v1/FDf.m

 end
 
 IAfb = model.I{6}
+disp("vfb = ");
+disp(vfb);
+crf(vfb)
 pAfb = crf(vfb) * IAfb * vfb
 
-disp("v = ");
-disp(v);
+% disp("v = ");
+% disp(v);
 
 disp("IAfb = ");
 disp(IAfb);
-disp("IA = ");
-disp (IA);
+%disp("IA = ");
+%disp (IA);
 
 disp ("pAfb = ");
 disp (pAfb);
-disp("pA = ");
-disp (pA);
+%disp("pA = ");
+%disp (pA);
 
-disp("S = ");
-disp (S);
+%disp("S = ");
+%disp (S);
 % IA
 % pA
 
 
 afb = - IAfb \ pAfb;			% floating base accn without gravity
 
-disp("U =");
-disp(U);
-disp("d =");
-disp(d);
-disp("u =");
-disp(u);
+%disp("U =");
+%disp(U);
+%disp("d =");
+%disp(d);
+%disp("u =");
+%disp(u);
 
 disp(" --- third loop ---");
 
   a{i} = a{i} + S{i}*qdd(i);
 end
 
-disp("a = ");
-a
+% disp("a = ");
+% a
+% 
+% disp("Xup = ");
+% Xup
+% 
+% disp("v = ");
+% v
 
-disp("Xup = ");
-Xup
-
-disp("v = ");
-v
-
-accel = qdd
+% accel = qdd
 afb = Xfb \ afb + a_grav;		% true flt base accn in ref coords

File spatial_v1/customfloat.m

 function [afb, qdd] = customfloat()
 
-nb = 2;
+nb = 1;
 bf = 0;
 skew = 0;
 taper = 0;
 model.Xtree{i} = Xtrans([0 0 0]);
 model.jaxis{i} = [0 0 1]';
 mass = 1.;
-CoM = [1 0 0];
+CoM = [0 1 0];
 Icm = diag([1 1 1]);
 model.I{i} = mcI(mass, CoM, Icm);
 
-i = 2;
-model.parent(i) = 1;
-model.Xtree{i} = Xtrans([2 0 0]);
-model.jaxis{i} = [0 0 1]';
-mass = 1.;
-CoM = [1 0 0];
-Icm = diag([1 1 1]);
-model.I{i} = mcI(mass, CoM, Icm);
+% i = 2;
+% model.parent(i) = 1;
+% model.Xtree{i} = Xtrans([2 0 0]);
+% model.jaxis{i} = [0 0 1]';
+% mass = 1.;
+% CoM = [1 0 0];
+% Icm = diag([1 1 1]);
+% model.I{i} = mcI(mass, CoM, Icm);
 
 floatmodel = floatbase (model);
 
 v_B = zeros(6,1);
 
 tau(1) = 1;
-
+v_B = [0 0 -1 1 0 0]';
 [afb, qdd] = FDf (floatmodel, X_B, v_B, q, qd, tau, {}, [0; -9.81; 0]);
 

File spatial_v1/jcalc.m

-function  [Xj,S] = jcalc( pitch, q, axis )
+function  [Xj,S] = jcalc( pitch, q )
 
 % jcalc  Calculate joint transform and motion subspace.
 % [Xj,S]=jcalc(pitch,q) calculates the joint transform and motion subspace
 % (pitch==any other value) joint.  For revolute and helical joints, q is
 % the joint angle.  For prismatic joints, q is the linear displacement.
 
-if nargin < 3
-	axis = [0;0;1];
-end
-
-% type information:
-% 0 rotz
-% 1 roty
-% 2 rotx
-
 if pitch == 0				% revolute joint
-	if axis == [0;0;1]
-  	Xj = Xrotz(q);
-  	S = [0;0;1;0;0;0];
-	elseif axis == [0;1;0]
-  	Xj = Xroty(q);
-  	S = [0;1;0;0;0;0];
-	elseif axis == [1;0;0]
-  	Xj = Xrotx(q);
-  	S = [1;0;0;0;0;0];
-	else
-		disp("invalid joint axis!")
-		return
-	end
+  Xj = Xrotz(q);
+  S = [0;0;1;0;0;0];
 elseif pitch == inf			% prismatic joint
   Xj = Xtrans([0 0 q]);
   S = [0;0;0;0;0;1];
  */
 struct Body {
 	Body() :
+		mMass (1.),
+		mCenterOfMass (0., 0., 0.),
 		mSpatialInertia (
 				0., 0., 0., 0., 0., 0.,	
 				0., 0., 0., 0., 0., 0.,	
 				0., 0., 0., 0., 0., 0.,	
 				0., 0., 0., 0., 0., 0.,	
 				0., 0., 0., 0., 0., 0.
-				),
-		mCenterOfMass (0., 0., 0.),
-		mMass (1.) { };
+				)
+	{ };
 	Body(const Body &body) :
-		mSpatialInertia (body.mSpatialInertia),
+		mMass (body.mMass),
 		mCenterOfMass (body.mCenterOfMass),
-		mMass (body.mMass) {};
+		mSpatialInertia (body.mSpatialInertia)
+	{};
 	Body& operator= (const Body &body) {
 		if (this != &body) {
 			mSpatialInertia = body.mSpatialInertia;
 			mCenterOfMass = body.mCenterOfMass;
 			mMass = body.mMass;
 		}
+
+		return *this;
 	}
 	/** \brief Constructs a body out of the given parameters
 	 *
 
 	~Body() {};
 
+	/// \brief The mass of the body
+	double mMass;
+	/// \brief The position of the center of mass in body coordinates
+	Vector3d mCenterOfMass;
 	/// \brief The spatial inertia that contains both mass and inertia information
 	SpatialAlgebra::SpatialMatrix mSpatialInertia;
-	/// \brief The position of the center of mass in body coordinates
-	Vector3d mCenterOfMass;
-	/// \brief The mass of the body
-	double mMass;
 };
 
 }

File src/Contacts.h

 		body_id (0),
 		point (0., 0., 0.),
 		normal (0., 0., 0.),
-		acceleration (0.)
+		acceleration (0.),
+		force (0.)
 	{	}
 	ContactInfo (const ContactInfo &contact_info) :
 		body_id (contact_info.body_id),
 		point (contact_info.point),
 		normal (contact_info.normal),
-		acceleration (contact_info.acceleration)
+		acceleration (contact_info.acceleration),
+		force (contact_info.force)
 	{}
 	ContactInfo& operator= (const ContactInfo &contact_info) {
 		if (this != &contact_info) {
 			point = contact_info.point;
 			normal = contact_info.normal;
 			acceleration = contact_info.acceleration;
+			force = contact_info.force;
 		}
 
 		return *this;
 		body_id (body),
 		point (contact_point),
 		normal (contact_normal),
-		acceleration (0.)
+		acceleration (0.),
+		force (0.)
 	{	}
 
 	ContactInfo (unsigned int body, const Vector3d &contact_point, const Vector3d &contact_normal, const double accel):
 		body_id (body),
 		point (contact_point),
 		normal (contact_normal),
-		acceleration (accel)
+		acceleration (accel),
+		force (force)
 	{	}
 
 	/// \brief The id of the body of which the motion is constrained
 	Vector3d normal;
 	/// \brief Acceleration value of the constraint along the normal
 	double acceleration;
+	/// \brief Force acting along the normal
+	double force;
 };
 
 }

File src/Dynamics.cc

 #include "Model.h"
 #include "Contacts.h"
 #include "Dynamics.h"
-#include "Dynamics_stdvec.h"
+// #include "Dynamics_stdvec.h"
+#include "Dynamics_experimental.h"
 #include "Kinematics.h"
 
 using namespace SpatialAlgebra;
  * \param Q     state vector of the internal joints
  * \param QDot  velocity vector of the internal joints
  * \param Tau   actuations of the internal joints
- * \param QDDot accelerations of the internals joints (output)
- */
-void ForwardDynamics (
-		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
-		cmlVector &QDDot
-		) {
-	std::vector<double> Q_stdvec (Q.size());
-	std::vector<double> QDot_stdvec (QDot.size());
-	std::vector<double> QDDot_stdvec (QDDot.size());
-	std::vector<double> Tau_stdvec (Tau.size());
-
-	unsigned int i;
-	for (i = 0; i < Q.size(); i++)
-		Q_stdvec[i] = Q[i];
-
-	for (i = 0; i < QDot.size(); i++)
-		QDot_stdvec[i] = QDot[i];
-
-	for (i = 0; i < QDDot.size(); i++)
-		QDDot_stdvec[i] = QDDot[i];
-
-	for (i = 0; i < Tau.size(); i++)
-		Tau_stdvec[i] = Tau[i];
-
-	ForwardDynamics (model, Q_stdvec, QDot_stdvec, Tau_stdvec, QDDot_stdvec);
-
-	for (i = 0; i < QDDot.size(); i++)
-		QDDot[i] = QDDot_stdvec[i];
-}
-
-/*
- * \param model rigid body model
- * \param Q     state vector of the internal joints
- * \param QDot  velocity vector of the internal joints
- * \param Tau   actuations of the internal joints
  * \param X_B   transformation into base coordinates
  * \param v_B   velocity of the base (in base coordinates)
  * \param f_B   forces acting on the base (in base coordinates)
 		const cmlVector &Tau,
 		cmlVector &QDDot
 		) {
+	LOG << "-------- " << __func__ << " --------" << std::endl;
 	cmlVector q_expl (Q.size() - 6);
 	cmlVector qdot_expl (QDot.size() - 6);
 	cmlVector tau_expl (Tau.size() - 6);
 	LOG << "Q = " << Q << std::endl;
 	LOG << "QDot = " << QDot << std::endl;
 
+	SpatialMatrix permutation (
+			0., 0., 0., 0., 0., 1.,
+			0., 0., 0., 0., 1., 0.,
+			0., 0., 0., 1., 0., 0.,
+			1., 0., 0., 0., 0., 0.,
+			0., 1., 0., 0., 0., 0.,
+			0., 0., 1., 0., 0., 0.
+			);
+
 	SpatialMatrix X_B = XtransRotZYXEuler (Vector3d (Q[0], Q[1], Q[2]), Vector3d (Q[3], Q[4], Q[5]));
-
-	//
 	SpatialVector v_B (QDot[5], QDot[4], QDot[3], QDot[0], QDot[1], QDot[2]);
 	SpatialVector a_B (0., 0., 0., 0., 0., 0.);
 
 	SpatialVector f_B (Tau[5], Tau[4], Tau[3], Tau[0], Tau[1], Tau[2]);
+
 	// we also have to add any external force onto 
 	f_B += model.f_ext[0];
 
 	}
 }
 
-/*
- * \param model rigid body model
- * \param Q     state vector of the internal joints
- * \param QDot  velocity vector of the internal joints
- * \param Tau   actuations of the internal joints
- * \param X_B   transformation into base coordinates
- * \param v_B   velocity of the base (in base coordinates)
- * \param f_B   forces acting on the base (in base coordinates)
- * \param a_B   accelerations of the base (output, in base coordinates)
- * \param QDDot accelerations of the internals joints (output)
- */
-void ForwardDynamicsFloatingBaseExpl (
+void ForwardDynamics (
 		Model &model,
 		const cmlVector &Q,
 		const cmlVector &QDot,
 		const cmlVector &Tau,
-		const SpatialMatrix &X_B,
-		const SpatialVector &v_B,
-		const SpatialVector &f_B,
-		SpatialVector &a_B,
 		cmlVector &QDDot
 		) {
-	std::vector<double> Q_stdvec (Q.size());
-	std::vector<double> QDot_stdvec (QDot.size());
-	std::vector<double> QDDot_stdvec (QDDot.size());
-	std::vector<double> Tau_stdvec (Tau.size());
-
-	unsigned int i;
-	for (i = 0; i < Q.size(); i++)
-		Q_stdvec[i] = Q[i];
-
-	for (i = 0; i < QDot.size(); i++)
-		QDot_stdvec[i] = QDot[i];
-
-	for (i = 0; i < QDDot.size(); i++)
-		QDDot_stdvec[i] = QDDot[i];
-
-	for (i = 0; i < Tau.size(); i++)
-		Tau_stdvec[i] = Tau[i];
-
-	ForwardDynamicsFloatingBaseExpl (model, Q_stdvec, QDot_stdvec, Tau_stdvec, X_B, v_B, f_B, a_B, QDDot_stdvec);
-
-	for (i = 0; i < QDDot.size(); i++)
-		QDDot[i] = QDDot_stdvec[i];
-}
-
-void ForwardDynamics (
-		Model &model,
-		const std::vector<double> &Q,
-		const std::vector<double> &QDot,
-		const std::vector<double> &Tau,
-		std::vector<double> &QDDot
-		) {
-	if (model.floating_base) {
+	if (model.experimental_floating_base) {
 		cmlVector q (Q.size());
 		cmlVector qdot (QDot.size());
 		cmlVector qddot (QDDot.size());
 	assert (model.tau.size() == Tau.size() + 1);
 
 	for (i = 0; i < Q.size(); i++) {
-		model.q.at(i+1) = Q.at(i);
-		model.qdot.at(i+1) = QDot.at(i);
-		model.qddot.at(i+1) = QDDot.at(i);
-		model.tau.at(i+1) = Tau.at(i);
+		model.q[i+1] = Q[i];
+		model.qdot[i+1] = QDot[i];
+		model.qddot[i+1] = QDDot[i];