diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24e0fa89438c80184ac5a84b922e2811892c3b97..1b8de66329a637722baff2f8dd1b953a3be27626 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(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)
diff --git a/manifest.xml b/manifest.xml
index 961c4171bedd68e6ba0f74f737ae9d8a51806007..93b40043f278d0d7016cd46c324dbe9959fd4d7b 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -32,4 +32,6 @@
<depend package="tf"/>
<depend package="realtime_tools"/>
+
+ <depend package="jrl_dynamics_urdf"/>
</package>
diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py
index 99c5f33b67d7a87d7137420b9bb41b4701a3e8b5..dc9d0eab021620080159d0310e8c085fde0cd72a 100644
--- a/src/dynamic_graph/ros/__init__.py
+++ b/src/dynamic_graph/ros/__init__.py
@@ -1,5 +1,6 @@
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
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b0de6eeaff65f6cca598d44055f43117b9507a7
--- /dev/null
+++ b/src/robot_model.cpp
@@ -0,0 +1,186 @@
+#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.
diff --git a/src/robot_model.hh b/src/robot_model.hh
new file mode 100644
index 0000000000000000000000000000000000000000..d5239c07f3eadc1854e5db711245088082658c0f
--- /dev/null
+++ b/src/robot_model.hh
@@ -0,0 +1,102 @@
+#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