diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9a4ac431ed4ae3a1368195705468339f5e67305..24e0fa89438c80184ac5a84b922e2811892c3b97 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
-include(FindPkgConfig)
+INCLUDE(cmake/base.cmake)
 
 set(ROS_BUILD_TYPE RelWithDebInfo)
 
@@ -15,10 +15,17 @@ rosbuild_gensrv()
 
 rosbuild_add_boost_directories()
 
-pkg_check_modules(JRL_MAL REQUIRED jrl-mal)
-pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
-pkg_check_modules(DYNAMIC_GRAPH_PYTHON REQUIRED dynamic-graph-python)
-pkg_check_modules(SOT_CORE REQUIRED sot-core)
+SET(PKG_CONFIG_ADDITIONAL_VARIABLES
+  ${PKG_CONFIG_ADDITIONAL_VARIABLES}
+  plugindirname
+  plugindir
+  )
+
+ADD_REQUIRED_DEPENDENCY(jrl-mal)
+ADD_REQUIRED_DEPENDENCY(dynamic-graph)
+ADD_REQUIRED_DEPENDENCY(dynamic-graph-python)
+ADD_REQUIRED_DEPENDENCY(sot-core)
+ADD_REQUIRED_DEPENDENCY(sot-dynamic)
 
 include_directories(include)
 
@@ -36,105 +43,58 @@ rosbuild_add_library(ros_bridge
 # are not installed.
 set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True)
 
-rosbuild_add_library(ros_import src/ros_import.cpp src/ros_import.hh)
-rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
-  ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_import ros_bridge)
-target_link_libraries(ros_import
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_import PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-rosbuild_add_library(ros_export src/ros_export.cpp src/ros_export.hh)
-rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
-  ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_export ros_bridge)
-target_link_libraries(ros_export
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_export PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-rosbuild_add_library(ros_joint_state
-  src/ros_joint_state.cpp src/ros_joint_state)
-rosbuild_add_compile_flags(ros_joint_state
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_joint_state ${JRL_MAL_LDFLAGS}
-  ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_joint_state ros_bridge)
-target_link_libraries(ros_joint_state
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_joint_state PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-rosbuild_add_library(ros_interpreter
-  src/ros_interpreter.cpp)
-rosbuild_add_compile_flags(ros_interpreter
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${DYNAMIC_GRAPH_PYTHON_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_interpreter ${JRL_MAL_LDFLAGS}
-  ${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_interpreter ros_bridge)
-target_link_libraries(ros_interpreter
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES}
-  ${DYNAMIC_GRAPH_PYTHON_LIBRARIES} ${SOT_CORE_LIBRARIES})
-set_target_properties(ros_interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-# Stand alone remote dynamic-graph Python interpreter.
-rosbuild_add_executable(interpreter src/interpreter.cpp)
-target_link_libraries(interpreter ros_interpreter)
-rosbuild_add_compile_flags(interpreter ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(interpreter ${JRL_MAL_LDFLAGS}
-  ${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
-set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
-
-INSTALL(TARGETS ros_bridge DESTINATION lib)
-INSTALL(TARGETS ros_import DESTINATION lib)
-INSTALL(TARGETS ros_export DESTINATION lib)
-INSTALL(TARGETS ros_joint_state DESTINATION lib)
-INSTALL(TARGETS ros_interpreter DESTINATION lib)
-INSTALL(TARGETS interpreter DESTINATION bin)
+MACRO(COMPILE_PLUGIN NAME)
+  rosbuild_add_library(${NAME} 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)
+  target_link_libraries(${NAME} ros_bridge)
+  set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
+  INSTALL(TARGETS ${NAME} DESTINATION lib)
+
+  DYNAMIC_GRAPH_PYTHON_MODULE("ros/${NAME}"
+    ${NAME}
+    ros/${NAME}/wrap
+    )
+
+  SET_TARGET_PROPERTIES(ros/${NAME}/wrap
+    PROPERTIES
+    COMPILE_FLAGS
+    ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
+    LINK_FLAGS
+    ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
+    )
+ENDMACRO()
 
 INCLUDE(cmake/python.cmake)
 
 INCLUDE_DIRECTORIES(${DYNAMIC_GRAPH_INCLUDE_DIRS})
 LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
 
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_import"
-  ros_import
-  ros/ros_import/wrap
-  )
-SET_TARGET_PROPERTIES(ros/ros_import/wrap
-  PROPERTIES
-  COMPILE_FLAGS
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
-  LINK_FLAGS
-  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
-  )
+COMPILE_PLUGIN(ros_import)
+COMPILE_PLUGIN(ros_export)
+COMPILE_PLUGIN(ros_export)
 
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_export"
-  ros_export
-  ros/ros_export/wrap
-  )
-SET_TARGET_PROPERTIES(ros/ros_export/wrap
-  PROPERTIES
-  COMPILE_FLAGS
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
-  LINK_FLAGS
-  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
-  )
+COMPILE_PLUGIN(ros_joint_state)
+target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
 
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_joint_state"
-  ros_joint_state
-  ros/ros_joint_state/wrap
-  )
-SET_TARGET_PROPERTIES(ros/ros_joint_state/wrap
-  PROPERTIES
-  COMPILE_FLAGS
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
-  LINK_FLAGS
-  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
-  )
+# ros_interperter library.
+rosbuild_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)
+target_link_libraries(ros_interpreter ros_bridge)
+set_target_properties(ros_interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
+INSTALL(TARGETS ros_interpreter DESTINATION lib)
+
+# Stand alone remote dynamic-graph Python interpreter.
+rosbuild_add_executable(interpreter src/interpreter.cpp)
+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)
+set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
+INSTALL(TARGETS interpreter DESTINATION bin)
 
 ADD_SUBDIRECTORY(src)
diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py
index e0a82102f07af1bcaed99349f813520800df4410..edfceaddc676194dea217aa94c425407e7a83eb0 100644
--- a/src/dynamic_graph/ros/ros.py
+++ b/src/dynamic_graph/ros/ros.py
@@ -15,6 +15,7 @@ class Ros(object):
         self.rosImport = RosImport('rosImport{0}'.format(suffix))
         self.rosExport = RosExport('rosExport{0}'.format(suffix))
         self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
+        self.rosJointState.retrieveJointNames(self.robot.dynamic.name)
 
         plug(self.robot.device.state, self.rosJointState.state)
         self.robot.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index f6a78b593748efcf6f56cd1153d423673c72320b..77791371a37ac49b1b87a07a8cd353ddcfb293e5 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -1,46 +1,106 @@
+#include <boost/assign.hpp>
 #include <boost/bind.hpp>
 #include <boost/format.hpp>
 
+#include <dynamic-graph/command.h>
 #include <dynamic-graph/factory.h>
+#include <dynamic-graph/pool.h>
+#include <sot-dynamic/dynamic.h>
 
 #include "dynamic_graph_bridge/ros_init.hh"
 #include "ros_joint_state.hh"
 #include "sot_to_ros.hh"
 
-//FIXME: this is very very wrong but we need to update abstract-robot-dynamics
-// to publish joint names.
-static const char* dof_names[] =
-  {
-    "RLEG_JOINT0", "RLEG_JOINT1", "RLEG_JOINT2",
-    "RLEG_JOINT3", "RLEG_JOINT4", "RLEG_JOINT5",
-    "LLEG_JOINT0", "LLEG_JOINT1", "LLEG_JOINT2",
-    "LLEG_JOINT3", "LLEG_JOINT4", "LLEG_JOINT5",
-    "CHEST_JOINT0", "CHEST_JOINT1",
-    "HEAD_JOINT0", "HEAD_JOINT1",
-    "RARM_JOINT0", "RARM_JOINT1", "RARM_JOINT2",
-    "RARM_JOINT3", "RARM_JOINT4", "RARM_JOINT5", "RARM_JOINT6",
-    "LARM_JOINT0", "LARM_JOINT1", "LARM_JOINT2",
-    "LARM_JOINT3", "LARM_JOINT4", "LARM_JOINT5", "LARM_JOINT6",
-    "RHAND_JOINT0", "RHAND_JOINT1",
-    "RHAND_JOINT2", "RHAND_JOINT3", "RHAND_JOINT4",
-    "LHAND_JOINT0", "LHAND_JOINT1",
-    "LHAND_JOINT2", "LHAND_JOINT3", "LHAND_JOINT4",
-    0
-  };
-
-// Number of dofs in left and right robot hands.
-//
-// This part is very poorly written: currently, dynamic-graph uses a
-// lighter robot model without the hands. This model do not have the
-// dofs corresponding to the robot hand dofs.  Therefore we put them
-// manually to zero to please ROS.  If this is not the case, some
-// packages such as rviz will behave badly.
-static const std::size_t handsDofsCount = 10;
-
 namespace dynamicgraph
 {
   DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosJointState, "RosJointState");
 
+  namespace command
+  {
+    using ::dynamicgraph::command::Command;
+    using ::dynamicgraph::command::Value;
+
+    class RetrieveJointNames : public Command
+    {
+    public:
+      RetrieveJointNames (RosJointState& entity,
+			  const std::string& docstring);
+      virtual Value doExecute ();
+    };
+
+    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)
+	  {
+	    // If we only have one dof, the dof name is the joint name.
+	    if (joint->numberDof() == 1)
+	      {
+		jointState.name[joint->rankInConfiguration()] =
+		  joint->getName();
+	      }
+	    // ...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();
+		}
+	  }
+	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 ());
+
+      std::vector<Value> values = getParameterValues ();
+      std::string name = values[0].value ();
+
+      if (!dynamicgraph::PoolStorage::getInstance ()->existEntity (name))
+	{
+	  std::cerr << "invalid entity name" << std::endl;
+	  return Value ();
+	}
+
+      dynamicgraph::sot::Dynamic* dynamic =
+	dynamic_cast<dynamicgraph::sot::Dynamic*>
+	(&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
+      if (!dynamic)
+	{
+	  std::cerr << "entity is not a Dynamic entity" << std::endl;
+	  return Value ();
+	}
+
+      CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
+      if (!robot)
+	{
+	  std::cerr << "no robot in the dynamic entity" << std::endl;
+	  return Value ();
+	}
+
+      entity.jointState ().name.resize (robot->numberDof());
+      buildJointNames (entity.jointState (), robot->rootJoint());
+      return Value ();
+    }
+  } // end of namespace command.
+
   RosJointState::RosJointState (const std::string& n)
     : Entity (n),
       // do not use callbacks, so do not create a useless spinner
@@ -61,7 +121,17 @@ namespace dynamicgraph
     jointState_.header.seq = 0;
     jointState_.header.stamp.sec = 0;
     jointState_.header.stamp.nsec = 0;
-    jointState_.header.frame_id = "odom";
+    jointState_.header.frame_id = "";
+
+    std::string docstring =
+      "\n"
+      "  Retrieve joint names using robot model contained in a Dynamic entity\n"
+      "\n"
+      "  Input:\n"
+      "    - dynamic entity name (i.e. robot.dynamic.name)\n"
+      "\n";
+    addCommand ("retrieveJointNames",
+		new command::RetrieveJointNames (*this, docstring));
   }
 
   RosJointState::~RosJointState ()
@@ -76,7 +146,14 @@ namespace dynamicgraph
 	lastPublicated_ = ros::Time::now ();
 
 	// State size without the free floating.
-	std::size_t s = state_.access (t).size () - 6;
+	std::size_t s = state_.access (t).size ();
+
+	// Safety check: if data are inconsistent, clear
+	// the joint names to avoid sending erroneous data.
+	// This should not happen unless you change
+	// the robot model at run-time.
+	if (s != jointState_.name.size())
+	  jointState_.name.clear();
 
 	// Update header.
 	++jointState_.header.seq;
@@ -85,24 +162,10 @@ namespace dynamicgraph
 	jointState_.header.stamp.sec = now.sec;
 	jointState_.header.stamp.nsec = now.nsec;
 
-	// Fill names if needed.
-	if (jointState_.name.size () != s + handsDofsCount)
-	  {
-	    jointState_.name.resize (s + handsDofsCount);
-	    for (std::size_t i = 0; i < s + handsDofsCount; ++i)
-	      {
-		if (dof_names[i] == 0)
-		  break;
-		jointState_.name[i] = dof_names[i];
-	      }
-	  }
-
 	// Fill position.
-	jointState_.position.resize (s + handsDofsCount);
+	jointState_.position.resize (s);
 	for (std::size_t i = 0; i < s; ++i)
-	  jointState_.position[i] = state_.access (t) (i + 6);
-	for (std::size_t i = 0; i < handsDofsCount; ++i)
-	  jointState_.position[i + s] = 0.;
+	  jointState_.position[i] = state_.access (t) (i);
 
 	publisher_.msg_ = jointState_;
 	publisher_.unlockAndPublish ();
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index 9d9ff2b3ea5e3dccdc35e950490b39344dd292f7..6ce379c2e3bb659971c255f1c5dbfaf74c26af13 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -27,6 +27,11 @@ namespace dynamicgraph
     virtual ~RosJointState ();
 
     int& trigger (int&, int);
+
+    sensor_msgs::JointState& jointState ()
+    {
+      return jointState_;
+    }
   private:
     ros::NodeHandle& nh_;
     signalVectorIn_t state_;