diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..367d52393f33bb9d6c7a68b3ceda1e8cd51f1724
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,76 @@
+#
+# Copyright 2014 CNRS
+#
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/cpack.cmake)
+INCLUDE(cmake/eigen.cmake)
+INCLUDE(cmake/python.cmake)
+
+SET(PROJECT_NAME pinocchio)
+SET(PROJECT_DESCRIPTION "Rigid multi body dynamics algorithms")
+SET(PROJECT_URL "http://github.com/stac-of-tasks/pinocchio")
+
+# Disable -Werror on Unix for now.
+SET(CXX_DISABLE_WERROR True)
+SET(CMAKE_VERBOSE_MAKEFILE True)
+
+project(${PROJECT_NAME})
+SETUP_PROJECT()
+
+# ----------------------------------------------------
+# --- DEPENDANCIES -----------------------------------
+# ----------------------------------------------------
+ADD_REQUIRED_DEPENDENCY("eigenpy >= v1.0.1")
+
+# ----------------------------------------------------
+# --- INCLUDE ----------------------------------------
+# ----------------------------------------------------
+SET(${PROJECT_NAME}_HEADERS
+ spatial/se3.hpp
+ spatial/motion.hpp
+ spatial/force.hpp
+ spatial/inertia.hpp
+ spatial/fwd.hpp
+ spatial/skew.hpp
+ multibody/joint.hpp
+ multibody/model.hpp
+ tools/timer.hpp
+)
+
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
+
+FOREACH(header ${${PROJECT_NAME}_HEADERS})
+ GET_FILENAME_COMPONENT(headerName ${header} NAME)
+ IF(WIN32)
+ execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different
+ ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
+ ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
+ ELSE(WIN32)
+ execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink
+ ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
+ ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
+ ENDIF(WIN32)
+ INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/${header}
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
+ENDFOREACH(header)
+
+# ----------------------------------------------------
+# --- TARGETS ----------------------------------------
+# ----------------------------------------------------
+ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL unittest/tspatial.cpp)
+PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
+
+ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL unittest/rnea.cpp)
+PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
+
+SETUP_PROJECT_FINALIZE()
+SETUP_PROJECT_CPACK()
+
diff --git a/COPYING.LESSER b/COPYING.LESSER
new file mode 100644
index 0000000000000000000000000000000000000000..65c5ca88a67c30becee01c5a8816d964b03862f9
--- /dev/null
+++ b/COPYING.LESSER
@@ -0,0 +1,165 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+ This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+ 0. Additional Definitions.
+
+ As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+ "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+ An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+ A "Combined Work" is a work produced by combining or linking an
+Application with the Library. The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+ The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+ The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+ 1. Exception to Section 3 of the GNU GPL.
+
+ You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+ 2. Conveying Modified Versions.
+
+ If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+ a) under this License, provided that you make a good faith effort to
+ ensure that, in the event an Application does not supply the
+ function or data, the facility still operates, and performs
+ whatever part of its purpose remains meaningful, or
+
+ b) under the GNU GPL, with none of the additional permissions of
+ this License applicable to that copy.
+
+ 3. Object Code Incorporating Material from Library Header Files.
+
+ The object code form of an Application may incorporate material from
+a header file that is part of the Library. You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+ a) Give prominent notice with each copy of the object code that the
+ Library is used in it and that the Library and its use are
+ covered by this License.
+
+ b) Accompany the object code with a copy of the GNU GPL and this license
+ document.
+
+ 4. Combined Works.
+
+ You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+ a) Give prominent notice with each copy of the Combined Work that
+ the Library is used in it and that the Library and its use are
+ covered by this License.
+
+ b) Accompany the Combined Work with a copy of the GNU GPL and this license
+ document.
+
+ c) For a Combined Work that displays copyright notices during
+ execution, include the copyright notice for the Library among
+ these notices, as well as a reference directing the user to the
+ copies of the GNU GPL and this license document.
+
+ d) Do one of the following:
+
+ 0) Convey the Minimal Corresponding Source under the terms of this
+ License, and the Corresponding Application Code in a form
+ suitable for, and under terms that permit, the user to
+ recombine or relink the Application with a modified version of
+ the Linked Version to produce a modified Combined Work, in the
+ manner specified by section 6 of the GNU GPL for conveying
+ Corresponding Source.
+
+ 1) Use a suitable shared library mechanism for linking with the
+ Library. A suitable mechanism is one that (a) uses at run time
+ a copy of the Library already present on the user's computer
+ system, and (b) will operate properly with a modified version
+ of the Library that is interface-compatible with the Linked
+ Version.
+
+ e) Provide Installation Information, but only if you would otherwise
+ be required to provide such information under section 6 of the
+ GNU GPL, and only to the extent that such information is
+ necessary to install and execute a modified version of the
+ Combined Work produced by recombining or relinking the
+ Application with a modified version of the Linked Version. (If
+ you use option 4d0, the Installation Information must accompany
+ the Minimal Corresponding Source and Corresponding Application
+ Code. If you use option 4d1, you must provide the Installation
+ Information in the manner specified by section 6 of the GNU GPL
+ for conveying Corresponding Source.)
+
+ 5. Combined Libraries.
+
+ You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+ a) Accompany the combined library with a copy of the same work based
+ on the Library, uncombined with any other library facilities,
+ conveyed under the terms of this License.
+
+ b) Give prominent notice with the combined library that part of it
+ is a work based on the Library, and explaining where to find the
+ accompanying uncombined form of the same work.
+
+ 6. Revised Versions of the GNU Lesser General Public License.
+
+ The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+ If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.
diff --git a/README.md b/README.md
index fced57885fe637eea7a3981c87ec7fd60c0ed500..2670ad6c74934c7187036ceb74c77970cb761837 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,31 @@
-pinocchio
-=========
+eigenpy
+===========
-Dynamic computations using Spatial Algebra
+Setup
+-----
+
+To compile this package, it is recommended to create a separate build
+directory:
+
+ mkdir _build
+ cd _build
+ cmake [OPTIONS] ..
+ make install
+
+Please note that CMake produces a `CMakeCache.txt` file which should
+be deleted to reconfigure a package from scratch.
+
+
+### Dependencies
+
+The matrix abstract layer depends on several packages which
+have to be available on your machine.
+
+ - Libraries:
+ - eigen3
+ - System tools:
+ - CMake (>=2.6)
+ - pkg-config
+ - usual compilation tools (GCC/G++, make, etc.)
+ - Python 2.7
+ - Boost python
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
new file mode 100644
index 0000000000000000000000000000000000000000..733ef2e24209ec5b24b9b6d59a4de4106805ccbf
--- /dev/null
+++ b/doc/Doxyfile.extra.in
@@ -0,0 +1,7 @@
+INPUT = @CMAKE_SOURCE_DIR@/src \
+ @CMAKE_SOURCE_DIR@/doc/additionalDoc
+IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures
+
+FILE_PATTERNS = *.cc *.cpp *.h *.hpp *.hxx
+
+TAGFILES = \
diff --git a/doc/additionalDoc/package.hh b/doc/additionalDoc/package.hh
new file mode 100644
index 0000000000000000000000000000000000000000..2ce3915d1857ce660f704ab0e3bd2a636e8930aa
--- /dev/null
+++ b/doc/additionalDoc/package.hh
@@ -0,0 +1,26 @@
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ * This file is part of sot-dynamic.
+ * sot-dynamic is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dynamic is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/** \mainpage
+\section soth_section_introduction Introduction
+
+The soth package is ...
+
+**/
diff --git a/doc/footer.html b/doc/footer.html
new file mode 100644
index 0000000000000000000000000000000000000000..535d304e7f00b9b77241c4bf6e63e883164c570c
--- /dev/null
+++ b/doc/footer.html
@@ -0,0 +1,11 @@
+ <br><br>
+ <hr>
+ <center>
+ <img src="./pictures/footer.jpg" Height=100>
+ <br>soth library documentation</br>
+ </center>
+ <hr>
+ </center>
+ </body>
+ </head>
+
diff --git a/doc/header.html b/doc/header.html
new file mode 100644
index 0000000000000000000000000000000000000000..ca3e578a0a37b115093297273260a2729989cf19
--- /dev/null
+++ b/doc/header.html
@@ -0,0 +1,7 @@
+<HTML>
+ <HEAD>
+ <TITLE>soth library documentation</TITLE>
+ <LINK HREF="package.css" REL="stylesheet" TYPE="text/css">
+ </HEAD>
+ <BODY>
+
diff --git a/doc/latex/se3.tex b/doc/latex/se3.tex
new file mode 100644
index 0000000000000000000000000000000000000000..470a9aa2db4976d90cc2a1ffc7e82f7206ee47ce
--- /dev/null
+++ b/doc/latex/se3.tex
@@ -0,0 +1,193 @@
+\documentclass[11pt,twoside,a4paper]{article}
+\usepackage{amssymb}
+\usepackage{amsmath}
+
+
+\newcommand{\BIN}{\begin{bmatrix}}
+\newcommand{\BOUT}{\end{bmatrix}}
+\newcommand{\calR}{\mathcal{R}}
+\newcommand{\calE}{\mathcal{E}}
+\newcommand{\repr}{\cong}
+\newcommand{\dpartial}[2]{\frac{\partial{#1}}{\partial{#2}}}
+\newcommand{\ddpartial}[2]{\frac{\partial^2{#1}}{\partial{#2}^2}}
+
+\newcommand{\aRb}{\ \leftidx{^A}R_B}
+\newcommand{\aMb}{\ \leftidx{^A}M_B}
+\newcommand{\amb}{\ \leftidx{^A}m_B}
+\newcommand{\apb}{{\ \leftidx{^A}{AB}{}}}
+\newcommand{\aXb}{\ \leftidx{^A}X_B}
+
+\newcommand{\bRa}{\ \leftidx{^B}R_A}
+\newcommand{\bMa}{\ \leftidx{^B}M_A}
+\newcommand{\bma}{\ \leftidx{^B}m_A}
+\newcommand{\bpa}{\ \leftidx{^B}{BA}{}}
+\newcommand{\bXa}{\ \leftidx{^B}X_A}
+
+\newcommand{\ap}{\ \leftidx{^A}p}
+\newcommand{\bp}{\ \leftidx{^B}p}
+
+\newcommand{\afs}{\ \leftidx{^A}\phi}
+\newcommand{\bfs}{\ \leftidx{^B}\phi}
+\newcommand{\af}{\ \leftidx{^A}f}
+\renewcommand{\bf}{\ \leftidx{^B}f}
+\newcommand{\an}{\ \leftidx{^A}\tau}
+\newcommand{\bn}{\ \leftidx{^B}\tau}
+
+\newcommand{\avs}{\ \leftidx{^A}\nu}
+\newcommand{\bvs}{\ \leftidx{^B}\nu}
+\newcommand{\w}{\omega}
+\newcommand{\av}{\ \leftidx{^A}v}
+\newcommand{\bv}{\ \leftidx{^B}v}
+\newcommand{\aw}{\ \leftidx{^A}\w}
+\newcommand{\bw}{\ \leftidx{^B}\w}
+
+\newcommand{\aI}{\ \leftidx{^A}I}
+\newcommand{\bI}{\ \leftidx{^B}I}
+\newcommand{\cI}{\ \leftidx{^C}I}
+\newcommand{\aY}{\ \leftidx{^A}Y}
+\newcommand{\bY}{\ \leftidx{^B}Y}
+\newcommand{\cY}{\ \leftidx{^c}Y}
+\newcommand{\aXc}{\ \leftidx{^A}X_C}
+\newcommand{\aMc}{\ \leftidx{^A}M_C}
+\newcommand{\aRc}{\ \leftidx{^A}R_C}
+\newcommand{\apc}{\ \leftidx{^A}{AC}{}}
+\newcommand{\bXc}{\ \leftidx{^B}X_C}
+\newcommand{\bRc}{\ \leftidx{^B}R_C}
+\newcommand{\bMc}{\ \leftidx{^B}M_C}
+\newcommand{\bpc}{\ \leftidx{^B}{BC}{}}
+
+\usepackage{leftidx}
+
+\begin{document}
+\title{SE(3) operations}
+\author{N. Mansard}
+\date{}
+\maketitle
+
+\section{Rigid transformation}
+$$m : p \in \calE(3) \rightarrow m(p) \in E(3)$$
+Transformation from B to A:
+$$\amb : \bp \in \calR^3 \repr \calE(3) \ \rightarrow\ \ap = \amb(\bp) = \aMb\ \bp$$
+$$ \ap = \aRb \bp + \apb$$
+$$\aMb = \BIN \aRb & \apb \\ 0 & 1 \BOUT $$
+Transformation from A to B:
+$$\bp = \aRb^T \ap + \bpa, \quad\textrm{with }\bpa = - \aRb^T \apb$$
+$$\bMa = \BIN \aRb^T & - \aRb^T \apb \\ 0 & 1 \BOUT $$
+For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
+$$\bMa = \BIN \bRa & -\bRa \apb \\ 0 & 1 \BOUT = \BIN E & -E r \\ 0 & 1 \BOUT $$
+$$\aMb = \BIN \bRa^T & \apb \\ 0 & 1 \BOUT = \BIN E^T & r \\ 0 & 1 \BOUT $$
+
+\section{Composition}
+$$ \aMb \bMc = \BIN \aRb \bRc & \apb + \aRb \bpc \\ 0 & 1 \BOUT $$
+$$ \aMb^{-1} \aMc = \BIN \aRb^T \aRc & \aRb^T (\apc - \apb) \\ 0 & 1 \BOUT $$
+
+
+
+\section{Motion Application}
+$$\avs = \BIN \av \\ \aw \BOUT$$
+$$\bvs = \bXa\avs$$
+$$ \aXb = \BIN \aRb & \apb_\times \aRb \\ 0 & \aRb \BOUT $$
+$$ \aXb^{-1} = \bXa = \BIN \aRb^T & -\aRb^T \apb_\times \\ 0 & \aRb^T \BOUT $$
+For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
+$$ \bXa = \BIN \bRa & - \bRa \apb_\times \\ 0 & \bRa \BOUT = \BIN E & -E r_\times \\ 0 & E \BOUT$$
+$$ \aXb = \BIN \bRa^T & \apb_\times \bRa^T \\ 0 & \bRa^T \BOUT = \BIN E^T & r_\times E^T \\ 0 & E^T \BOUT$$
+
+\section{Force Application}
+$$\afs = \BIN \af \\ \an \BOUT$$
+$$\bfs = \bXa^* \afs$$
+For any $\phi,\nu$, $\phi\dot\nu = \afs^T \avs = \bfs^T \bvs$ and then:
+$$\aXb^* = \aXb^{-T} = \BIN \aRb & 0 \\ \apb_\times \aRb & \aRb \BOUT$$
+(because $\apb_\times^T = - \apb_\times$).
+$$\aXb^{-*} = \bXa^* = \BIN \aRb^T & 0 \\ -\aRb^T \apb_\times & \aRb^T \BOUT$$
+For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
+$$\bXa^* = \BIN \bRa & 0 \\ -\bRa \apb_\times & \bRa \BOUT = \BIN E & 0 \\ - E r_\times & E \BOUT $$
+$$\aXb^* = \BIN \bRa^T & 0 \\ \apb_\times \bRa^T & \bRa^T \BOUT = \BIN E^T & 0 \\ r_\times E^T & E^T \BOUT $$
+
+\section{Inertia application}
+
+$$\aY: \avs \rightarrow \afs = \aY \avs$$
+
+Coordinate transform:
+$$\bY = \bXa^{*} \aY \bXa^{-1}$$
+since:
+$$\bfs = \bXa^* \bfs = \bXa^* \aI \aXb \bvs$$
+Cannonical form. The inertia about the center of mass $c$ is:
+$$\cY = \BIN m & 0 \\ 0 & \cI \BOUT$$
+Expressed in any non-centered coordinate system $A$:
+$$\aY = \aXc^* \cI \aXc^{-1} = \BIN m & m\ ^AAC_\times^T \\ m\ ^AAC_\times & \aI + m \apc_\times \apc\times^T \BOUT $$
+Changing the coordinates system from $B$ to $A$:
+$$\aY = \aXb^* \bXc^* \cI \bXc^{-1} \aXb^{-1} $$
+$$ = \BIN m & m [\apb + \aRb \bpc]_\times^T \\ m [\apb + \aRb \bpc]_\times & \aRb \bI \aRb^T - m [\apb + \aRb \bpc]_\times^2 \BOUT$$
+Representing the spatial inertia in $B$ by the triplet $(m,\bpc,\bI)$, the expression in $A$ is:
+$$ \amb: \bY = (m,\bpc,\bI) \rightarrow \aY = (m,\apb+\aRb \bpc,\aRb \bI \aRb^T)$$
+Similarly, the inverse action is:
+$$ \amb^{-1}: \aY \rightarrow \bY = (m,\aRb^T(^AAC-\apb),\aRb^T\aI \aRb) $$
+
+Motion-to-force map:
+$$ Y \nu = \BIN m & mc_\times^T \\ mc_\times & I+mc_\times c_\times^T \BOUT \BIN v \\ \omega \BOUT
+ = \BIN m v - mc \times \omega \\ mc \times v + I \omega - mc \times ( c\times \omega) \BOUT$$
+
+Nota: the square of the cross product is:
+$$\BIN x\\y\\z\BOUT_ \times^2 = \BIN 0&-z&y \\ z&0&-x \\ -y&x&0 \BOUT^2 = \BIN -y^2-z^2&xy&xz \\ xy&-x^2-z^2&yz \\ xz&yz&-x^2-y^2 \BOUT$$
+There is no computational interest in using it.
+
+\section{Cross products}
+
+Motion-motion product:
+$$\nu_1 \times \nu_2 = \BIN v_1\\\omega_1\BOUT \times \BIN v_2\\\omega_2\BOUT = \BIN v_1 \times \omega_2 + \omega_1 \times v_2 \\ \omega_1 \times \omega_2 \BOUT $$
+Motion-force product:
+$$\nu \times \phi = \BIN v\\\omega\BOUT \times \BIN f\\ \tau \BOUT = \BIN \omega \times f \\ \omega \times \tau + v \times f \BOUT $$
+A special form of the motion-force product is often used:
+\begin{align*}\nu \times (Y \nu) &= \nu \times \BIN mv - mc\times \omega \\ mc\times v + I \omega - mc\times(c\times \omega) \BOUT \\&= \BIN m \omega\times v - \omega\times(mc\times \omega) \\ \omega \times ( mc \times v) + \omega \times (I\omega) -\omega \times(c \times( mc\times \omega)) -v\times(mc \times \omega)\BOUT\end{align*}
+Setting $\beta=mc \times \omega$, this product can be written:
+$$\nu \times (Y \nu) = \BIN \omega \times (m v - \beta) \\ \omega \times( c \times (mv-\beta)+I\omega) - v \times \beta \BOUT$$
+This last form cost five $\times$, four $+$ and one $3\times3$ matrix-vector multiplication.
+
+\section{Joint}
+
+We denote by $1$ the coordinate system attached to the parent (predecessor) body at the joint input, ad by $2$ the coordinate system attached to the (child) successor body at the joint output. We neglect the possible time variation of the joint model (ie the bias velocity $\sigma = \nu(q,0)$ is null).
+
+The joint geometry is expressed by the rigid transformation from the input to the ouput, parametrized by the joint coordinate system $q \in \mathcal{Q}$:
+$$ ^2m_1 \repr \ ^2M_1(q)$$
+
+The joint velocity (i.e. the velocity of the child wrt. the parent in the child coordinate system) is:
+$$^2\nu_{12} = \nu_J(q,v_q) = \ ^2S(q) v_q $$
+where $^2S$ is the joint Jacobian (or constraint matrix) that define the motion subspace allowed by the joint, and $v_q$ is the joint coordinate velocity (i.e. an element of the Lie algebra associated with the joint coordinate manifold), which would be $v_q=\dot q$ when $\dot q$ exists.
+
+The joint acceleration is:
+$$^2\alpha_{12} = S \dot v_q + c_J + \ ^2\nu_{1} \times \ ^2\nu_{12}$$
+where $c_J = \sum_{i=1}^{n_q} \dpartial{S}{q_i} \dot q_i$ (null in the usual cases) and $^2\nu_{1}$ is the velocity of the parent body with respect to an absolute (Galilean) coordinate system\footnote{The abosulte velocity $\nu_{1}$ is also the relative velocity wrt. the Galilean coordinate system $\Omega$. The exhaustive notation should be $\nu_{\Omega1}$ but $\nu_1$ is prefered for simplicity.}.
+
+The joint calculations take as input the joint position $q$ and velocity $v_q$ and should output $^2M_1$, $^2\nu_{12}$ and $^2c$ (this last vector being often a trivial $0_6$ vector). In addition, the joint model should store the position of the joint input in the central coordinate system of the previous joint $^0m_1$ which is a constant value.
+
+The joint integrator computes the exponential map associated with the joint manifold. The function inputs are the initial position $q_0$, the velocity $v_q$ and the length of the integration interval $t$. It computes $q_t$ as:
+$$ q_t = q_0 + \int_0^t v_q dt$$
+For the simple vectorial case where $v_q=\dot q$, we have $q_t=q_0 + t v_q$. Written in the more general case of a Lie groupe, we have $q_t = q_0 exp(t v_q)$ where $exp$ denotes the exponential map (i.e. integration of a constant vector field from the Lie algebra into the Lie group). This integration only consider first order explicit Euler. More general integrators (e.g. Runge-Kutta in Lie groupes) remains to be written. Adequate references are welcome.
+
+\section{RNEA}
+
+\subsection{Initialization}
+$$^0\nu_0 = 0 ; \ ^0\alpha_0 = -g$$
+
+In the following, the coordinate system $i$ is attached to the output of the joint (child body), while $lambda(i)$ is the central coordinate system attached to the parent joint. The coordinated system associated with the joint input is denoted by $i_0$. The constant rigid transformation from $\lambda(i)$ to the joint input is then $^{\lambda(i)}M_{i_0}$.
+
+
+\subsection{Forward loop}
+For each joint $i$, update the joint calculation $\mathbf j_i$.calc($q,v_q$). This compute $\mathbf{j}.M = \ ^{\lambda(i)}M_{i_0}(q)$, $\mathbf{j}.\nu = \ ^i\nu_{{\lambda(i)}i}(q,v_q)$, $\mathbf{j}.S = \ ^iS(q)$ and $\mathbf{j}.c = \sum_{k=1}^{n_q} \dpartial{^iS}{q_k} \dot q_k$. Attached to the joint is also its placement in body $\lambda(i)$ denoted by $\mathbf{j}.M_0 =\ ^{\lambda(i)}M_{i_0}$. Then:
+
+$$^{\lambda(i)}M_i = \mathbf{j}.M_0 \ \mathbf{j}.M $$
+$$^0M_i = \ ^0M_{\lambda(i)} \ ^{\lambda(i)}M_i$$
+$$^i\nu_{i}= \ ^{\lambda(i)}X_i^{-1} \ ^{\lambda(i)}\nu_{{\lambda(i)}} + \mathbf{j}.\nu$$
+$$^i\alpha_{i}= \ ^{\lambda(i)}X_i^{-1} \ ^{\lambda(i)}\alpha_{{\lambda(i)}} + \mathbf{j}.S \dot v_q + \mathbf{j}.c + \ ^i\nu_{i} \times \mathbf{j}.\nu$$
+$$^i\phi_i= \ ^iY_i \ ^i\alpha_i + \ ^i\nu_i \times \ ^iY_i \ ^i\nu_i - \ ^0X_i^{-*}\ ^0\phi_i^{ext}$$
+
+\subsection{Backward loop}
+For each joint $i$ from leaf to root, do:
+$$\tau_i = \mathbf{j}.S^T \ ^i\phi_i$$
+$$^{\lambda(i)}\phi_{\lambda(i)} \ +\!\!= \ ^{\lambda(i)}X_i^{*} \ ^i\phi_i$$
+
+\subsection{Nota}
+It is more efficient to apply $X^{-1}$ than $X$. Similarly, it is more efficient to apply $X^{-*}$ than $X^*$. Therefore, it is better to store the transformations $^{\lambda(i)}m_i$ and $^0m_i$ than $^im_{\lambda(i)}$ and $^im_0$.
+
+
+\end{document}
diff --git a/doc/package.css b/doc/package.css
new file mode 100644
index 0000000000000000000000000000000000000000..f992392b8d5f99929d049a86a66bfaf807cdc431
--- /dev/null
+++ b/doc/package.css
@@ -0,0 +1,230 @@
+body {
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ color: #5D5D5D;
+}
+
+dl {
+ border: 1.5px #82b6d7 solid;
+ width: 97%;
+ padding: 5px;
+ color: #330077;
+}
+
+code {
+ color: #3C9A35;
+}
+
+td.md {
+ color: #0066CC;
+}
+
+h1 {
+ padding-top: 50px;
+ padding: 0px;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-variant: small-caps;
+ color:#0066CC;
+ text-align: center;
+}
+
+h2,h3,hr {
+ magin-top: 15px;
+ padding: 0px;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-variant: small-caps;
+ color:#0066CC;
+}
+
+h4 {
+ color: #3C9A35;
+}
+
+a:link {
+ font-weight: bold;
+ text-decoration: none;
+ color:#0066CC
+}
+
+a:hover, a:active {
+ text-decoration: underline;
+ color: #3C9A35;
+}
+
+a:visited {
+ font-weight: bold;
+ color: #3C9A35;
+ text-decoration: none;
+}
+
+DIV.memitem
+{
+ spacing: 10px;
+ width:100%;
+ background:#FFFFFF;
+ font-size:100%;
+ line-height:normal;
+ border-width: 1px;
+ border-style: solid;
+ border-color: #808080;
+ -moz-border-radius: 8px 8px 8px 8px;
+}
+
+DIV.memproto
+{
+ width:100%;
+ background:#F0F0F0;
+ font-size:100%;
+ line-height:normal;
+ border-width: 1px;
+ border-style: solid;
+ border-color: #808080;
+ -moz-border-radius: 8px 8px 8px 8px;
+}
+
+DIV.memdoc
+{
+ padding: 10px;
+ width:100%;
+ font-size:100%;
+ line-height:normal;
+}
+
+DIV.tabs
+{
+ float : left;
+ width : 100%;
+ background : url("tab_b.gif") repeat-x bottom;
+ margin-bottom : 4px;
+}
+
+DIV.tabs UL
+{
+ margin : 0px;
+ padding-left : 10px;
+ list-style : none;
+}
+
+DIV.tabs LI, DIV.tabs FORM
+{
+ display : inline;
+ margin : 0px;
+ padding : 0px;
+}
+
+DIV.tabs FORM
+{
+ float : right;
+}
+
+DIV.tabs A
+{
+ float : left;
+ background : url("tab_r.gif") no-repeat right top;
+ border-bottom : 1px solid #84B0C7;
+ font-size : x-small;
+ font-weight : bold;
+ text-decoration : none;
+}
+
+DIV.tabs A:hover
+{
+ background-position: 100% -150px;
+}
+
+DIV.tabs A:link, DIV.tabs A:visited,
+DIV.tabs A:active, DIV.tabs A:hover
+{
+ color: #1A419D;
+}
+
+DIV.tabs SPAN
+{
+ float : left;
+ display : block;
+ background : url("tab_l.gif") no-repeat left top;
+ padding : 5px 9px;
+ white-space : nowrap;
+}
+
+DIV.tabs INPUT
+{
+ float : right;
+ display : inline;
+ font-size : 1em;
+}
+
+DIV.tabs TD
+{
+ font-size : x-small;
+ font-weight : bold;
+ text-decoration : none;
+}
+
+
+
+DIV.tabs SPAN {float : none;}
+
+DIV.tabs A:hover SPAN
+{
+ background-position: 0% -150px;
+}
+
+DIV.tabs LI#current A
+{
+ background-position: 100% -150px;
+ border-width : 0px;
+}
+
+DIV.tabs LI#current SPAN
+{
+ background-position: 0% -150px;
+ padding-bottom : 6px;
+}
+
+DIV.nav
+{
+ background : none;
+ border : none;
+ border-bottom : 1px solid #84B0C7;
+}
+
+DIV.groupHeader
+{
+ padding-top: 30px;
+ padding-bottom: 20px;
+ background : none;
+ border : none;
+ border-bottom : 1px solid #84B0C7;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-variant: small-caps;
+ font-size: 14pt;
+ color:#0066CC;
+}
+
+.directory p
+{
+ margin: 0px;
+ white-space: nowrap;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-size: 10pt;
+ font-weight: normal;
+}
+
+
+.directory h3
+{
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ margin: 0px;
+ margin-top: 1em;
+ padding-bottom: 20px;
+ font-size: 12pt;
+ font-variant: small-caps;
+ text-align: center;
+}
+
+.directory a:visited {
+ font-weight: bold;
+ text-decoration: none;
+ color:#0066CC
+}
+
diff --git a/doc/pictures/HRP2.jpg b/doc/pictures/HRP2.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..377681b20575b32c765e0ad9403723a0e3ea5c75
Binary files /dev/null and b/doc/pictures/HRP2.jpg differ
diff --git a/doc/pictures/footer.jpg b/doc/pictures/footer.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..377681b20575b32c765e0ad9403723a0e3ea5c75
Binary files /dev/null and b/doc/pictures/footer.jpg differ
diff --git a/doc/pictures/footer.txt b/doc/pictures/footer.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5c1322c49a0ff9e84f4bd2384f25dafb52e252b0
--- /dev/null
+++ b/doc/pictures/footer.txt
@@ -0,0 +1,2 @@
+Copy in this directory the image you wish to use in the footer of the documentation or edit file sot-core/doc/footer.html and
+remove this file.
diff --git a/doc/pictures/soth.tif b/doc/pictures/soth.tif
new file mode 100644
index 0000000000000000000000000000000000000000..3f93b62ae3164d17aa3f31dba849de7c09735af3
Binary files /dev/null and b/doc/pictures/soth.tif differ
diff --git a/pinocchio.pc.cmake b/pinocchio.pc.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..1810450ffa0d87196719a704b0093aa199bd87c5
--- /dev/null
+++ b/pinocchio.pc.cmake
@@ -0,0 +1,13 @@
+prefix=${CMAKE_INSTALL_PREFIX}
+exec_prefix=${install_pkg_prefix}
+libdir=${install_pkg_exec_prefix}/lib
+includedir=${install_pkg_prefix}/include
+datarootdir=${install_pkg_prefix}/share
+docdir=${install_pkg_datarootdir}/doc/${PROJECT_NAME}
+
+Name: ${PROJECT_NAME}
+Description:
+Version: ${PROJECT_VERSION}
+Requires: ${PACKAGE_REQUIREMENTS}
+Libs: ${LIBDIR_KW}${install_pkg_libdir} ${${PROJECT_NAME}_LDFLAGS}
+Cflags: -I${install_pkg_include_dir} ${${PROJECT_NAME}_CXXFLAGS}
diff --git a/src/multibody/joint.hpp b/src/multibody/joint.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..680f950ef15a13fb344f21be82e2747f362e61c3
--- /dev/null
+++ b/src/multibody/joint.hpp
@@ -0,0 +1,174 @@
+#ifndef __se3_joint_hpp__
+#define __se3_joint_hpp__
+
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include <Eigen/Geometry>
+
+namespace se3
+{
+ template<class C>
+ struct traits {};
+
+ template<typename JointData>
+ struct JointDataBase
+ {
+ typedef typename traits<JointData>::Constraint_t Constraint_t;
+ typedef typename traits<JointData>::Transformation_t Transformation_t;
+ typedef typename traits<JointData>::Velocity_t Velocity_t;
+ typedef typename traits<JointData>::Bias_t Bias_t;
+
+ const Constraint_t & S() { return static_cast<JointData*>(this)->S; }
+ const Transformation_t & M() { return static_cast<JointData*>(this)->M; };
+ const Velocity_t & v() { return static_cast<JointData*>(this)->v; };
+ const Bias_t & c() { return static_cast<JointData*>(this)->c; };
+ };
+
+
+
+ template<typename Joint>
+ struct JointBase
+ {
+ typedef typename traits<Joint>::JointData JointData;
+ typedef typename traits<Joint>::Placement_t Placement_t;
+ typedef typename traits<Joint>::Configuration_t Configuration_t;
+ typedef typename traits<Joint>::Velocity_t Velocity;
+
+ JointData createData() { return static_cast<Joint*>(this)->createData(); }
+ };
+
+
+
+ // struct JointRXData;
+ // template<>
+ // struct traits<JointRXData>
+ // {
+ // typedef Eigen::Matrix<double,6,1> Constraint_t;
+ // typedef se3::SE3 Transformation_t;
+ // typedef Eigen::Matrix<double,6,1> Velocity_t;
+ // typedef BiasZero Bias_t;
+ // };
+
+ // struct JointRXData : public JointDataBase<JointRXData>
+ // {
+ // typedef typename traits<JointRXData>::Constraint_t Constraint_t;
+ // typedef typename traits<JointRXData>::Transformation_t Transformation_t;
+ // typedef typename traits<JointRXData>::Velocity_t Velocity_t;
+ // typedef typename traits<JointRXData>::Bias_t Bias_t;
+
+ // Constraint_t S;
+ // Transformation_t M;
+ // Velocity_t v;
+ // Bias_t c;
+
+ // JointRXData() { S << 0,0,0,1,0,0; }
+ // };
+
+ // struct JointRX;
+ // template<>
+ // struct traits<JointRX>
+ // {
+ // typedef JointRXData JointData;
+ // typedef SE3 Placement_t;
+ // typedef double Configuration_t;
+ // typedef double Velocity_t;
+ // };
+
+ // struct JointRX : public JointBase<JointRX>
+ // {
+ // typedef traits<JointRX>::JointData JointData;
+
+ // JointData createData() const { return JointData(); }
+ // void calc( JointData& data, Configuration_t q, Configuration_t v )
+ // {
+
+
+
+ // }
+ // };
+
+ struct JointDataRX;
+ struct JointModelRX;
+
+ template<>
+ struct traits<JointDataRX>
+ {
+ struct BiasZero {};
+ template<typename D>
+ friend const Eigen::MatrixBase<D> & operator+ ( const BiasZero&, const Eigen::MatrixBase<D>& v) { return v; }
+ template<typename D>
+ friend const Eigen::MatrixBase<D> & operator+ ( const Eigen::MatrixBase<D>& v, const BiasZero&) { return v; }
+ friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
+
+ typedef Eigen::Matrix<double,6,1> Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef Motion Velocity_t;
+ typedef BiasZero Bias_t;
+ typedef JointModelRX JointModel;
+ };
+
+ struct JointDataRX
+ {
+ typedef typename traits<JointDataRX>::Constraint_t Constraint_t;
+ typedef typename traits<JointDataRX>::Transformation_t Transformation_t;
+ typedef typename traits<JointDataRX>::Velocity_t Velocity_t;
+ typedef typename traits<JointDataRX>::Bias_t Bias_t;
+ typedef typename traits<JointDataRX>::JointModel JointModel;
+ //typedef typename traits<JointDataRX>::;
+
+ Constraint_t S;
+ Transformation_t M;
+ Velocity_t v;
+ Bias_t c;
+
+ JointDataRX() : M(1)
+ {
+ S << 0,0,0,1,0,0;
+ }
+ };
+
+ template<>
+ struct traits<JointModelRX>
+ {
+ typedef JointDataRX JointData;
+ typedef SE3 Placement_t;
+ };
+
+ struct JointModelRX
+ {
+ typedef traits<JointModelRX>::JointData JointData;
+
+ static const int nq = 1;
+ static const int nv = 1;
+ int idx_q,idx_v;
+
+ JointModelRX() : idx_q(-1),idx_v(-1) {} // Default constructor for std::vector
+ JointModelRX( int index_q,int index_v ) : idx_q(index_q),idx_v(index_v) {}
+ JointModelRX( int index_q,int index_v, const JointModelRX& ) : idx_q(index_q),idx_v(index_v) {}
+
+ JointData createData() const { return JointData(); }
+ void calc( JointData& data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs )
+ {
+ const double & q = qs[idx_q];
+ const double & v = vs[idx_v];
+
+ //data.M.rotation( Eigen::AngleAxis<double>(q, Eigen::Vector3d::UnitX()).matrix() );
+ double ca,sa; sincos(q,&sa,&ca);
+ Eigen::Matrix3d R3;
+ R3 <<
+ 1,0,0,
+ 0,ca,sa,
+ 0,-sa,ca;
+ data.M.rotation(R3);
+ Eigen::Vector3d v3; v3 << v,0,0;
+ data.v.angular(v3);
+ }
+ };
+
+
+
+
+} // namespace se3
+
+#endif // ifndef __se3_joint_hpp__
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..67e6f3a2dfe0255e7fc9f2f8bb4afe58f45bfe63
--- /dev/null
+++ b/src/multibody/model.hpp
@@ -0,0 +1,153 @@
+#ifndef __se3_model_hpp__
+#define __se3_model_hpp__
+
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include <Eigen/StdVector>
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelRX);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointDataRX);
+
+namespace se3
+{
+ struct Model;
+ struct Data;
+
+ class Model
+ {
+ public:
+ typedef int Index;
+
+ int nq;
+ int nv;
+ int nbody;
+
+ std::vector<Inertia> inertias;
+ std::vector<SE3> jointPlacements;
+ std::vector<JointModelRX> joints;
+ std::vector<Index> parents;
+ std::vector<std::string> names;
+
+ Motion gravity;
+ static const Eigen::Vector3d gravity981;
+
+ Model()
+ : nq(0)
+ , nv(0)
+ , nbody(1)
+ , inertias(1)
+ , jointPlacements(1)
+ , joints(1)
+ , parents(1)
+ , names(1)
+ , gravity( gravity981,Eigen::Vector3d::Zero() )
+ {
+ names[0] = "universe";
+ }
+ Index addBody( Index parent,const JointModelRX & j,const SE3 & placement,
+ const Inertia & Y,const std::string & name = "" );
+ Index getBodyId( const std::string & name ) const;
+ const std::string& getBodyName( Index index ) const;
+
+ };
+
+ class Data
+ {
+ public:
+
+ const Model& model;
+ std::vector<JointDataRX> joints;
+ std::vector<Motion> a; // Body acceleration
+ std::vector<Motion> v; // Body velocity
+ std::vector<Force> f; // Body force
+ std::vector<SE3> oMi; // Body absolute placement (wrt world)
+ std::vector<SE3> liMi; // Body relative placement (wrt parent)
+ Eigen::VectorXd tau; // Joint forces
+
+ Data( const Model& ref );
+
+
+ };
+
+ const Eigen::Vector3d Model::gravity981 (0,0,9.81);
+
+} // namespace se3
+
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+namespace se3
+{
+
+ std::string random(const int len)
+ {
+ std::string res;
+ static const char alphanum[] =
+ "0123456789"
+ "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+ "abcdefghijklmnopqrstuvwxyz";
+
+ for (int i=0; i<len;++i)
+ res += alphanum[std::rand() % (sizeof(alphanum) - 1)];
+ return res;
+}
+
+ Model::Index Model::addBody( Index parent,const JointModelRX & j,const SE3 & placement,
+ const Inertia & Y,const std::string & name )
+ {
+ assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
+ &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
+ assert( (j.nq>=0)&&(j.nv>=0) );
+
+ Index idx = nbody ++;
+
+ joints .push_back(JointModelRX(nq,nv,j));
+ inertias .push_back(Y);
+ parents .push_back(parent);
+ jointPlacements.push_back(placement);
+ names .push_back( (name!="")?name:random(8) );
+
+ nq += j.nq;
+ nv += j.nv;
+ return idx;
+ }
+ Model::Index Model::getBodyId( const std::string & name ) const
+ {
+ Index res = std::find(names.begin(),names.end(),name) - names.begin();
+ assert( (res>=0)&&(res<nbody) );
+ return res;
+ }
+
+ const std::string& Model::getBodyName( Model::Index index ) const
+ {
+ assert( (index>=0)&&(index<nbody) );
+ return names[index];
+ }
+
+ Data::Data( const Model& ref )
+ :model(ref)
+ ,joints(0)
+ ,a(ref.nbody)
+ ,v(ref.nbody)
+ ,f(ref.nbody)
+ ,oMi(ref.nbody)
+ ,liMi(ref.nbody)
+ ,tau(ref.nbody)
+ {
+ for(int i=0;i<model.nbody;++i) joints.push_back(model.joints[i].createData());
+ }
+
+
+} // namespace se3
+
+#endif // ifndef __se3_model_hpp__
diff --git a/src/spatial/force.hpp b/src/spatial/force.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c2f28763decacf677136e6a92cf4436a60ba5a03
--- /dev/null
+++ b/src/spatial/force.hpp
@@ -0,0 +1,121 @@
+#ifndef __se3_force_hpp__
+#define __se3_force_hpp__
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "pinocchio/spatial/fwd.hpp"
+
+namespace se3
+{
+ template<typename _Scalar, int _Options>
+ class ForceTpl
+ {
+ public:
+ typedef _Scalar Scalar;
+ enum { Options = _Options };
+ typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+ typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+ typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
+ typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
+ typedef MotionTpl<Scalar,Options> Motion;
+ typedef SE3Tpl<Scalar,Options> SE3;
+ enum { LINEAR = 0, ANGULAR = 3 };
+
+ public:
+ // Constructors
+ ForceTpl() : m_n(), m_f() {}
+ ForceTpl(const Vector3 & f,const Vector3 & n) : m_n(n), m_f(f) {}
+ ForceTpl(const Vector6 & v) : m_n(v.template segment<3>(ANGULAR)),
+ m_f(v.template segment<3>(LINEAR)) {}
+
+ // initializers
+ static ForceTpl Zero() { return ForceTpl(Vector3::Zero(), Vector3::Zero()); }
+ static ForceTpl Random() { return ForceTpl(Vector3::Random(), Vector3::Random()); }
+
+ Vector6 toVector() const
+ {
+ Vector6 f;
+ f.template segment<3>(ANGULAR) = m_n;
+ f.template segment<3>(LINEAR) = m_f;
+ return f;
+ }
+ operator Vector6 () const { return toVector(); }
+
+ // Getters
+ const Vector3 & linear() const { return m_f; }
+ const Vector3 & angular() const { return m_n; }
+
+ // Arithmetic operators
+ ForceTpl & operator=(const Vector6 & phi)
+ {
+ m_n = phi.template segment<3>(ANGULAR);
+ m_f = phi.template segment<3>(LINEAR);
+ return *this;
+ }
+
+ ForceTpl operator+(const ForceTpl & phi) const
+ {
+ return ForceTpl(m_f+phi.m_f,m_n+phi.m_n);
+ }
+
+ ForceTpl& operator+= (const ForceTpl & phi)
+ {
+ m_f += phi.m_f;
+ m_n += phi.m_n;
+ return *this;
+ }
+
+ ForceTpl operator-() const
+ {
+ return ForceTpl(-m_f, -m_n);
+ }
+
+ ForceTpl operator-(const ForceTpl & phi) const
+ {
+ return ForceTpl(m_f-phi.m_f,m_n-phi.m_n);
+ }
+
+ // ForceTpl operator*(Scalar a) const
+ // {
+ // return ForceTpl(m_n*a, m_f*a);
+ // }
+
+ // friend ForceTpl operator*(Scalar a, const ForceTpl & phi)
+ // {
+ // return ForceTpl(phi.n()*a, phi.f()*a);
+ // }
+
+
+ /// af = aXb.act(bf)
+ ForceTpl se3Action(const SE3 & m) const
+ {
+ Vector3 Rf = static_cast<Vector3>(m.rotation()*linear());
+ return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular());
+ }
+ /// bf = aXb.actInv(af)
+ ForceTpl se3ActionInverse(const SE3 & m) const
+ {
+ return ForceTpl(m.rotation().transpose()*linear(),
+ m.rotation().transpose()*(angular() - m.translation().cross(linear())) );
+ }
+
+ friend std::ostream & operator << (std::ostream & os, const ForceTpl & phi)
+ {
+ os
+ << "f =\n" << phi.linear() << std::endl
+ << "tau =\n" << phi.angular() << std::endl;
+ return os;
+ }
+
+ public: //
+ private:
+ Vector3 m_n;
+ Vector3 m_f;
+ };
+
+ typedef ForceTpl<double> Force;
+
+} // namespace se3
+
+#endif // ifndef __se3_force_hpp__
+
diff --git a/src/spatial/fwd.hpp b/src/spatial/fwd.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..980553c215d8139296b577fbd4a6fb9caf3c81bd
--- /dev/null
+++ b/src/spatial/fwd.hpp
@@ -0,0 +1,15 @@
+#ifndef __se3_fwd_hpp__
+#define __se3_fwd_hpp__
+
+#include <Eigen/Core>
+
+namespace se3
+{
+ template<typename _Scalar, int _Options=0> class SE3Tpl;
+ template<typename _Scalar, int _Options=0> class MotionTpl;
+ template<typename _Scalar, int _Options=0> class ForceTpl;
+ template<typename _Scalar, int _Options=0> class InertiaTpl;
+
+} // namespace se3
+
+#endif // ifndef __se3_fwd_hpp__
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..87291b894bc19bfc992a8501bd41ab790492c4af
--- /dev/null
+++ b/src/spatial/inertia.hpp
@@ -0,0 +1,152 @@
+#ifndef __se3_inertia_hpp__
+#define __se3_inertia_hpp__
+
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+
+
+namespace se3
+{
+ template<typename _Scalar, int _Options>
+ class InertiaTpl
+ {
+ public:
+ typedef _Scalar Scalar;
+ enum { Options = _Options };
+ typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+ typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+ typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
+ typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
+ typedef MotionTpl<Scalar,Options> Motion;
+ typedef ForceTpl<Scalar,Options> Force;
+ typedef SE3Tpl<Scalar,Options> SE3;
+ typedef InertiaTpl<Scalar,Options> Inertia;
+ enum { LINEAR = 0, ANGULAR = 3 };
+ typedef Eigen::SelfAdjointView<Matrix3,Eigen::Upper> Symmetric3;
+
+ public:
+ // Constructors
+ InertiaTpl() : m(), c(), dense_I(), I(dense_I) {}
+ InertiaTpl(Scalar m_,
+ const Vector3 &c_,
+ const Matrix3 &I_)
+ : m(m_),
+ c(c_),
+ dense_I(I_),
+ I(dense_I) {}
+ InertiaTpl(Scalar _m,
+ const Vector3 &_c,
+ const Symmetric3 &_I)
+ : m(_m),
+ c(_c),
+ dense_I(),
+ I(dense_I) { I = _I; }
+ InertiaTpl(const InertiaTpl & clone) // Clone constructor for std::vector
+ : m(clone.m),
+ c(clone.c),
+ dense_I(clone.dense_I),
+ I(dense_I) {}
+ InertiaTpl& operator= (const InertiaTpl& clone) // Copy operator for std::vector
+ {
+ m=clone.m; c=clone.c; dense_I=clone.dense_I;
+ return *this;
+ }
+
+ // Initializers
+ static Inertia Zero()
+ {
+ return InertiaTpl(0.,
+ Vector3::Zero(),
+ Matrix3::Zero());
+ }
+ static Inertia Identity()
+ {
+ return InertiaTpl(1.,
+ Vector3::Zero(),
+ Matrix3::Identity());
+ }
+ static Inertia Random()
+ {
+ return InertiaTpl(Eigen::internal::random<Scalar>(),
+ Vector3::Random(),
+ Matrix3::Random().template selfadjointView<Eigen::Upper>());
+ }
+
+ // Getters
+ Scalar mass() const { return m; }
+ const Vector3 & lever() const { return c; }
+ const Symmetric3 & inertia() const { return I; }
+
+ Matrix6 toMatrix() const
+ {
+ Matrix6 M;
+ M.template block<3,3>(LINEAR, LINEAR ) = m*Matrix3::Identity();
+ M.template block<3,3>(LINEAR, ANGULAR) = -m*skew(c);
+ M.template block<3,3>(ANGULAR,LINEAR ) = m*skew(c);
+ M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)I - m*skew(c)*skew(c);
+ return M;
+ }
+ operator Matrix6 () const { return toMatrix(); }
+
+ // Arithmetic operators
+ Inertia operator+(const InertiaTpl &other) const
+ {
+ return InertiaTpl(m+other.m, c+other.c, I+other.I);
+ }
+
+ Force operator*(const Motion &v) const
+ {
+ Vector3 mcxw = m*c.cross(v.angular());
+ return Force( m*v.linear()-mcxw,
+ m*c.cross(v.linear()) + I*v.angular() - c.cross(mcxw) );
+ }
+
+ /// aI = aXb.act(bI)
+ Inertia se3Action(const SE3 & M) const
+ {
+ /* The multiplication RIR' has a particular form that could be used, however it
+ * does not seems to be more efficient, see http://stackoverflow.com/questions/
+ * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
+ return Inertia( m,
+ M.translation()+M.rotation()*c,
+ M.rotation()*I*M.rotation().transpose());
+ }
+ /// bI = aXb.actInv(aI)
+ Inertia se3ActionInverse(const SE3 & M) const
+ {
+ return Inertia( m,
+ M.rotation().transpose()*(c-M.translation()),
+ M.rotation().transpose()*I*M.rotation());
+ }
+
+ //
+ Force vxiv( const Motion& v ) const
+ {
+ Vector3 mcxw = m*c.cross(v.angular());
+ Vector3 mv_mcxw = m*v.linear()-mcxw;
+ return Force( v.angular().cross(mv_mcxw),
+ v.angular().cross(c.cross(mv_mcxw)+I*v.angular())-v.linear().cross(mcxw) );
+ }
+
+ friend std::ostream & operator<< (std::ostream &os, const Inertia &I)
+ {
+ os << "m =\n" << I.m << "\n"
+ << "c =\n" << I.c << "\n"
+ << "I =\n" << (Matrix3)I.I;
+ return os;
+ }
+
+ public:
+ private:
+ Scalar m;
+ Vector3 c;
+ Matrix3 dense_I;
+ Symmetric3 I;
+ };
+
+
+ typedef InertiaTpl<double,0> Inertia;
+
+} // namespace se3
+
+#endif // ifndef __se3_inertia_hpp__
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b70b0c6f7e3a27450e2a78fc1845547361cea06f
--- /dev/null
+++ b/src/spatial/motion.hpp
@@ -0,0 +1,136 @@
+#ifndef __se3_motion_hpp__
+#define __se3_motion_hpp__
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/force.hpp"
+
+namespace se3
+{
+ template<typename _Scalar, int _Options>
+ class MotionTpl
+ {
+ public:
+ typedef _Scalar Scalar;
+ enum { Options = _Options };
+ typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+ typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+ typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
+ typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
+ typedef ForceTpl<Scalar,Options> Force;
+ typedef SE3Tpl<Scalar,Options> SE3;
+ enum { LINEAR = 0, ANGULAR = 3 };
+
+ public:
+ // Constructors
+ MotionTpl() : m_w(), m_v() {}
+ MotionTpl(const Vector3 & v, const Vector3 & w) : m_w(w), m_v(v) {}
+ MotionTpl(const Vector6 & v) :
+ m_w(v.template segment<3>(ANGULAR)),
+ m_v(v.template segment<3>(LINEAR)) {}
+
+ static MotionTpl Zero() { return MotionTpl(Vector3::Zero(), Vector3::Zero()); }
+ static MotionTpl Random() { return MotionTpl(Vector3::Random(),Vector3::Random()); }
+ Vector6 toVector() const
+ {
+ Vector6 v;
+ v.template segment<3>(ANGULAR) = m_w;
+ v.template segment<3>(LINEAR) = m_v;
+ return v;
+ }
+ operator Vector6 () const { return toVector(); }
+
+ // Getters
+ const Vector3 & angular() const { return m_w; }
+ const Vector3 & linear() const { return m_v; }
+ void angular(const Vector3 & w) { m_w=w; }
+ void linear (const Vector3 & v) { m_v=v; }
+
+ // Arithmetic operators
+ MotionTpl & operator=(const Vector6 & v)
+ {
+ m_w = v.template segment<3>(ANGULAR);
+ m_v = v.template segment<3>(LINEAR);
+ return *this;
+ }
+
+ MotionTpl operator-() const
+ {
+ return MotionTpl(-m_v, -m_w);
+ }
+
+ MotionTpl operator+(const MotionTpl & v2) const
+ {
+ return MotionTpl(m_v+v2.m_v,m_w+v2.m_w);
+ }
+ MotionTpl& operator+=(const MotionTpl & v2)
+ {
+ m_v+=v2.m_v;
+ m_w+=v2.m_w;
+ return *this;
+ }
+
+
+ // MotionTpl operator*(Scalar a) const
+ // {
+ // return MotionTpl(m_w*a, m_v*a);
+ // }
+
+ // friend MotionTpl operator*(Scalar a, const MotionTpl & mv)
+ // {
+ // return MotionTpl(mv.w()*a, mv.v()*a);
+ // }
+
+ MotionTpl cross(const MotionTpl& v2) const
+ {
+ return MotionTpl( m_v.cross(v2.m_w)+m_w.cross(v2.m_v),
+ m_w.cross(v2.m_w) );
+ }
+
+ ForceTpl<Scalar,Options> cross(const ForceTpl<Scalar,Options>& phi) const
+ {
+ return ForceTpl<Scalar,Options>
+ ( m_w.cross(phi.linear()),
+ m_w.cross(phi.angular())+m_v.cross(phi.linear()) );
+ }
+
+
+ // template<typename S,int O>
+ // friend MotionTpl<S,O> operator^( const MotionTpl<S,O> &, const MotionTpl<S,O> & );
+ // template<typename S,int O>
+ // friend ForceTpl<S,O> operator^( const MotionTpl<S,O> &, const ForceTpl<S,O> & );
+
+ MotionTpl se3Action(const SE3 & m) const
+ {
+ Vector3 Rw = static_cast<Vector3>(m.rotation() * angular());
+ return MotionTpl(m.rotation()*linear() + m.translation().cross(Rw), Rw);
+ }
+ /// bv = aXb.actInv(av)
+ MotionTpl se3ActionInverse(const SE3 & m) const
+ {
+ return MotionTpl(m.rotation().transpose()*(linear()-m.translation().cross(angular())),
+ m.rotation().transpose()*angular());
+ }
+
+
+
+
+ friend std::ostream & operator << (std::ostream & os, const MotionTpl & mv)
+ {
+ os << "v =\n" << mv.linear() << std::endl
+ << "w =\n" << mv.angular() << std::endl;
+ return os;
+ }
+
+ public:
+ private:
+ Vector3 m_w;
+ Vector3 m_v;
+ };
+
+ typedef MotionTpl<double> Motion;
+
+} // namespace se3
+
+#endif // ifndef __se3_motion_hpp__
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a1454f4ee97596a70c823c0518dd4049ddb7d948
--- /dev/null
+++ b/src/spatial/se3.hpp
@@ -0,0 +1,130 @@
+#ifndef __se3_se3_hpp__
+#define __se3_se3_hpp__
+
+#include <Eigen/Geometry>
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/skew.hpp"
+
+namespace se3
+{
+
+ /** The rigid transform aMb can be seen in two ways:
+ *
+ * - given a point p expressed in frame B by its coordinate vector Bp, aMb
+ * computes its coordinates in frame A by Ap = aMb Bp.
+ * - aMb displaces a solid S centered at frame A into the solid centered in
+ * B. In particular, the origin of A is displaced at the origin of B: $^aM_b
+ * ^aA = ^aB$.
+
+ * The rigid displacement is stored as a rotation matrix and translation vector by:
+ * aMb (x) = aRb*x + aAB
+ * where aAB is the vector from origin A to origin B expressed in coordinates A.
+ */
+
+ template<typename _Scalar, int _Options>
+ class SE3Tpl
+ {
+ public:
+
+ typedef _Scalar Scalar;
+ enum { Options = _Options };
+ typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+ typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
+ typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+ typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
+ typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
+ typedef Eigen::Quaternion<Scalar,Options> Quaternion;
+ typedef MotionTpl<Scalar,Options> Motion;
+ typedef ForceTpl<Scalar,Options> Force;
+ //typedef ActionTpl<Scalar,Options> Action;
+ enum { LINEAR = 0, ANGULAR = 3 };
+
+ public:
+ // Constructors
+ SE3Tpl() : rot(), trans() {}
+ SE3Tpl(const Matrix3 & R, const Vector3 & p) : rot(R), trans(p) {}
+ SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
+
+ const Matrix3 & rotation() const { return rot; }
+ const Vector3 & translation() const { return trans; }
+ void rotation(const Matrix3 & R) { rot=R; }
+ void translation(const Vector3 & p) { trans=p; }
+
+ static SE3Tpl Identity()
+ {
+ return SE3Tpl(1);
+ }
+ static SE3Tpl Random()
+ {
+ Eigen::Quaternion<Scalar,Options> q(Vector4::Random());
+ q.normalize();
+ return SE3Tpl(q.matrix(),Vector3::Random());
+ }
+
+ Eigen::Matrix<Scalar,4,4,Options> toHomogeneousMatrix() const
+ {
+ Eigen::Matrix<Scalar,4,4,Options> M;
+ M.template block<3,3>(0,0) = rot;
+ M.template block<3,1>(0,3) = trans;
+ M.template block<1,3>(3,0).setZero();
+ M(3,3) = 1;
+ return M;
+ }
+
+ /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
+ Matrix6 toActionMatrix() const
+ {
+ Matrix6 M;
+ M.template block<3,3>(ANGULAR,ANGULAR)
+ = M.template block<3,3>(LINEAR,LINEAR) = rot;
+ M.template block<3,3>(ANGULAR,LINEAR).setZero();
+ M.template block<3,3>(LINEAR,ANGULAR)
+ = skew(trans) * M.template block<3,3>(ANGULAR,ANGULAR);
+ return M;
+ }
+
+ /// aXb = bXa.inverse()
+ SE3Tpl inverse() const
+ {
+ return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
+ }
+
+ void disp(std::ostream & os)
+ {
+ os << " R =\n" << rot << std::endl
+ << " p =\n" << trans.transpose() << std::endl;
+ }
+
+
+ /* --- GROUP ACTIONS ON M6, F6 and I6 --- */
+
+ /// ay = aXb.act(by)
+ template<typename D> D act (const D & d) const { return d.se3Action(*this); }
+ /// by = aXb.actInv(ay)
+ template<typename D> D actInv(const D & d) const { return d.se3ActionInverse(*this); }
+
+ Vector3 act (const Vector3& p) const { return (rot*p+trans).eval(); }
+ Vector3 actInv(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }
+
+ SE3Tpl act (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
+ SE3Tpl actInv (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot,
+ rot.transpose()*(m2.trans-trans));}
+
+ /* --- OPERATORS -------------------------------------------------------- */
+ operator Eigen::Matrix<Scalar,4,4,Options> () const { return toHomogeneousMatrix(); }
+ operator Matrix6() const { return toActionMatrix(); }
+ SE3Tpl operator*(const SE3Tpl & m2) const { return this->act(m2); }
+ friend std::ostream & operator << (std::ostream & os,const SE3Tpl & X) { X.disp(os); return os; }
+
+ public:
+ private:
+ Matrix3 rot;
+ Vector3 trans;
+ };
+
+ typedef SE3Tpl<double,0> SE3;
+
+} // namespace se3
+
+#endif // ifndef __se3_se3_hpp__
+
diff --git a/src/spatial/skew.hpp b/src/spatial/skew.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c0241613f2907138768366c5c762e17a3a71917
--- /dev/null
+++ b/src/spatial/skew.hpp
@@ -0,0 +1,14 @@
+namespace se3
+{
+ template <typename D>
+ inline Eigen::Matrix<typename D::Scalar,3,3,D::Options>
+ skew(const Eigen::MatrixBase<D> & v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ Eigen::Matrix<typename D::Scalar,3,3,D::Options> m;
+ m(0,0) = 0 ; m(0,1) = -v[2]; m(0,2) = v[1];
+ m(1,0) = v[2]; m(1,1) = 0 ; m(1,2) = -v[0];
+ m(2,0) = -v[1]; m(2,1) = v[0]; m(2,2) = 0 ;
+ return m;
+ }
+} // namespace se3
diff --git a/src/tools/timer.hpp b/src/tools/timer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b741e2c2c1560da2222ec9b1eba913ab59cc33f
--- /dev/null
+++ b/src/tools/timer.hpp
@@ -0,0 +1,42 @@
+#include <sys/time.h>
+#include <iostream>
+#include <stack>
+
+#define SMOOTH(s) for(int _smooth=0;_smooth<s;++_smooth)
+
+/* Return the time spent in secs. */
+inline double operator- ( const struct timeval & t1,const struct timeval & t0)
+{
+ return (t1.tv_sec - t0.tv_sec)+1e-6*(t1.tv_usec - t0.tv_usec);
+}
+
+struct StackTicToc
+{
+ enum Unit { S = 1, MS = 1000, US = 1000000 };
+ Unit DEFAULT_UNIT;
+ static std::string unitName(Unit u)
+ { switch(u) { case S: return "s"; case MS: return "ms"; case US: return "us"; } return ""; }
+
+ std::stack<struct timeval> stack;
+ mutable struct timeval t0;
+
+StackTicToc( Unit def = MS ) : DEFAULT_UNIT(def) {}
+
+ inline void tic() {
+ stack.push(t0);
+ gettimeofday(&(stack.top()),NULL);
+ }
+
+ inline double toc(const Unit factor)
+ {
+ gettimeofday(&t0,NULL);
+ double dt = (t0-stack.top())*factor;
+ stack.pop();
+ return dt;
+ }
+ inline void toc( std::ostream& os, double SMOOTH=1 )
+ {
+ os << toc(DEFAULT_UNIT)/SMOOTH << " " << unitName(DEFAULT_UNIT) << std::endl;
+ }
+};
+
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..30907ea1d25bc570c18574e3e0a76d3d2d47c33d
--- /dev/null
+++ b/unittest/rnea.cpp
@@ -0,0 +1,106 @@
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+
+
+int main()
+{
+ using namespace Eigen;
+ using namespace se3;
+
+
+ se3::Model model;
+ model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Random(),Inertia::Random(),"root");
+
+
+ model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg1");
+ model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg2");
+ model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg3");
+ model.addBody(model.getBodyId("rleg3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg4");
+ model.addBody(model.getBodyId("rleg4"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg5");
+ model.addBody(model.getBodyId("rleg5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg6");
+
+ model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg1");
+ model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg2");
+ model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg3");
+ model.addBody(model.getBodyId("lleg3"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg4");
+ model.addBody(model.getBodyId("lleg4"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg5");
+ model.addBody(model.getBodyId("lleg5"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg6");
+
+ model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso1");
+ model.addBody(model.getBodyId("torso1"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso2");
+ model.addBody(model.getBodyId("torso2"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso3");
+ model.addBody(model.getBodyId("torso3"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck1");
+ model.addBody(model.getBodyId("neck1"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck2");
+ model.addBody(model.getBodyId("neck2"),JointModelRX(),SE3::Random(),Inertia::Random(),"neck3");
+
+ model.addBody(model.getBodyId("torso3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm1");
+ model.addBody(model.getBodyId("rarm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm2");
+ model.addBody(model.getBodyId("rarm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm3");
+ model.addBody(model.getBodyId("rarm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm4");
+ model.addBody(model.getBodyId("rarm4"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm5");
+ model.addBody(model.getBodyId("rarm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm6");
+ model.addBody(model.getBodyId("rarm6"),JointModelRX(),SE3::Random(),Inertia::Random(),"rgrip");
+
+ model.addBody(model.getBodyId("torso3"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm1");
+ model.addBody(model.getBodyId("larm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm2");
+ model.addBody(model.getBodyId("larm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm3");
+ model.addBody(model.getBodyId("larm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm4");
+ model.addBody(model.getBodyId("larm4"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm5");
+ model.addBody(model.getBodyId("larm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm6");
+ model.addBody(model.getBodyId("larm6"),JointModelRX(),SE3::Random(),Inertia::Random(),"lgrip");
+
+ se3::Data data(model);
+
+ VectorXd q = VectorXd::Random(model.nq);
+ VectorXd v = VectorXd::Random(model.nv);
+ VectorXd a = VectorXd::Random(model.nv);
+
+ data.v[0] = Motion::Zero();
+ data.a[0] = -model.gravity;
+
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(1000)
+ {
+ for( int i=1;i<model.nbody;++i )
+ {
+ JointModelRX & jmodel = model.joints[i];
+ JointDataRX & jdata = data.joints[i];
+ jmodel.calc(jdata,q,v);
+ VectorBlock<VectorXd> qdd = a.segment(jmodel.idx_v,jmodel.nv);
+
+ const Model::Index & parent = model.parents[i];
+ const SE3 & liMi = data.liMi[i] = model.jointPlacements[i]*jdata.M;
+
+ if(parent>0) data.oMi[i] = data.oMi[parent]*liMi;
+ else data.oMi[i] = liMi;
+
+ data.v[i] = jdata.v;
+ if(parent>0) data.v[i] += liMi.actInv(data.v[parent]);
+
+ data.a[i] = Motion(jdata.S*qdd) + jdata.c + data.v[i].cross(jdata.v);
+ if(parent>0) data.a[i] += liMi.actInv(data.a[parent]);
+
+ data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
+ }
+
+ for( int i=model.nbody-1;i>0;--i )
+ {
+ const Model::Index & parent = model.parents[i];
+ JointModelRX & jmodel = model.joints[i];
+
+ data.tau.segment(jmodel.idx_v,jmodel.nv) = data.joints[i].S.transpose()*data.f[i].toVector();
+ if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
+
+ }
+ }
+ timer.toc(std::cout,1000);
+
+ return 0;
+}
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d771a42d811c9c3cbe10e308bc5347e596569c2c
--- /dev/null
+++ b/unittest/tspatial.cpp
@@ -0,0 +1,235 @@
+#include <iostream>
+
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+
+bool testSE3()
+{
+ using namespace se3;
+ typedef Eigen::Matrix<double,4,4> Matrix4;
+ typedef SE3::Matrix6 Matrix6;
+ typedef SE3::Vector3 Vector3;
+
+ SE3 amb = SE3::Random();
+ SE3 bmc = SE3::Random();
+ SE3 amc = amb*bmc;
+
+ Matrix4 aMb = amb;
+ Matrix4 bMc = bmc;
+
+ // Test internal product
+ Matrix4 aMc = amc;
+ assert(aMc.isApprox(aMb*bMc));
+
+ Matrix4 bMa = amb.inverse();
+ assert(bMa.isApprox(aMb.inverse()));
+
+ { // Test point action
+ Vector3 p = Vector3::Random();
+ Eigen::Matrix<double,4,1> p4; p4.head(3) = p; p4[3] = 1;
+
+ Vector3 Mp = (aMb*p4).head(3);
+ assert(amb.act(p).isApprox(Mp));
+ Vector3 Mip = (aMb.inverse()*p4).head(3);
+ assert(amb.actInv(p).isApprox(Mip));
+ }
+
+ { // Test action matrix
+ Matrix6 aXb = amb;
+ Matrix6 bXc = bmc;
+ Matrix6 aXc = amc;
+ assert(aXc.isApprox(aXb*bXc));
+
+ Matrix6 bXa = amb.inverse();
+ assert(bXa.isApprox(aXb.inverse()));
+ }
+
+
+ return true;
+}
+
+bool testMotion()
+{
+ using namespace se3;
+ typedef Eigen::Matrix<double,4,4> Matrix4;
+ typedef SE3::Matrix6 Matrix6;
+ typedef SE3::Vector3 Vector3;
+ typedef Motion::Vector6 Vector6;
+
+ SE3 amb = SE3::Random();
+ SE3 bmc = SE3::Random();
+ SE3 amc = amb*bmc;
+
+ Motion bv = Motion::Random();
+ Motion bv2 = Motion::Random();
+
+ Vector6 bv_vec = bv;
+ Vector6 bv2_vec = bv2;
+
+ // Test .+.
+ Vector6 bvPbv2_vec = bv+bv2;
+ assert( bvPbv2_vec.isApprox(bv_vec+bv2_vec) );
+ // Test -.
+ Vector6 Mbv_vec = -bv;
+ assert( Mbv_vec.isApprox(-bv_vec) );
+ // Test .+=.
+ Motion bv3 = bv; bv3 += bv2;
+ assert(bv3.toVector().isApprox(bv_vec+bv2_vec));
+ // Test .=V6
+ bv3 = bv2_vec;
+ assert(bv3.toVector().isApprox(bv2_vec));
+ // Test constructor from V6
+ Motion bv4(bv2_vec);
+ assert(bv4.toVector().isApprox(bv2_vec));
+
+ // Test action
+ Matrix6 aXb = amb;
+ assert( amb.act(bv).toVector().isApprox(aXb*bv_vec));
+ // Test action inverse
+ Matrix6 bXc = bmc;
+ assert( bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec));
+
+ // Test double action
+ Motion cv = Motion::Random();
+ bv = bmc.act(cv);
+ assert( amb.act(bv).toVector().isApprox(amc.act(cv).toVector()) );
+
+ // Simple test for cross product vxv
+ Motion vxv = bv.cross(bv);
+ assert(vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
+
+ // Simple test for cross product vxf
+ Force f = bv.toVector();
+ Force vxf = bv.cross(f);
+ assert( vxf.linear().isApprox( bv.angular().cross(f.linear())));
+ assert( vxf.angular().isMuchSmallerThan(1e-3));
+
+ // Test frame change for vxf
+ Motion av = Motion::Random();
+ Force af = Force::Random();
+ bv = amb.actInv(av);
+ Force bf = amb.actInv(af);
+ Force avxf = av.cross(af);
+ Force bvxf = bv.cross(bf);
+ assert( avxf.toVector().isApprox( amb.act(bvxf).toVector()) );
+
+ // Test frame change for vxv
+ av = Motion::Random();
+ Motion aw = Motion::Random();
+ bv = amb.actInv(av);
+ Motion bw = amb.actInv(aw);
+ Motion avxw = av.cross(aw);
+ Motion bvxw = bv.cross(bw);
+ assert( avxw.toVector().isApprox( amb.act(bvxw).toVector()) );
+
+ return true;
+}
+
+
+bool testForce()
+{
+ using namespace se3;
+ typedef Eigen::Matrix<double,4,4> Matrix4;
+ typedef SE3::Matrix6 Matrix6;
+ typedef SE3::Vector3 Vector3;
+ typedef Force::Vector6 Vector6;
+
+ SE3 amb = SE3::Random();
+ SE3 bmc = SE3::Random();
+ SE3 amc = amb*bmc;
+
+ Force bf = Force::Random();
+ Force bf2 = Force::Random();
+
+ Vector6 bf_vec = bf;
+ Vector6 bf2_vec = bf2;
+
+ // Test .+.
+ Vector6 bfPbf2_vec = bf+bf2;
+ assert( bfPbf2_vec.isApprox(bf_vec+bf2_vec) );
+ // Test -.
+ Vector6 Mbf_vec = -bf;
+ assert( Mbf_vec.isApprox(-bf_vec) );
+ // Test .+=.
+ Force bf3 = bf; bf3 += bf2;
+ assert(bf3.toVector().isApprox(bf_vec+bf2_vec));
+ // Test .= V6
+ bf3 = bf2_vec;
+ assert(bf3.toVector().isApprox(bf2_vec));
+ // Test constructor from V6
+ Force bf4(bf2_vec);
+ assert(bf4.toVector().isApprox(bf2_vec));
+
+ // Test action
+ Matrix6 aXb = amb;
+ assert( amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec));
+ // Test action inverse
+ Matrix6 bXc = bmc;
+ assert( bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec));
+ // Test double action
+ Force cf = Force::Random();
+ bf = bmc.act(cf);
+ assert( amb.act(bf).toVector().isApprox(amc.act(cf).toVector()) );
+
+ // Simple test for cross product
+ // Force vxv = bf.cross(bf);
+ // assert(vxv.toVector().isMuchSmallerThan(bf.toVector()));
+
+ return true;
+}
+
+bool testInertia()
+{
+ using namespace se3;
+ typedef Inertia::Matrix6 Matrix6;
+ typedef Inertia::Matrix3 Matrix3;
+ typedef Inertia::Vector3 Vector3;
+ typedef Eigen::Matrix<double,4,4> Matrix4;
+
+ Inertia aI = Inertia::Random();
+ Matrix6 matI = aI;
+ assert( (matI(0,0) == aI.mass())
+ && (matI(1,1) == aI.mass())
+ && (matI(1,1) == aI.mass()) );
+ assert( (matI-matI.transpose()).isMuchSmallerThan(matI) );
+ assert( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
+
+ Inertia I1 = Inertia::Identity();
+ assert( I1.toMatrix() == Matrix6::Identity() );
+
+ // Test motion-to-force map
+ Motion v = Motion::Random();
+ Force f = I1 * v;
+ assert( f.toVector() == v.toVector() );
+
+ // Test Inertia group application
+ SE3 bma = SE3::Random();
+ Inertia bI = bma.act(aI);
+ Matrix6 bXa = bma;
+ assert( (bma.rotation()*aI.inertia()*bma.rotation().transpose()).isApprox((Matrix3)bI.inertia()) );
+ assert( (bXa.transpose().inverse() * aI.toMatrix() * bXa.inverse()).isApprox( bI.toMatrix()) );
+
+ // Test inverse action
+ assert( (bXa.transpose() * bI.toMatrix() * bXa).isApprox( bma.actInv(bI).toMatrix()) );
+
+ // Test vxIv cross product
+ v = Motion::Random();
+ f = aI*v;
+ Force vxf = v.cross(f);
+ Force vxIv = aI.vxiv(v);
+ assert( vxf.toVector().isApprox(vxIv.toVector()) );
+
+ return true;
+}
+
+int main()
+{
+ testSE3();
+ testMotion();
+ testForce();
+ testInertia();
+ return 1;
+}
+