Merge pull request #31 from stack-of-tasks/devel

Move master branch to v3.

.version

+3.0.5

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

cmake @ 54177e44

-Subproject commit bd35ff07dc0ae377ba1d04815ff98a270d31f4d7
+Subproject commit 54177e44a1440222184865f1449c40b708eeaaa4

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

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[]);
   

package.xml

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

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

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);						\

src/dynamic_graph/ros/__init__.py

+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

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

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

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

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.

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;
 

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

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";

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");

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>
 

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;

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"));
   
 }