Merge branch 'topic/sot-pinocchio'

[python] Remove Pinocchio wrapper and take pinocchio model and data directly as input
[python] Update Class HumanoidRobot
[c++] Deal with spherical Joints
[c++] Add kineromeo.py tutorial (uses gepetto-viewer for visualization)

Conflicts:
	CMakeLists.txt

.gitignore

 *.pyc
+*~
 build
 .gitignore

.travis.yml

@@ -8,16 +8,22 @@ after_success:
 branches:
   only:
   - master
+  - topic/sot-pinocchio
 compiler:
 - clang
 - gcc
 matrix:
   allow_failures:
   - compiler: clang
+
 before_install:
 - git submodule update --init --recursive
+#Add robotpkg and pinocchio dependencies
+- sudo sh -c "echo \"deb http://robotpkg.openrobots.org/packages/debian precise robotpkg\" >> /etc/apt/sources.list "
+- curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
 - sudo apt-get update -qq
-- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev python-sphinx python-numpy
+- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev 
+  python-sphinx python-numpy libtinyxml-dev robotpkg-console-bridge robotpkg-urdfdom-headers robotpkg-urdfdom robotpkg-eigenpy
 - sudo pip install cpp-coveralls
 language: cpp
 notifications:

.travis/build

@@ -14,10 +14,11 @@ rm -rf "$build_dir" "$install_dir"
 mkdir -p "$build_dir"
 mkdir -p "$install_dir"
 
+
 # Setup environment variables.
-export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$install_dir/lib:/opt/openrobots/lib:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
-export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
+export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH"
 export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
 
 install_dependency()
@@ -31,11 +32,18 @@ install_dependency()
     make install
 }
 
-# Retrieve jrl-mathtools
-install_dependency jrl-umi3218/jrl-mathtools
-install_dependency jrl-umi3218/jrl-mal
-install_dependency laas/abstract-robot-dynamics
-install_dependency jrl-umi3218/jrl-dynamics
+install_pinocchio_devel()
+{
+    echo "--> Compiling stack-of-tasks/pinocchio"
+    mkdir -p "$build_dir/stack-of-tasks/pinocchio"
+    cd "$build_dir"
+    $git_clone -b devel git://github.com/stack-of-tasks/pinocchio stack-of-tasks/pinocchio
+    cd "$build_dir/stack-of-tasks/pinocchio"
+    cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+    make install
+}
+
+install_pinocchio_devel
 install_dependency proyan/dynamic-graph
 install_dependency proyan/dynamic-graph-python
 install_dependency proyan/sot-core

CMakeLists.txt

@@ -23,8 +23,8 @@ INCLUDE(cmake/cpack.cmake)
 
 SET(PROJECT_NAME sot-dynamic)
 
-SET(PROJECT_DESCRIPTION "jrl-dynamics bindings for dynamic-graph.")
-SET(PROJECT_URL "http://github.com/stack-of-tasks/sot-dynamic")
+SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
+SET(PROJECT_URL "https://github.com/proyan/sot-dynamic/tree/topic/sot-pinocchio")
 
 SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
 
@@ -38,9 +38,9 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
   )
 
 SETUP_PROJECT()
-
 # Search for dependencies.
-ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.19.0")
+ADD_REQUIRED_DEPENDENCY("pinocchio")
+ADD_REQUIRED_DEPENDENCY("eigenpy")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
 ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
 ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
@@ -69,7 +69,7 @@ PKG_CONFIG_APPEND_LIBS(${LIBRARY_NAME})
 
 # Search for dependencies.
 # Boost
-SET(BOOST_COMPONENTS filesystem system)
+SET(BOOST_COMPONENTS filesystem system unit_test_framework python)
 SEARCH_FOR_BOOST()
 SEARCH_FOR_EIGEN()
 
@@ -78,7 +78,7 @@ ADD_SUBDIRECTORY(include)
 ADD_SUBDIRECTORY(src)
 ADD_SUBDIRECTORY(doc)
 ADD_SUBDIRECTORY(python)
-ADD_SUBDIRECTORY(unitTesting)
+#ADD_SUBDIRECTORY(unitTesting)
 
 SETUP_PROJECT_FINALIZE()
 SETUP_PROJECT_CPACK()

README.md

 sot-dynamic
 ===========
 
-[![Build Status](https://travis-ci.org/stack-of-tasks/sot-dynamic.png?branch=master)](https://travis-ci.org/stack-of-tasks/sot-dynamic)
-[![Coverage Status](https://coveralls.io/repos/stack-of-tasks/sot-dynamic/badge.png)](https://coveralls.io/r/stack-of-tasks/sot-dynamic)
+[![Build Status](https://travis-ci.org/proyan/sot-dynamic.png?branch=master)](https://travis-ci.org/proyan/sot-dynamic)
+[![Coverage Status](https://coveralls.io/repos/proyan/sot-dynamic/badge.png)](https://coveralls.io/r/proyan/sot-dynamic)
 
 This software provides robot dynamic computation for dynamic-graph
 by using jrl-dynamics.
@@ -28,18 +28,15 @@ The matrix abstract layer depends on several packages which
 have to be available on your machine.
 
  - Libraries:
-   - [jrl-dynamics][jrl-dynamics] (>= 1.16.1)
    - [dynamic-graph][dynamic-graph] (>= 3.0.0)
    - [sot-core][sot-core] (>= 3.0.0)
- - Closed source libraries:
-   - hrp2Dynamics (>= 1.3.0)
-   - hrp2-10-optimized (>= 1.3.0) [optional]
+   - [pinocchio][pinocchio] (>= 1.1.2)
  - System tools:
    - CMake (>=2.6)
    - pkg-config
    - usual compilation tools (GCC/G++, make, etc.)
 
 
-[dynamic-graph]: http://github.com/stack-of-tasks/dynamic-graph
-[jrl-dynamics]: http://github.com/jrl-umi3218/jrl-dynamics
-[sot-core]: http://github.com/stack-of-tasks/sot-core
+[dynamic-graph]: http://github.com/proyan/dynamic-graph
+[pinocchio]: http://github.com/stack-of-tasks/pinocchio
+[sot-core]: http://github.com/proyan/sot-core

custom_cmake/python.cmake

+# Copyright (C) 2008-2016 LAAS-CNRS, JRL AIST-CNRS.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program 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 General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+
+#
+# SOT_DYNAMIC_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME
+# ---------------------------
+#
+# Add a python submodule to dynamic_graph
+#
+#  SUBMODULENAME : the name of the submodule (can be foo/bar),
+#
+#  LIBRARYNAME   : library to link the submodule with.
+#
+#  TARGETNAME    : name of the target: should be different for several
+#                  calls to the macro.
+#
+#  NOTICE : Before calling this macro, set variable NEW_ENTITY_CLASS as
+#           the list of new Entity types that you want to be bound.
+#           Entity class name should match the name referencing the type
+#           in the factory.
+#
+MACRO(SOT_DYNAMIC_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
+  FINDPYTHON()
+  
+  SET(PYTHON_MODULE ${TARGETNAME})
+  
+  ADD_LIBRARY(${PYTHON_MODULE}
+    MODULE
+    ${PROJECT_SOURCE_DIR}/src/python-module-py.cpp)
+  #${PROJECT_SOURCE_DIR}/src/sot-dynamic-py.cpp)
+  
+  FILE(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME})
+  SET_TARGET_PROPERTIES(${PYTHON_MODULE}
+    PROPERTIES PREFIX ""
+    OUTPUT_NAME dynamic_graph/${SUBMODULENAME}/wrap
+    )
+  
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} "-Wl,--no-as-needed")
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${LIBRARYNAME} ${PYTHON_LIBRARY})
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${Boost_LIBRARIES})
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} pinocchio)
+  TARGET_LINK_LIBRARIES(${PYTHON_MODULE} eigenpy)
+
+  PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} dynamic-graph)
+  PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} pinocchio)
+  PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} eigenpy)
+  
+  INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
+  
+  #
+  # Installation
+  #
+  SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/dynamic_graph/${SUBMODULENAME})
+
+  INSTALL(TARGETS ${PYTHON_MODULE}
+    DESTINATION
+    ${PYTHON_INSTALL_DIR})
+
+  SET(ENTITY_CLASS_LIST "")
+  FOREACH (ENTITY ${NEW_ENTITY_CLASS})
+    SET(ENTITY_CLASS_LIST "${ENTITY_CLASS_LIST}${ENTITY}('')\n")
+  ENDFOREACH(ENTITY ${NEW_ENTITY_CLASS})
+
+  CONFIGURE_FILE(
+    ${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/submodule/__init__.py.cmake
+    ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME}/__init__.py
+    )
+
+  INSTALL(
+    FILES ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME}/__init__.py
+    DESTINATION ${PYTHON_INSTALL_DIR}
+    )
+
+ENDMACRO(DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME)
\ No newline at end of file

include/sot-dynamic/matrix-inertia.h

@@ -29,13 +29,6 @@
 
 #include <sot/core/matrix-geometry.hh>
 
-namespace dynamicsJRLJapan
-{
-  class HumanoidDynamicMultiBody;
-}
-
-class CjrlHumanoidDynamicRobot;
-class CjrlJoint;
 
 /* --------------------------------------------------------------------- */
 /* --- API ------------------------------------------------------------- */

src/CMakeLists.txt

@@ -14,6 +14,7 @@
 # received a copy of the GNU Lesser General Public License along with
 # sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
 
+INCLUDE(../custom_cmake/python.cmake)
 INCLUDE(../cmake/python.cmake)
 LINK_DIRECTORIES(${Boost_LIBRARY_DIRS})
 
@@ -29,7 +30,6 @@ ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
 SET(integrator-force-rk4_plugins_dependencies integrator-force)
 SET(integrator-force-exact_plugins_dependencies integrator-force)
 
-
 FOREACH(lib ${plugins})
   ADD_LIBRARY(${lib} SHARED ${lib}.cpp)
 
@@ -44,7 +44,8 @@ FOREACH(lib ${plugins})
 
   TARGET_LINK_LIBRARIES(${lib} ${Boost_LIBRARIES})
 
-  PKG_CONFIG_USE_DEPENDENCY(${lib} jrl-dynamics)
+  PKG_CONFIG_USE_DEPENDENCY(${lib} pinocchio)
+  PKG_CONFIG_USE_DEPENDENCY(${lib} eigenpy)
   PKG_CONFIG_USE_DEPENDENCY(${lib} sot-core)
   PKG_CONFIG_USE_DEPENDENCY(${lib} dynamic-graph)
 
@@ -53,7 +54,7 @@ FOREACH(lib ${plugins})
   # build python submodule
   STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
 
-  DYNAMIC_GRAPH_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
+  SOT_DYNAMIC_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
     ${lib}
     sot-dynamics-${PYTHON_LIBRARY_NAME}-wrap
     )
@@ -61,13 +62,13 @@ ENDFOREACH(lib)
 
 # Main Library
 ADD_LIBRARY(${LIBRARY_NAME} SHARED sot-dynamic.cpp)
-TARGET_LINK_LIBRARIES(${LIBRARY_NAME} jrl-dynamics)
+TARGET_LINK_LIBRARIES(${LIBRARY_NAME} pinocchio)
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} sot-core)
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dynamic-graph)
-PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} jrl-dynamics)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
 PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} sot-core)
 PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
-
+TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
 INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
 TARGET_LINK_LIBRARIES(dynamic ${LIBRARY_NAME})
@@ -79,5 +80,4 @@ INSTALL(FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/tools.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/parser.py
   DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics
-  )
-
+  )
\ No newline at end of file

src/angle-estimator-command.h

@@ -35,7 +35,10 @@ namespace dynamicgraph { namespace sot {
     class FromSensor : public Command
     {
     public:
-      virtual ~FromSensor() {}
+      virtual ~FromSensor() {
+	sotDEBUGIN(15);
+	sotDEBUGOUT(15);
+      }
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command

src/dynamic-command.h

@@ -32,124 +32,16 @@ namespace dynamicgraph { namespace sot {
     using ::dynamicgraph::command::Command;
     using ::dynamicgraph::command::Value;
 
-    // Command SetFiles
-    class SetFiles : public Command
+    // Command DisplayModel
+    class DisplayModel : public Command
     {
     public:
-      virtual ~SetFiles() {}
+      virtual ~DisplayModel() {	sotDEBUGIN(15);
+	sotDEBUGOUT(15);}
       /// Create command and store it in Entity
       /// \param entity instance of Entity owning this command
       /// \param docstring documentation of the command
-      SetFiles(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)
-		(Value::STRING)(Value::STRING)(Value::STRING), docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string vrmlDirectory = values[0].value();
-	std::string vrmlMainFile = values[1].value();
-	std::string xmlSpecificityFiles = values[2].value();
-	std::string xmlRankFile = values[3].value();
-	robot.setVrmlDirectory(vrmlDirectory);
-	robot.setVrmlMainFile(vrmlMainFile);
-	robot.setXmlSpecificityFile(xmlSpecificityFiles);
-	robot.setXmlRankFile(xmlRankFile);
-	// return void
-	return Value();
-      }
-    }; // class SetFiles
-
-    // Command Parse
-    class Parse : public Command
-    {
-    public:
-      virtual ~Parse() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      Parse(Dynamic& entity, const std::string& docstring) :
-	Command(entity, std::vector<Value::Type>(), docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	if(! robot.init ) robot.parseConfigFiles();
-	else std::cout << "  !! Already parsed." << std::endl;
-	// return void
-	return Value();
-      }
-    }; // class Parse
-
-    // Command SetProperty
-    class SetProperty : public Command
-    {
-    public:
-      virtual ~SetProperty() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      SetProperty(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
-		docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string property = values[0].value();
-	std::string value = values[1].value();
-
-	robot.m_HDR->setProperty(property, value);
-	return Value();
-      }
-    }; // class SetProperty
-
-    // Command GetProperty
-    class GetProperty : public Command
-    {
-    public:
-      virtual ~GetProperty() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      GetProperty(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING),
-		docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string property = values[0].value();
-	std::string value;
-
-	if(! robot.m_HDR->getProperty(property, value) )
-	  {
-	    if( property == "vrmlDirectory" ) value = robot.vrmlDirectory;
-	    else if( property == "xmlSpecificityFile" ) value = robot.xmlSpecificityFile;
-	    else if( property == "xmlRankFile" ) value = robot.xmlRankFile;
-	    else if( property == "vrmlMainFile" ) value = robot.vrmlMainFile;
-	  }
-
-	return Value(value);
-      }
-    }; // class GetProperty
-
-    // Command CreateRobot
-    class CreateRobot : public Command
-    {
-    public:
-      virtual ~CreateRobot() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      CreateRobot(Dynamic& entity, const std::string& docstring) :
+      DisplayModel(Dynamic& entity, const std::string& docstring) :
 	Command(entity, std::vector<Value::Type>(),
 		docstring)
       {
@@ -157,307 +49,18 @@ namespace dynamicgraph { namespace sot {
       virtual Value doExecute()
       {
 	Dynamic& robot = static_cast<Dynamic&>(owner());
-	robot.createRobot();
-	return Value();
-      }
-    }; // class CreateRobot
-
-    // Command CreateJoint
-    class CreateJoint : public Command
-    {
-    public:
-      virtual ~CreateJoint() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      CreateJoint(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)
-		(Value::MATRIX), docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string jointName = values[0].value();
-	std::string jointType = values[1].value();
-	dynamicgraph::Matrix position = values[2].value();
-	robot.createJoint(jointName, jointType, position);
-	return Value();
-      }
-    }; // class CreateJoint
-
-    // Command SetRootJoint
-    class SetRootJoint : public Command
-    {
-    public:
-      virtual ~SetRootJoint() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      SetRootJoint(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING), docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string jointName = values[0].value();
-	robot.setRootJoint(jointName);
-	return Value();
-      }
-    }; // class SetRootJoint
-
-    // Command AddJoint
-    class AddJoint : public Command
-    {
-    public:
-      virtual ~AddJoint() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      AddJoint(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
-		docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string parentName = values[0].value();
-	std::string childName = values[1].value();
-	robot.addJoint(parentName, childName);
-	return Value();
-      }
-    }; // class AddJoint
-
-    // Command SetDofBounds
-    class SetDofBounds : public Command
-    {
-    public:
-      virtual ~SetDofBounds() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      SetDofBounds(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::UNSIGNED)
-		(Value::DOUBLE)(Value::DOUBLE), docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string jointName = values[0].value();
-	unsigned int dofId = values[1].value();
-	double minValue = values[2].value();
-	double maxValue = values[3].value();
-	robot.setDofBounds(jointName, dofId, minValue, maxValue);
-	return Value();
-      }
-    }; // class SetDofBounds
-
-    // Command SetMass
-    class SetMass : public Command
-    {
-    public:
-      virtual ~SetMass() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      SetMass(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::DOUBLE),
-		docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string jointName = values[0].value();
-	double mass = values[1].value();
-	robot.setMass(jointName, mass);
-	return Value();
-      }
-    }; // class SetMass
-
-    // Command SetLocalCenterOfMass
-    class SetLocalCenterOfMass : public Command
-    {
-    public:
-      virtual ~SetLocalCenterOfMass() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      SetLocalCenterOfMass(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::VECTOR),
-		docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string jointName = values[0].value();
-	dynamicgraph::Vector com = values[1].value();
-	robot.setLocalCenterOfMass(jointName, com);
-	return Value();
-      }
-    }; // class SetLocalCenterOfMass
-
-    // Command SetInertiaMatrix
-    class SetInertiaMatrix : public Command
-    {
-    public:
-      virtual ~SetInertiaMatrix() {}
-      /// Create command and store it in Entity
-      /// \param entity instance of Entity owning this command
-      /// \param docstring documentation of the command
-      SetInertiaMatrix(Dynamic& entity, const std::string& docstring) :
-	Command(entity, boost::assign::list_of(Value::STRING)(Value::MATRIX),
-		docstring)
-      {
-      }
-      virtual Value doExecute()
-      {
-	Dynamic& robot = static_cast<Dynamic&>(owner());
-	std::vector<Value> values = getParameterValues();
-	std::string jointName = values[0].value();
-	dynamicgraph::Matrix