Add RosRobotModel entity.

CMakeLists.txt

 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
-INCLUDE(cmake/base.cmake)
+include(cmake/base.cmake)
 
 set(ROS_BUILD_TYPE RelWithDebInfo)
 
@@ -15,25 +15,20 @@ rosbuild_gensrv()
 
 rosbuild_add_boost_directories()
 
-SET(PKG_CONFIG_ADDITIONAL_VARIABLES
+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)
+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)
 
-file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/ros_export")
-file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/ros_import")
-file(MAKE_DIRECTORY
-  "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/ros_joint_state")
-
 rosbuild_add_library(ros_bridge
   src/converter.hh
   include/dynamic_graph_bridge/ros_init.hh src/ros_init.cpp
@@ -43,58 +38,61 @@ rosbuild_add_library(ros_bridge
 # are not installed.
 set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True)
 
-MACRO(COMPILE_PLUGIN NAME)
+macro(compile_plugin NAME)
+  file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${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)
+  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)
+  install(TARGETS ${NAME} DESTINATION lib)
 
-  DYNAMIC_GRAPH_PYTHON_MODULE("ros/${NAME}"
+  dynamic_graph_python_module("ros/${NAME}"
     ${NAME}
     ros/${NAME}/wrap
     )
 
-  SET_TARGET_PROPERTIES(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()
+endmacro()
 
-INCLUDE(cmake/python.cmake)
+include(cmake/python.cmake)
 
-INCLUDE_DIRECTORIES(${DYNAMIC_GRAPH_INCLUDE_DIRS})
-LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
+include_directories(${DYNAMIC_GRAPH_include_DIRS})
+link_directories(${DYNAMIC_GRAPH_LIBRARY_DIRS})
 
-COMPILE_PLUGIN(ros_import)
-COMPILE_PLUGIN(ros_export)
-COMPILE_PLUGIN(ros_export)
+compile_plugin(ros_import)
+compile_plugin(ros_export)
+compile_plugin(ros_export)
 
-COMPILE_PLUGIN(ros_joint_state)
+compile_plugin(ros_joint_state)
 target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
 
+compile_plugin(robot_model)
+
 # 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)
+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)
+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)
+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)
+install(TARGETS interpreter DESTINATION bin)
 
-ADD_SUBDIRECTORY(src)
+add_subdirectory(src)

manifest.xml

@@ -32,4 +32,6 @@
   <depend package="tf"/>
 
   <depend package="realtime_tools"/>
+
+  <depend package="jrl_dynamics_urdf"/>
 </package>

src/dynamic_graph/ros/__init__.py

 from ros_import import RosImport
 from ros_export import RosExport
 from ros_joint_state import RosJointState
+from robot_model import RosRobotModel
 
 from ros import Ros

src/robot_model.cpp

+#include <limits>
+
+#include <boost/bind.hpp>
+
+#include <ros/ros.h>
+
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/null-ptr.hh>
+#include <jrl/dynamics/urdf/parser.hh>
+
+#include "dynamic_graph_bridge/ros_init.hh"
+#include "robot_model.hh"
+
+namespace dynamicgraph
+{
+  namespace dg = dynamicgraph;
+
+  namespace command
+  {
+    LoadFromParameterServer::LoadFromParameterServer
+    (RosRobotModel& entity,
+     const std::string& docstring)
+      : Command (entity, std::vector<Value::Type>(), docstring)
+    {}
+
+    Value
+    LoadFromParameterServer::doExecute()
+    {
+      RosRobotModel& entity =
+	static_cast<RosRobotModel&>(owner ());
+      entity.loadFromParameterServer ();
+      return Value ();
+    }
+
+    LoadUrdf::LoadUrdf(RosRobotModel& entity, const std::string& docstring)
+      : Command (entity, std::vector<Value::Type>(), docstring)
+    {}
+
+    Value
+    LoadUrdf::doExecute()
+    {
+      RosRobotModel& entity =
+	static_cast<RosRobotModel&>(owner ());
+
+      const std::vector<Value>& values = getParameterValues ();
+      std::string resourceName = values[0].value ();
+
+      entity.loadUrdf (resourceName);
+      return Value ();
+    }
+  } // end of namespace command.
+
+  RosRobotModel::RosRobotModel (const std::string& name)
+    : Entity(name),
+      robot_ (0),
+      lastComputation_ (std::numeric_limits<int>::min()),
+      q_ (dynamicgraph::nullptr,
+	  "RosRobotModel(" + name + ")::input(vector)::q"),
+      dq_ (dynamicgraph::nullptr,
+	   "RosRobotModel(" + name + ")::input(vector)::dq"),
+      ddq_ (dynamicgraph::nullptr,
+	    "RosRobotModel(" + name + ")::input(vector)::ddq")
+  {
+    signalRegistration(q_);
+    signalRegistration(dq_);
+    signalRegistration(ddq_);
+
+    std::string docstring;
+
+    docstring =
+      "\n"
+      "  Load the robot model from the parameter server.\n"
+      "\n"
+      "  This is the recommended method.\n"
+      "\n";
+    addCommand ("loadFromParameterServer",
+		new command::LoadFromParameterServer (*this, docstring));
+
+    docstring =
+      "\n"
+      "  Load the robot model from an URDF file.\n"
+      "\n";
+    addCommand ("loadUrdf", new command::LoadUrdf (*this, docstring));
+  }
+
+  RosRobotModel::~RosRobotModel ()
+  {}
+
+  void
+  RosRobotModel::loadUrdf (const std::string& filename)
+  {
+    jrl::dynamics::urdf::Parser parser;
+    robot_ = parser.parse(filename, "base_footprint_joint");
+    buildSignals ();
+  }
+
+  void
+  RosRobotModel::loadFromParameterServer ()
+  {
+    jrl::dynamics::urdf::Parser parser;
+
+    ros::NodeHandle& nh = 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.");
+    robot_ = parser.parseStream (robotDescription, "base_footprint_joint");
+    buildSignals ();
+  }
+
+  void
+  RosRobotModel::buildSignals ()
+  {
+    // iterate on tree nodes and add signals
+    typedef dg::SignalTimeDependent<MatrixHomogeneous, int> signal_t;
+    ::std::vector<CjrlJoint*> jointsVect = robot_->jointVector();
+
+    for (uint i=0; i<jointsVect.size(); ++i)
+      {
+	CjrlJoint* currentJoint = jointsVect[i];
+	std::string signame = currentJoint->getName();
+
+	signal_t* sig
+	  = new signal_t
+	  (boost::bind
+	   (&RosRobotModel::computeBodyPosition, this, currentJoint, _1, _2),
+	   0,
+	   "RosRobotModel(" + getName () + ")::output(matrix)::" + signame);
+	sig->addDependency (q_);
+	sig->addDependency (dq_);
+	sig->addDependency (ddq_);
+	genericSignalRefs_.push_back (sig);
+	signalRegistration (*sig);
+      }
+    // force recomputation as the model has changed.
+    lastComputation_ = std::numeric_limits<int>::min();
+  }
+
+  RosRobotModel::MatrixHomogeneous&
+  RosRobotModel::computeBodyPosition (CjrlJoint* joint,
+				      MatrixHomogeneous& position,
+				      int t)
+  {
+    update (t);
+    for(unsigned i = 0; i < position.nbRows(); ++i)
+      for(unsigned j = 0; j < position.nbCols(); ++j)
+	position.elementAt(i,j) =
+	  joint->currentTransformation().m[i * position.nbCols() + j];
+    return position;
+  }
+
+  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;
+    }
+  } // end of anonymous namespace.
+
+  void
+  RosRobotModel::update (int t)
+  {
+    if (t <= lastComputation_)
+      return;
+
+    vectorN q = convertVector (q_ (t));
+    vectorN dq = convertVector (dq_ (t));
+    vectorN ddq = convertVector (dq_ (t));
+
+    if (!robot_->currentConfiguration(q))
+      throw std::runtime_error ("failed to update current configuration");
+    if (!robot_->currentVelocity(dq))
+      throw std::runtime_error ("failed to update current velocity");
+    if (!robot_->currentAcceleration(ddq))
+      throw std::runtime_error ("failed to update current acceleration");
+    robot_->computeForwardKinematics();
+    lastComputation_ = t;
+  }
+
+  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
+} // end of namespace dynamicgraph.

src/robot_model.hh

+#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
+# define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
+# include <list>
+# include <string>
+
+# include <jrl/mal/boost.hh>
+# include "jrl/mal/matrixabstractlayer.hh"
+
+namespace ml = maal::boost;
+
+# include <jrl/dynamics/dynamicsfactory.hh>
+
+//# include <sot/core/flags.hh>
+# include <dynamic-graph/command.h>
+# include <dynamic-graph/entity.h>
+//# include <dynamic-graph/pool.h>
+# include <dynamic-graph/signal-ptr.h>
+# include <dynamic-graph/signal-time-dependent.h>
+//# include <sot/core/exception-dynamic.hh>
+# include <sot/core/matrix-homogeneous.hh>
+
+namespace dynamicgraph
+{
+  class RosRobotModel;
+
+  namespace command
+  {
+    using ::dynamicgraph::command::Command;
+    using ::dynamicgraph::command::Value;
+
+    /// \brief Load from the robot_description parameter of the
+    /// parameter server.
+    ///
+    /// This is the recommended method as it ensures model consistency
+    /// between the control and the other nodes.
+    class LoadFromParameterServer : public Command
+    {
+    public:
+      explicit LoadFromParameterServer(RosRobotModel& entity,
+				       const std::string& docstring);
+      Value doExecute();
+    };
+
+    /// \brief Load model from an URDF file.
+    class LoadUrdf : public Command
+    {
+    public:
+      explicit LoadUrdf(RosRobotModel& entity, const std::string& docstring);
+      Value doExecute();
+    };
+  } // end of namespace command.
+
+  /// \brief This entity load either the current model available in
+  /// the robot_description parameter or a specified file and provides
+  /// various data such as body positions, jacobians, etc.
+  ///
+  /// This relies on jrl_dynamics_urdf to load the model and jrl-dynamics
+  /// to realize the computation.
+  class RosRobotModel : public dynamicgraph::Entity
+  {
+    DYNAMIC_GRAPH_ENTITY_DECL();
+  public:
+    typedef ::dynamicgraph::sot::MatrixHomogeneous MatrixHomogeneous;
+    RosRobotModel(const std::string& n);
+    virtual ~RosRobotModel();
+
+    void loadUrdf(const std::string& filename);
+    void loadFromParameterServer();
+
+  protected:
+    void buildSignals();
+
+    /// \brief Callback Computing the position of the body attached
+    /// to the provided joint.
+    ///
+    MatrixHomogeneous&
+    computeBodyPosition (CjrlJoint* joint,
+			 MatrixHomogeneous& position,
+			 int t);
+
+    /// \brief Update data if necessary by updating the robot
+    /// configuration/velocity/acceleration.
+    ///
+    /// \param t current time
+    void update (int t);
+  private:
+    CjrlHumanoidDynamicRobot* robot_;
+    std::list< ::dynamicgraph::SignalBase<int>* > genericSignalRefs_;
+
+    /// \brief When did the last computation occur?
+    int lastComputation_;
+
+    /// \brief Robot current configuration.
+    dynamicgraph::SignalPtr<ml::Vector,int> q_;
+    /// \brief Robot current velocity.
+    dynamicgraph::SignalPtr<ml::Vector,int> dq_;
+    /// \brief Robot current acceleration.
+    dynamicgraph::SignalPtr<ml::Vector,int> ddq_;
+  };
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH