diff --git a/.version b/.version
new file mode 100644
index 0000000000000000000000000000000000000000..eca690e737b32fd51aab5f8ebbc5f16e61c84aee
--- /dev/null
+++ b/.version
@@ -0,0 +1 @@
+3.0.5
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7a7ff8a5710de2ddd071926f9dfdfd19c41ee40e..8dcb45fd8a477579a0b88a0c04032b2025a8bcc2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,14 +17,20 @@
 
 # Catkin part
 
+
 cmake_minimum_required(VERSION 2.4.6)
 
+include(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/eigen.cmake)
+include(cmake/ros.cmake)
+include(cmake/GNUInstallDirs.cmake)
+include(cmake/python.cmake)
+
 project(dynamic_graph_bridge)
 
 find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf)
 find_package(realtime_tools)
-find_package(Boost REQUIRED COMPONENTS program_options)
-
 
 ## LAAS cmake submodule part
 set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
@@ -39,10 +45,8 @@ set(${PROJECT_NAME}_HEADERS
   include/dynamic_graph_bridge/sot_loader.hh
   include/dynamic_graph_bridge/sot_loader_basic.hh
   )
-include(cmake/base.cmake)
-include(cmake/ros.cmake)
-include(cmake/GNUInstallDirs.cmake)
-include(cmake/python.cmake)
+SEARCH_FOR_EIGEN()
+SEARCH_FOR_BOOST()
 
 SETUP_PROJECT()
 
@@ -58,17 +62,20 @@ set(PKG_CONFIG_ADDITIONAL_VARIABLES
 
 # Add dependency to SoT specific packages.
 SET(SOT_PKGNAMES
-jrl-mal
-dynamic-graph
-dynamic-graph-python
-sot-core
-sot-dynamic
-jrl-dynamics-urdf
 dynamic_graph_bridge_msgs)
 
 add_required_dependency(roscpp)
+add_required_dependency(tf)
 add_required_dependency("realtime_tools >= 1.8")
-add_required_dependency(bullet)
+add_required_dependency(tf2_bullet)
+ADD_REQUIRED_DEPENDENCY("pinocchio")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-dynamic-pinocchio >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
+
+add_required_dependency(dynamic_graph_bridge_msgs)
 
 foreach(sot_pkgname ${SOT_PKGNAMES})
   add_required_dependency(${sot_pkgname})
@@ -82,8 +89,7 @@ add_library(ros_bridge
   include/dynamic_graph_bridge/ros_init.hh src/ros_init.cpp
   src/sot_to_ros.hh src/sot_to_ros.cpp
   )
-pkg_config_use_dependency(ros_bridge jrl-mal)
-pkg_config_use_dependency(ros_bridge bullet)
+pkg_config_use_dependency(ros_bridge tf2_bullet)
 pkg_config_use_dependency(ros_bridge dynamic_graph_bridge_msgs)
 install(TARGETS ros_bridge DESTINATION lib)
 
@@ -99,10 +105,9 @@ macro(compile_plugin NAME)
   message(lib path ${LIBRARY_OUTPUT_PATH})
   file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${NAME}")
   add_library(${NAME} SHARED src/${NAME}.cpp src/${NAME}.hh)
-  pkg_config_use_dependency(${NAME} jrl-mal)
   pkg_config_use_dependency(${NAME} dynamic-graph)
   pkg_config_use_dependency(${NAME} sot-core)
-  pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
+  #pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
   pkg_config_use_dependency(${NAME} dynamic_graph_bridge_msgs)
   add_dependencies(${NAME} ros_bridge)
   target_link_libraries(${NAME} ros_bridge)
@@ -117,13 +122,12 @@ macro(compile_plugin NAME)
     )
 
   PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap realtime_tools)
-  PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap jrl-mal)
   PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph)
   PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap sot-core)
   PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph_bridge_msgs)
 endmacro()
 
-include(cmake/python.cmake)
+#include(cmake/python.cmake)
 
 # Build Sot Entities
 compile_plugin(ros_publish)
@@ -131,13 +135,13 @@ compile_plugin(ros_subscribe)
 compile_plugin(ros_time)
 compile_plugin(ros_joint_state)
 
-target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
+target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dp-dynamic.so")
+target_link_libraries(ros_publish ros_bridge)
 
-compile_plugin(robot_model)
+#compile_plugin(robot_model)
 
 # ros_interperter library.
 add_library(ros_interpreter src/ros_interpreter.cpp)
-pkg_config_use_dependency(ros_interpreter jrl-mal)
 pkg_config_use_dependency(ros_interpreter dynamic-graph)
 pkg_config_use_dependency(ros_interpreter sot-core)
 pkg_config_use_dependency(ros_interpreter roscpp)
@@ -154,27 +158,25 @@ install(TARGETS ros_interpreter DESTINATION lib)
 add_executable(interpreter src/interpreter.cpp)
 add_dependencies(interpreter ros_interpreter)
 target_link_libraries(interpreter ros_interpreter)
-pkg_config_use_dependency(interpreter jrl-mal)
 pkg_config_use_dependency(interpreter dynamic-graph)
 pkg_config_use_dependency(interpreter sot-core)
-pkg_config_use_dependency(interpreter sot-dynamic)
+pkg_config_use_dependency(interpreter sot-dynamic-pinocchio)
 pkg_config_use_dependency(interpreter dynamic_graph_bridge_msgs)
 # set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
 #install(TARGETS interpreter DESTINATION bin)
 
 # Stand alone embedded intepreter with a robot controller.
 add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp src/sot_loader_basic.cpp)
-pkg_config_use_dependency(geometric_simu roscpp)
-target_link_libraries(geometric_simu  ros_bridge ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
+pkg_config_use_dependency(geometric_simu roscpp tf)
+target_link_libraries(geometric_simu  ros_bridge tf ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
 
 # Sot loader library
 add_library(sot_loader src/sot_loader.cpp src/sot_loader_basic.cpp)
 pkg_config_use_dependency(sot_loader dynamic-graph)
 pkg_config_use_dependency(sot_loader sot-core)
-target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp)
+target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge tf)
 install(TARGETS sot_loader DESTINATION lib)
 
-
 add_subdirectory(src)
 
 # Deal with the ROS part.
@@ -183,12 +185,12 @@ generate_messages( DEPENDENCIES std_msgs )
 
 # This is necessary so that the pc file generated by catking is similar to the on
 # done directly by jrl-cmake-modules
-catkin_package(CATKIN_DEPENDS message_runtime roscpp realtime_tools bullet ${SOT_PKGNAMES}
-LIBRARIES ros_bridge ros_interpreter
+catkin_package(CATKIN_DEPENDS message_runtime roscpp realtime_tools tf2_bullet ${SOT_PKGNAMES} tf
+LIBRARIES ros_bridge ros_interpreter sot_loader
 )
 
 # Add libraries in pc file generated by cmake submodule
-PKG_CONFIG_APPEND_LIBS(ros_bridge ros_interpreter)
+PKG_CONFIG_APPEND_LIBS(ros_bridge ros_interpreter sot_loader)
 
 #install ros executables
 install(PROGRAMS
diff --git a/cmake b/cmake
index bd35ff07dc0ae377ba1d04815ff98a270d31f4d7..54177e44a1440222184865f1449c40b708eeaaa4 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit bd35ff07dc0ae377ba1d04815ff98a270d31f4d7
+Subproject commit 54177e44a1440222184865f1449c40b708eeaaa4
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 30672e0bb3852b4fbf68311555b1e4b62262b5da..b740f5b7c60f81de796fe7a53f70ba43c3a0a8de 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -39,6 +39,7 @@
 #include "ros/ros.h"
 #include "std_srvs/Empty.h"
 #include <sensor_msgs/JointState.h>
+#include <tf/transform_broadcaster.h>
 
 // Sot Framework includes 
 #include <sot/core/debug.hh>
@@ -85,6 +86,10 @@ protected:
   virtual void startControlLoop();
 
 
+  //Robot Pose Publisher
+  tf::TransformBroadcaster freeFlyerPublisher_;
+  tf::Transform freeFlyerPose_;
+
 public:
   SotLoader();
   ~SotLoader() {};
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 6f7fbdfdc71521f2a1aad80f7c3e005927ffbb7c..b561d48a859513ab815807dbb8b07ce8e372e458 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -60,6 +60,8 @@ protected:
   po::variables_map vm_;
   std::string dynamicLibraryName_;
 
+  /// \brief Handle on the SoT library.
+  void * sotRobotControllerLibrary_;
 
   /// \brief Map between SoT state vector and some joint_state_links
   XmlRpc::XmlRpcValue stateVectorMap_;
@@ -89,9 +91,12 @@ public:
   // \brief Read user input to extract the path of the SoT dynamic library.
   int parseOptions(int argc, char *argv[]);
 
-  // \brief Load the SoT
+  /// \brief Load the SoT device corresponding to the robot.
   void Initialization();
 
+  /// \brief Unload the library which handles the robot device.
+  void CleanUp();
+
   // \brief Create a thread for ROS.
   virtual void initializeRosNode(int argc, char *argv[]);
   
diff --git a/package.xml b/package.xml
index 8b7740da2c1eee354301c2dd9ce1af2e645614a5..7bdb5a63bfaeacc351e0f76f2316aecf28d465ac 100644
--- a/package.xml
+++ b/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>dynamic_graph_bridge</name>
-  <version>2.0.9</version>
+  <version>3.0.6</version>
   <description>
 
      ROS bindings for dynamic graph.
@@ -27,13 +27,10 @@
   <depend>realtime_tools</depend>
   <depend>message_generation</depend>
   <depend>message_runtime</depend>
-
-  <depend>bullet</depend>
-  <depend>jrl-mal</depend>
+  <depend>tf2_bullet</depend>
   <depend>dynamic-graph</depend>
   <depend>dynamic-graph-python</depend>
   <depend>sot-core</depend>
-  <depend>sot-dynamic</depend>
-  <depend>jrl-dynamics-urdf</depend>
+  <depend>sot-dynamic-pinocchio</depend>
   <depend>dynamic_graph_bridge_msgs</depend>
 </package>
diff --git a/scripts/robot_pose_publisher b/scripts/robot_pose_publisher
index fc34c316a4e4957c6fe0e07274f428a2f848f5e1..f742b2ac40e3db646688ff6451b254adfdfb3de2 100755
--- a/scripts/robot_pose_publisher
+++ b/scripts/robot_pose_publisher
@@ -11,6 +11,7 @@ import sensor_msgs.msg
 frame = ''
 childFrame = ''
 
+#DEPRECATED. Robot Pose is already being published
 def pose_broadcaster(msg):
     translation = msg.position[0:3];
     rotation = tf.transformations.quaternion_from_euler(msg.position[3], msg.position[4], msg.position[5])
diff --git a/src/converter.hh b/src/converter.hh
index 0668d40dec66baf5d242f2b3daecaa8a1a60d10b..7c953b68221fa3f4a1700f96e1da7452adbf7371 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -67,18 +67,18 @@ namespace dynamicgraph
   }
 
   // Vector
-  SOT_TO_ROS_IMPL(ml::Vector)
+  SOT_TO_ROS_IMPL(Vector)
   {
     dst.data.resize (src.size ());
-    for (unsigned i = 0; i < src.size (); ++i)
-      dst.data[i] =  src.elementAt (i);
+    for (int i = 0; i < src.size (); ++i)
+      dst.data[i] =  src (i);
   }
 
-  ROS_TO_SOT_IMPL(ml::Vector)
+  ROS_TO_SOT_IMPL(Vector)
   {
-    dst.resize ((int)src.data.size ());
-    for (unsigned i = 0; i < src.data.size (); ++i)
-      dst.elementAt (i) =  src.data[i];
+    dst.resize (src.data.size ());
+    for (unsigned int i = 0; i < src.data.size (); ++i)
+      dst (i) =  src.data[i];
   }
 
   // Vector3
@@ -86,12 +86,12 @@ namespace dynamicgraph
   {
     if (src.size () > 0)
       {
-	dst.x =  src.elementAt (0);
+	dst.x =  src (0);
 	if (src.size () > 1)
 	  {
-	    dst.y =  src.elementAt (1);
+	    dst.y =  src (1);
 	    if (src.size () > 2)
-	      dst.z =  src.elementAt (2);
+	      dst.z =  src (2);
 	  }
       }
   }
@@ -99,64 +99,58 @@ namespace dynamicgraph
   ROS_TO_SOT_IMPL(specific::Vector3)
   {
     dst.resize (3);
-    dst.elementAt (0) =  src.x;
-    dst.elementAt (1) =  src.y;
-    dst.elementAt (2) =  src.z;
+    dst (0) =  src.x;
+    dst (1) =  src.y;
+    dst (2) =  src.z;
   }
 
   // Matrix
-  SOT_TO_ROS_IMPL(ml::Matrix)
+  SOT_TO_ROS_IMPL(Matrix)
   {
-    dst.width = src.nbRows ();
-    dst.data.resize (src.nbCols () * src.nbRows ());
-    for (unsigned i = 0; i < src.nbCols () * src.nbRows (); ++i)
-      dst.data[i] =  src.elementAt (i);
-  }
 
-  ROS_TO_SOT_IMPL(ml::Matrix)
+    //TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to ColMajor.
+    dst.width = (unsigned int)src.rows ();
+    dst.data.resize (src.cols () * src.rows ());
+    for (int i = 0; i < src.cols () * src.rows (); ++i)
+      dst.data[i] =  src.data()[i];
+  }
+  
+  ROS_TO_SOT_IMPL(Matrix)
   {
     dst.resize (src.width, (unsigned int) src.data.size () / 
 		(unsigned int)src.width);
     for (unsigned i = 0; i < src.data.size (); ++i)
-      dst.elementAt (i) =  src.data[i];
+      dst.data()[i] =  src.data[i];
   }
 
   // Homogeneous matrix.
   SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
   {
-    btMatrix3x3 rotation;
-    btQuaternion quaternion;
-    for(unsigned i = 0; i < 3; ++i)
-      for(unsigned j = 0; j < 3; ++j)
-	rotation[i][j] = (float)src (i, j);
-    rotation.getRotation (quaternion);
-
-    dst.translation.x = src (0, 3);
-    dst.translation.y = src (1, 3);
-    dst.translation.z = src (2, 3);
-
-    dst.rotation.x = quaternion.x ();
-    dst.rotation.y = quaternion.y ();
-    dst.rotation.z = quaternion.z ();
-    dst.rotation.w = quaternion.w ();
+
+    sot::VectorQuaternion q(src.linear());
+    dst.translation.x = src.translation()(0);
+    dst.translation.y = src.translation()(1);
+    dst.translation.z = src.translation()(2);
+
+    dst.rotation.x = q.x();
+    dst.rotation.y = q.y();
+    dst.rotation.z = q.z();
+    dst.rotation.w = q.w();
+
   }
 
   ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
   {
-    btQuaternion quaternion
-      ((float)src.rotation.x,(float)src.rotation.y, 
-       (float)src.rotation.z,(float)src.rotation.w);
-    btMatrix3x3 rotation (quaternion);
-
-    // Copy the rotation component.
-    for(unsigned i = 0; i < 3; ++i)
-      for(unsigned j = 0; j < 3; ++j)
-	dst (i, j) = rotation[i][j];
+    sot::VectorQuaternion q(src.rotation.w,
+			    src.rotation.x,
+			    src.rotation.y,
+			    src.rotation.z);
+    dst.linear() = q.matrix();
 
     // Copy the translation component.
-    dst(0, 3) = src.translation.x;
-    dst(1, 3) = src.translation.y;
-    dst(2, 3) = src.translation.z;
+    dst.translation()(0) = src.translation.x;
+    dst.translation()(1) = src.translation.y;
+    dst.translation()(2) = src.translation.z;
   }
 
 
@@ -191,8 +185,8 @@ namespace dynamicgraph
 # define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)	\
   template <>								\
   inline void converter							\
-  (SotToRos<std::pair<T, ml::Vector> >::ros_t& dst,			\
-   const SotToRos<std::pair<T, ml::Vector> >::sot_t& src)		\
+  (SotToRos<std::pair<T, Vector> >::ros_t& dst,			\
+   const SotToRos<std::pair<T, Vector> >::sot_t& src)		\
   {									\
     makeHeader(dst.header);						\
     converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t> (dst.ATTRIBUTE, src); \
@@ -224,9 +218,9 @@ namespace dynamicgraph
 
   DG_BRIDGE_MAKE_SHPTR_IMPL(double);
   DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Vector);
+  DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
   DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Matrix);
+  DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
   DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
   DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
 
@@ -237,8 +231,8 @@ namespace dynamicgraph
 # define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)		\
   template <>								\
   inline void converter							\
-  (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst,			\
-   const SotToRos<std::pair<T, ml::Vector> >::ros_t& src)		\
+  (SotToRos<std::pair<T, Vector> >::sot_t& dst,			\
+   const SotToRos<std::pair<T, Vector> >::ros_t& src)		\
   {									\
     converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src.ATTRIBUTE); \
     do { EXTRA } while (0);						\
@@ -256,9 +250,9 @@ namespace dynamicgraph
 # define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA)		\
   template <>								\
   inline void converter							\
-  (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst,			\
+  (SotToRos<std::pair<T, Vector> >::sot_t& dst,			\
    const boost::shared_ptr						\
-   <SotToRos<std::pair<T, ml::Vector> >::ros_t const>& src)		\
+   <SotToRos<std::pair<T, Vector> >::ros_t const>& src)		\
   {									\
     converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src->ATTRIBUTE); \
     do { EXTRA } while (0);						\
diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py
index 3a14f188bdab8b5d4fb1cd7bcd3a3589871f8bd1..2f9ac5e6c5bae550f17864243538fc741e44dd89 100644
--- a/src/dynamic_graph/ros/__init__.py
+++ b/src/dynamic_graph/ros/__init__.py
@@ -1,12 +1,34 @@
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
 from ros_publish import RosPublish
 from ros_subscribe import RosSubscribe
 from ros_joint_state import RosJointState
-from robot_model import RosRobotModel
 
 from ros import Ros
 
 # aliases, for retro compatibility
-ros_import = ros_publish
-ros_export = ros_subscribe
 from ros import RosPublish as RosImport
 from ros import RosSubscribe as RosExport
+
+
+class RosRobotModel(DynamicPinocchio):
+    def __init__(self, name):
+        DynamicPinocchio.__init__(self, name)
+        self.namespace = "sot_controller"
+        self.jointsParameterName_ = "jrl_map"
+
+    def setJointsNamesParameter(self):
+        import rospy
+        if self.model is not None:
+            parameter_name = self.namespace + "/" + jointsParameterName_
+            jointsName = []
+            for i in xrange(self.model.njoints):
+                jointsName.append(self.model.names[i])
+            rospy.set_param(parameter_name,jointsName)
+        return
+
+    def setNamespace(self, ns):
+        self.namespace = ns
+        return
+
+    def curConf(self):
+        return self.position.value
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index a9981133d569ab2d992d14fc079b88df4acfae4b..e622377aae6087e6def9b37c53749ec4e5cbc1de 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -25,9 +25,7 @@
 
 int main(int argc, char *argv[])
 {
-  
   ros::init(argc, argv, "sot_ros_encapsulator");
-
   SotLoader aSotLoader;
   if (aSotLoader.parseOptions(argc,argv)<0)
     return -1;
@@ -37,5 +35,5 @@ int main(int argc, char *argv[])
   while(true){
     usleep(5000);
   }
-  
+ 
 }
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index 44911ea9dc86b927ce84361ffdbd8ba16afa8047..6632b249849708d5c725870eba33be6b94c53bda 100644
--- a/src/robot_model.cpp
+++ b/src/robot_model.cpp
@@ -2,18 +2,22 @@
 
 #include <dynamic-graph/all-commands.h>
 #include <dynamic-graph/factory.h>
-#include <jrl/dynamics/urdf/parser.hh>
+#include <pinocchio/multibody/parser/urdf.hpp>
+#include <pinocchio/multibody/model.hpp>
 
 #include "dynamic_graph_bridge/ros_init.hh"
 
+#include <ros/package.h>
+
 namespace dynamicgraph
 {
 
 RosRobotModel::RosRobotModel(const std::string& name)
-    : Dynamic(name,false),
+    : Dynamic(name),
       jointsParameterName_("jrl_map"),
       ns_("sot_controller")
 {
+
     std::string docstring;
 
     docstring =
@@ -54,19 +58,21 @@ RosRobotModel::~RosRobotModel()
 
 void RosRobotModel::loadUrdf (const std::string& filename)
 {
-    jrl::dynamics::urdf::Parser parser;
-
-    std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
-    for (;it!=specialJoints_.end();++it) {
-        parser.specifyREPName(it->first, it->second);
-    }
-    rosInit (false);
-
-    m_HDR = parser.parse(filename);
-
-    ros::NodeHandle nh(ns_);
-
-    nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
+  rosInit (false);
+  m_model = se3::urdf::buildModel(filename);
+  this->m_urdfPath = filename;
+  if (m_data) delete m_data;
+  m_data = new se3::Data(m_model);
+  init=true;
+
+  //  m_HDR = parser.parse(filename);
+  ros::NodeHandle nh(ns_);
+  
+  XmlRpc::XmlRpcValue JointsNamesByRank_;
+  JointsNamesByRank_.setSize(m_model.names.size());
+  std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
+  for (int i=0;it!=m_model.names.end();++it, ++i)  JointsNamesByRank_[i]= (*it);
+  nh.setParam(jointsParameterName_, JointsNamesByRank_);
 }
 
 void RosRobotModel::setNamespace (const std::string& ns)
@@ -76,91 +82,70 @@ void RosRobotModel::setNamespace (const std::string& ns)
 
 void RosRobotModel::loadFromParameterServer()
 {
-    jrl::dynamics::urdf::Parser parser;
-
-    std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
-    for (;it!=specialJoints_.end();++it) {
-        parser.specifyREPName(it->first, it->second);
-    }
-
     rosInit (false);
     std::string robotDescription;
     ros::param::param<std::string> ("/robot_description", robotDescription, "");
-
     if (robotDescription.empty ())
         throw std::runtime_error("No model available as ROS parameter. Fail.");
+    ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDF (robotDescription);
+    if (urdfTree)
+      se3::urdf::parseTree(urdfTree->getRoot(), 
+			   this->m_model, se3::SE3::Identity(), false);
+    else {
+      const std::string exception_message 
+	("robot_description not parsed correctly.");
+      throw std::invalid_argument(exception_message);
+    }
 
-    m_HDR = parser.parseStream (robotDescription);
-
+    this->m_urdfPath = "";
+    if (m_data) delete m_data;
+    m_data = new se3::Data(m_model);
+    init=true;
     ros::NodeHandle nh(ns_);
-
-    nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
-
+    
+    XmlRpc::XmlRpcValue JointsNamesByRank_;
+    JointsNamesByRank_.setSize(m_model.names.size());
+    //first joint is universe, second is freeflyer
+    std::vector<std::string>::const_iterator it = m_model.names.begin()+2;
+    for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
+    nh.setParam(jointsParameterName_, JointsNamesByRank_);
 }
 
-namespace
-{
-
-vectorN convertVector(const ml::Vector& v)
-{
-    vectorN res (v.size());
-    for (unsigned i = 0; i < v.size(); ++i)
-        res[i] = v(i);
-    return res;
-}
-
-ml::Vector convertVector(const vectorN& v)
-{
-    ml::Vector res;
-    res.resize((unsigned int)v.size());
-    for (unsigned i = 0; i < v.size(); ++i)
-        res(i) = v[i];
-    return res;
-}
-
-} // end of anonymous namespace.
-
 Vector RosRobotModel::curConf() const
 {
 
-    // The first 6 dofs are associated to the Freeflyer frame
-    // Freeflyer reference frame should be the same as global
-    // frame so that operational point positions correspond to
-    // position in freeflyer frame.
-
-    XmlRpc::XmlRpcValue ffpose;
-    ros::NodeHandle nh(ns_);
-    std::string param_name = "ffpose";
-    if (nh.hasParam(param_name)){
-        nh.getParam(param_name, ffpose);
-        ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
-        ROS_ASSERT(ffpose.size() == 6);
-        for (int32_t i = 0; i < ffpose.size(); ++i)
-        {
-            ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
-        }
-    }
-    else
-    {
-        ffpose.setSize(6);
-        for (int32_t i = 0; i < ffpose.size(); ++i)
-            ffpose[i] = 0.0;
-    }
-
-    if (!m_HDR )
-        throw std::runtime_error ("no robot loaded");
-    else {
-        vectorN currConf = m_HDR->currentConfiguration();
-        Vector res;
-        res = convertVector(currConf);
-
-        for (int32_t i = 0; i < ffpose.size(); ++i)
-            res(i) = static_cast<double>(ffpose[i]);
-
-        return res;
+  // The first 6 dofs are associated to the Freeflyer frame
+  // Freeflyer reference frame should be the same as global
+  // frame so that operational point positions correspond to
+  // position in freeflyer frame.
+  XmlRpc::XmlRpcValue ffpose;
+  ros::NodeHandle nh(ns_);
+  std::string param_name = "ffpose";
+  if (nh.hasParam(param_name)){
+    nh.getParam(param_name, ffpose);
+    ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
+    ROS_ASSERT(ffpose.size() == 6);
+    for (int32_t i = 0; i < ffpose.size(); ++i) {
+      ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
     }
+  }
+  else {
+    ffpose.setSize(6);
+    for (int32_t i = 0; i < ffpose.size(); ++i)  ffpose[i] = 0.0;
+  }
+  
+  if (!m_data )
+    throw std::runtime_error ("no robot loaded");
+  else {
+    //TODO: confirm accesscopy is for asynchronous commands
+    Vector currConf = jointPositionSIN.accessCopy();
+    for (int32_t i = 0; i < ffpose.size(); ++i)
+      currConf(i) = static_cast<double>(ffpose[i]);
+
+    return currConf;
+  }
 }
-
+  
 void
 RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
 {
diff --git a/src/robot_model.hh b/src/robot_model.hh
index b456b4e4f78a3b93d97e7ea416d8f045fde65b5d..646a894beb002e0f65d8bf3fd4dbab0236d04cd4 100644
--- a/src/robot_model.hh
+++ b/src/robot_model.hh
@@ -15,7 +15,7 @@ namespace dynamicgraph
   /// the robot_description parameter or a specified file and build
   /// a Dynamic entity
   ///
-  /// This relies on jrl_dynamics_urdf to load the model and jrl-dynamics
+  /// This relies on pinocchio urdf parser to load the model and pinocchio
   /// to realize the computation.
   class RosRobotModel : public sot::Dynamic
   {
@@ -35,9 +35,11 @@ namespace dynamicgraph
 
     unsigned getDimension () const
     {
-      if (!m_HDR)
+      if (!m_data)
 	throw std::runtime_error ("no robot loaded");
-      return m_HDR->numberDof();
+      //TODO: Configuration vector dimension or the dof?
+      return m_model.nv;
+      //return m_model.nq;
     }
 
 
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index 77b91408c155e953745773b84b95d9f961b08117..d2e1ba928c4a03b3586cbccd7e33b75635e33a3c 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -5,7 +5,7 @@
 #include <dynamic-graph/command.h>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/pool.h>
-#include <sot-dynamic/dynamic.h>
+#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
 
 #include "dynamic_graph_bridge/ros_init.hh"
 #include "ros_joint_state.hh"
@@ -32,41 +32,37 @@ namespace dynamicgraph
     RetrieveJointNames::RetrieveJointNames
     (RosJointState& entity, const std::string& docstring)
       : Command (entity, boost::assign::list_of (Value::STRING), docstring)
-    {}
-
-    namespace
     {
-      void
-      buildJointNames (sensor_msgs::JointState& jointState, CjrlJoint* joint)
-      {
-	if (!joint)
-	  return;
-	// Ignore anchors.
-	if (joint->numberDof() != 0)
-	  {
+}
+
+    namespace {
+      void  buildJointNames (sensor_msgs::JointState& jointState, se3::Model* robot_model) {
+	int cnt = 0;
+	for (int i=1;i<robot_model->njoints;i++) {
+	  // Ignore anchors.
+	  if (se3::nv(robot_model->joints[i]) != 0) {
 	    // If we only have one dof, the dof name is the joint name.
-	    if (joint->numberDof() == 1)
-	      {
-		jointState.name[joint->rankInConfiguration()] =
-		  joint->getName();
+	    if (se3::nv(robot_model->joints[i]) == 1) {
+	      jointState.name[cnt] =  robot_model->names[i];
+	      cnt++;
+	    }
+	    else {
+	      // ...otherwise, the dof name is the joint name on which
+	      // the dof id is appended.
+	      int joint_dof = se3::nv(robot_model->joints[i]);
+	      for(int j = 0; j<joint_dof; j++) {
+		boost::format fmt("%1%_%2%");
+		fmt % robot_model->names[i];
+		fmt % j;
+		jointState.name[cnt + j] =  fmt.str();
 	      }
-	    // ...otherwise, the dof name is the joint name on which
-	    // the dof id is appended.
-	    else
-	      for (unsigned i = 0; i < joint->numberDof(); ++i)
-		{
-		  boost::format fmt("%1%_%2%");
-		  fmt % joint->getName();
-		  fmt % i;
-		  jointState.name[joint->rankInConfiguration() + i] =
-		    fmt.str();
-		}
+	      cnt+=joint_dof;
+	    }
 	  }
-	for (unsigned i = 0; i < joint->countChildJoints (); ++i)
-	  buildJointNames (jointState, joint->childJoint (i));
+	}
       }
     } // end of anonymous namespace
-
+	  
     Value RetrieveJointNames::doExecute ()
     {
       RosJointState& entity = static_cast<RosJointState&> (owner ());
@@ -80,24 +76,24 @@ namespace dynamicgraph
 	  return Value ();
 	}
 
-      dynamicgraph::sot::Dynamic* dynamic =
-	dynamic_cast<dynamicgraph::sot::Dynamic*>
+      dynamicgraph::sot::DynamicPinocchio* dynamic =
+	dynamic_cast<dynamicgraph::sot::DynamicPinocchio*>
 	(&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
       if (!dynamic)
 	{
-	  std::cerr << "entity is not a Dynamic entity" << std::endl;
+	  std::cerr << "entity is not a DynamicPinocchio entity" << std::endl;
 	  return Value ();
 	}
 
-      CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
-      if (!robot)
+      se3::Model* robot_model = dynamic->m_model;
+      if (robot_model->njoints == 1)
 	{
 	  std::cerr << "no robot in the dynamic entity" << std::endl;
 	  return Value ();
 	}
 
-      entity.jointState ().name.resize (robot->numberDof());
-      buildJointNames (entity.jointState (), robot->rootJoint());
+      entity.jointState ().name.resize (robot_model->nv);
+      buildJointNames (entity.jointState (), robot_model);
       return Value ();
     }
   } // end of namespace command.
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index cb2115724d4b4a61bfb848546dd89b4f345ee310..10ee53acf9ea47494aaacef2c864bbab64fe4b7b 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -19,7 +19,7 @@ namespace dynamicgraph
     DYNAMIC_GRAPH_ENTITY_DECL();
   public:
     /// \brief Vector input signal.
-    typedef SignalPtr<ml::Vector, int> signalVectorIn_t;
+    typedef SignalPtr<Vector, int> signalVectorIn_t;
 
     static const double ROS_JOINT_STATE_PUBLISHER_RATE;
 
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 3bcf22b8cd0d918ec41bd18e456745d934c31229..20c4b9d73d6cc2d27c76fe2d921972169d20eded 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -13,6 +13,7 @@
 
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/command.h>
+#include <dynamic-graph/linear-algebra.h>
 
 #include "dynamic_graph_bridge/ros_init.hh"
 #include "ros_publish.hh"
@@ -82,22 +83,22 @@ namespace dynamicgraph
 	else if (type == "unsigned")
 	  entity.add<unsigned int> (signal, topic);
 	else if (type == "matrix")
-	  entity.add<ml::Matrix> (signal, topic);
+	  entity.add<Matrix> (signal, topic);
 	else if (type == "vector")
-	  entity.add<ml::Vector> (signal, topic);
+	  entity.add<Vector> (signal, topic);
 	else if (type == "vector3")
 	  entity.add<specific::Vector3> (signal, topic);
 	else if (type == "vector3Stamped")
-	  entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
+	  entity.add<std::pair<specific::Vector3, Vector> > (signal, topic);
 	else if (type == "matrixHomo")
 	  entity.add<sot::MatrixHomogeneous> (signal, topic);
 	else if (type == "matrixHomoStamped")
-	  entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+	  entity.add<std::pair<sot::MatrixHomogeneous, Vector> >
 	    (signal, topic);
 	else if (type == "twist")
 	  entity.add<specific::Twist> (signal, topic);
 	else if (type == "twistStamped")
-	  entity.add<std::pair<specific::Twist, ml::Vector> > (signal, topic);
+	  entity.add<std::pair<specific::Twist, Vector> > (signal, topic);
 	else
 	  throw std::runtime_error("bad type");
 	return Value ();
@@ -109,7 +110,8 @@ namespace dynamicgraph
 	  (entity,
 	   boost::assign::list_of (Value::STRING),
 	   docstring)
-      {}
+      {
+}
 
       Value Rm::doExecute ()
       {
@@ -139,6 +141,7 @@ namespace dynamicgraph
       rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
       lastPublicated_ ()
   {
+
     try {
       lastPublicated_ = ros::Time::now ();
     } catch (const std::exception& exc) {
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index fd217385fa86e5f8c880e1d1930131c62b4e8e77..4ff1d0154e90cc9d6c48aa57ee41043ee8d1f4df 100644
--- a/src/ros_publish.hxx
+++ b/src/ros_publish.hxx
@@ -15,19 +15,19 @@ namespace dynamicgraph
   template <>
   inline void
   RosPublish::sendData
-  <std::pair<sot::MatrixHomogeneous, ml::Vector> >
+  <std::pair<sot::MatrixHomogeneous, Vector> >
   (boost::shared_ptr
    <realtime_tools::RealtimePublisher
    <SotToRos
-   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t> > publisher,
+   <std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> > publisher,
    boost::shared_ptr
    <SotToRos
-   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalIn_t> signal,
+   <std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal,
    int time)
   {
     SotToRos
       <std::pair
-      <sot::MatrixHomogeneous, ml::Vector> >::ros_t result;
+      <sot::MatrixHomogeneous, Vector> >::ros_t result;
     if (publisher->trylock ())
       {
 	publisher->msg_.child_frame_id = "/dynamic_graph/world";
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index 4c1f02c84d00335f8973e9aa93120e757fd56572..8923ed70c13374199d93fa863afb37b7ee4ac43e 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -77,22 +77,22 @@ namespace dynamicgraph
 	else if (type == "unsigned")
 	  entity.add<unsigned int> (signal, topic);
 	else if (type == "matrix")
-	  entity.add<ml::Matrix> (signal, topic);
+	  entity.add<dg::Matrix> (signal, topic);
 	else if (type == "vector")
-	  entity.add<ml::Vector> (signal, topic);
+	  entity.add<dg::Vector> (signal, topic);
 	else if (type == "vector3")
 	  entity.add<specific::Vector3> (signal, topic);
 	else if (type == "vector3Stamped")
-	  entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
+	  entity.add<std::pair<specific::Vector3, dg::Vector> > (signal, topic);
 	else if (type == "matrixHomo")
 	  entity.add<sot::MatrixHomogeneous> (signal, topic);
 	else if (type == "matrixHomoStamped")
-	  entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+	  entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >
 	    (signal, topic);
 	else if (type == "twist")
 	  entity.add<specific::Twist> (signal, topic);
 	else if (type == "twistStamped")
-	  entity.add<std::pair<specific::Twist, ml::Vector> >
+	  entity.add<std::pair<specific::Twist, dg::Vector> >
 	    (signal, topic);
 	else
 	  throw std::runtime_error("bad type");
diff --git a/src/ros_subscribe.hh b/src/ros_subscribe.hh
index 480686c67524297d78777723c6eb3ab2dbbc8f86..17b47e576df9b38004bdb2bb1bf8ae0ab843a49f 100644
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
@@ -9,7 +9,7 @@
 # include <dynamic-graph/signal-time-dependent.h>
 # include <dynamic-graph/signal-ptr.h>
 # include <dynamic-graph/command.h>
-# include <sot/core/matrix-homogeneous.hh>
+# include <sot/core/matrix-geometry.hh>
 
 # include <ros/ros.h>
 
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index 12819e62bb1c9704ea8d024f806b9ff13d239b49..ded5de1398f5fe5bc9e9ad525ab889c2dd9286ae 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -4,14 +4,14 @@
 # include <boost/bind.hpp>
 # include <boost/date_time/posix_time/posix_time.hpp>
 # include <dynamic-graph/signal-caster.h>
+# include <dynamic-graph/linear-algebra.h>
 # include <dynamic-graph/signal-cast-helper.h>
-# include <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
 # include "dynamic_graph_bridge_msgs/Matrix.h"
 # include "dynamic_graph_bridge_msgs/Vector.h"
 # include "ros_time.hh"
 
-namespace ml = ::maal::boost;
+namespace dg = dynamicgraph;
 
 namespace dynamicgraph
 {
@@ -79,13 +79,13 @@ namespace dynamicgraph
     };
 
     template <typename T>
-    struct Add<std::pair<T, ml::Vector> >
+    struct Add<std::pair<T, dg::Vector> >
     {
       void operator () (RosSubscribe& RosSubscribe,
 			const std::string& signal,
 			const std::string& topic)
       {
-	typedef std::pair<T, ml::Vector> type_t;
+	typedef std::pair<T, dg::Vector> type_t;
 
 	typedef typename SotToRos<type_t>::sot_t sot_t;
 	typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index a8b55629a890a971a9b7cdf05777e414483e68ea..411ed6ca8f904f6e6d025dca72cc7648d2bbc4e2 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -76,15 +76,20 @@ void SotLoader::startControlLoop()
 void SotLoader::initializeRosNode(int argc, char *argv[])
 {
   SotLoaderBasic::initializeRosNode(argc, argv);
-  angleEncoder_.resize(nbOfJoints_);
+  //Temporary fix. TODO: where should nbOfJoints_ be initialized from?                         
+  if (ros::param::has("/sot/state_vector_map")) {
+    angleEncoder_.resize(nbOfJoints_);
+  }
+
   startControlLoop();
 }
 
 void 
 SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
 {
-  
   // Update joint values.w
+  assert(angleControl_.size() == angleEncoder_.size());
+
   sensorsIn["joints"].setName("angle");
   for(unsigned int i=0;i<angleControl_.size();i++)
     angleEncoder_[i] = angleControl_[i];
@@ -99,14 +104,28 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
   // Update joint values.
   angleControl_ = controlValues["joints"].getValues();
 
+  //Debug
+  std::map<std::string,dgs::ControlValues>::iterator it = controlValues.begin();
+  sotDEBUG (30)<<"ControlValues to be broadcasted:"<<std::endl;
+  for(;it!=controlValues.end(); it++){
+    sotDEBUG (30)<<it->first<<":";
+    std::vector<double> ctrlValues_ = it->second.getValues();
+    std::vector<double>::iterator it_d = ctrlValues_.begin();
+    for(;it_d!=ctrlValues_.end();it_d++) sotDEBUG (30)<<*it_d<<" ";
+    sotDEBUG (30)<<std::endl;
+  }
+  sotDEBUG (30)<<"End ControlValues"<<std::endl;
+
+
   // Check if the size if coherent with the robot description.
   if (angleControl_.size()!=(unsigned int)nbOfJoints_)
     {
-      std::cerr << " angleControl_ and nbOfJoints are different !"
+      std::cerr << " angleControl_"<<angleControl_.size()
+		<< " and nbOfJoints"<<(unsigned int)nbOfJoints_
+		<< " are different !"
                 << std::endl;
       exit(-1);
     }
-
   // Publish the data.
   joint_state_.header.stamp = ros::Time::now();  
   for(int i=0;i<nbOfJoints_;i++)
@@ -119,8 +138,25 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
         coefficient_parallel_joints_[i]*angleControl_[parallel_joints_to_state_vector_[i]];
     }
 
-  joint_pub_.publish(joint_state_);  
-
+  joint_pub_.publish(joint_state_);
+
+  //Publish robot pose
+  //get the robot pose values
+  std::vector<double> poseValue_ = controlValues["baseff"].getValues();
+
+  freeFlyerPose_.setOrigin(tf::Vector3(poseValue_[0],
+				       poseValue_[1],
+				       poseValue_[2]));
+  tf::Quaternion poseQ_(poseValue_[4],
+			poseValue_[5],
+			poseValue_[6],
+			poseValue_[3]);
+  freeFlyerPose_.setRotation(poseQ_);
+  //Publish
+  freeFlyerPublisher_.sendTransform(tf::StampedTransform(freeFlyerPose_,
+							 ros::Time::now(),
+							 "odom",
+							 "base_link"));
   
 }
 
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index e9c3f19346156dc20de34f63ad05bc829151df41..4193a9bd8781f0fd369b9bf6cd63b99a3c3ebabe 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -54,7 +54,8 @@ void createRosSpin(SotLoaderBasic *aSotLoaderBasic)
 
 
 SotLoaderBasic::SotLoaderBasic():
-  dynamic_graph_stopped_(true)
+  dynamic_graph_stopped_(true),
+  sotRobotControllerLibrary_(0)
 {
   readSotVectorStateParam();
   initPublication();
@@ -192,9 +193,9 @@ void SotLoaderBasic::Initialization()
 {
   
   // Load the SotRobotBipedController library.
-  void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
+  sotRobotControllerLibrary_ = dlopen( dynamicLibraryName_.c_str(),
                                              RTLD_LAZY | RTLD_GLOBAL );
-  if (!SotRobotControllerLibrary) {
+  if (!sotRobotControllerLibrary_) {
     std::cerr << "Cannot load library: " << dlerror() << '\n';
     return ;
   }
@@ -206,7 +207,7 @@ void SotLoaderBasic::Initialization()
   createSotExternalInterface_t * createSot =
     reinterpret_cast<createSotExternalInterface_t *> 
     (reinterpret_cast<long> 
-     (dlsym(SotRobotControllerLibrary, 
+     (dlsym(sotRobotControllerLibrary_, 
 	    "createSotExternalInterface")));
   const char* dlsym_error = dlerror();
   if (dlsym_error) {
@@ -219,6 +220,12 @@ void SotLoaderBasic::Initialization()
   cout <<"Went out from Initialization." << endl;
 }
 
+void SotLoaderBasic::CleanUp()
+{
+  /// Uncount the number of access to this library.
+  dlclose(sotRobotControllerLibrary_);
+}
+
 
 
 bool SotLoaderBasic::start_dg(std_srvs::Empty::Request& , 
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index 701087c5a1ad7508ff5dc7e9651ac4a0fd7fac30..e7a20758905723210165cc3c93a49b8b5d3c44ba 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -5,19 +5,19 @@ namespace dynamicgraph
 
   const char* SotToRos<double>::signalTypeName = "Double";
   const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
-  const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
-  const char* SotToRos<ml::Vector>::signalTypeName = "Vector";
+  const char* SotToRos<Matrix>::signalTypeName = "Matrix";
+  const char* SotToRos<Vector>::signalTypeName = "Vector";
   const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
   const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
   const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
   const char* SotToRos
-  <std::pair<specific::Vector3, ml::Vector> >::signalTypeName
+  <std::pair<specific::Vector3, Vector> >::signalTypeName
   = "Vector3Stamped";
   const char* SotToRos
-  <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
+  <std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName
   = "MatrixHomo";
   const char* SotToRos
-  <std::pair<specific::Twist, ml::Vector> >::signalTypeName
+  <std::pair<specific::Twist, Vector> >::signalTypeName
   = "Twist";
 
 } // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index d8a2cfab10a0163d3886b6c421b8d24fa3280581..a09f91c022bbc486dc95f844fb3146ce4f5324c7 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -5,7 +5,6 @@
 
 # include <boost/format.hpp>
 
-# include <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
 # include <std_msgs/UInt32.h>
 # include "dynamic_graph_bridge_msgs/Matrix.h"
@@ -18,8 +17,9 @@
 # include "geometry_msgs/Vector3Stamped.h"
 
 # include <dynamic-graph/signal-time-dependent.h>
+# include <dynamic-graph/linear-algebra.h>
 # include <dynamic-graph/signal-ptr.h>
-# include <sot/core/matrix-homogeneous.hh>
+# include <sot/core/matrix-geometry.hh>
 
 # define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)	\
   makeSignalString (typeid (*this).name (), NAME,			\
@@ -27,7 +27,6 @@
 
 namespace dynamicgraph
 {
-  namespace ml = maal::boost;
 
   /// \brief Types dedicated to identify pairs of (dg,ros) data.
   ///
@@ -88,9 +87,9 @@ namespace dynamicgraph
   };
 
   template <>
-  struct SotToRos<ml::Matrix>
+  struct SotToRos<Matrix>
   {
-    typedef ml::Matrix sot_t;
+    typedef Matrix sot_t;
     typedef dynamic_graph_bridge_msgs::Matrix ros_t;
     typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -102,16 +101,16 @@ namespace dynamicgraph
     template <typename S>
     static void setDefault(S& s)
     {
-      ml::Matrix m;
+      Matrix m;
       m.resize(0, 0);
       s.setConstant (m);
     }
   };
 
   template <>
-  struct SotToRos<ml::Vector>
+  struct SotToRos<Vector>
   {
-    typedef ml::Vector sot_t;
+    typedef Vector sot_t;
     typedef dynamic_graph_bridge_msgs::Vector ros_t;
     typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -123,7 +122,7 @@ namespace dynamicgraph
     template <typename S>
     static void setDefault(S& s)
     {
-      ml::Vector v;
+      Vector v;
       v.resize (0);
       s.setConstant (v);
     }
@@ -132,7 +131,7 @@ namespace dynamicgraph
   template <>
   struct SotToRos<specific::Vector3>
   {
-    typedef ml::Vector sot_t;
+    typedef Vector sot_t;
     typedef geometry_msgs::Vector3 ros_t;
     typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -144,7 +143,7 @@ namespace dynamicgraph
     template <typename S>
     static void setDefault(S& s)
     {
-      ml::Vector v;
+      Vector v;
       v.resize (0);
       s.setConstant (v);
     }
@@ -173,7 +172,7 @@ namespace dynamicgraph
   template <>
   struct SotToRos<specific::Twist>
   {
-    typedef ml::Vector sot_t;
+    typedef Vector sot_t;
     typedef geometry_msgs::Twist ros_t;
     typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -185,7 +184,7 @@ namespace dynamicgraph
     template <typename S>
     static void setDefault(S& s)
     {
-      ml::Vector v (6);
+      Vector v (6);
       v.setZero ();
       s.setConstant (v);
     }
@@ -193,9 +192,9 @@ namespace dynamicgraph
 
   // Stamped vector3
   template <>
-  struct SotToRos<std::pair<specific::Vector3, ml::Vector> >
+  struct SotToRos<std::pair<specific::Vector3, Vector> >
   {
-    typedef ml::Vector sot_t;
+    typedef Vector sot_t;
     typedef geometry_msgs::Vector3Stamped ros_t;
     typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -213,7 +212,7 @@ namespace dynamicgraph
 
   // Stamped transformation
   template <>
-  struct SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+  struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >
   {
     typedef sot::MatrixHomogeneous sot_t;
     typedef geometry_msgs::TransformStamped ros_t;
@@ -233,9 +232,9 @@ namespace dynamicgraph
 
   // Stamped twist
   template <>
-  struct SotToRos<std::pair<specific::Twist, ml::Vector> >
+  struct SotToRos<std::pair<specific::Twist, Vector> >
   {
-    typedef ml::Vector sot_t;
+    typedef Vector sot_t;
     typedef geometry_msgs::TwistStamped ros_t;
     typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;