Merge pull request #2 from francois-keith/master

Enable to create a controller for Romeo.

.gitmodules

 [submodule "cmake"]
 	path = cmake
 	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
+[submodule ".travis"]
+	path = .travis
+	url = git://github.com/jrl-umi3218/jrl-travis

.travis @ 52a6b185

+Subproject commit 52a6b185463163784cf6b17f5b92438cc53aeddf

.travis.yml

+language: cpp
+compiler: gcc
+env:
+  global:
+  - APT_DEPENDENCIES="doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev
+    libblas-dev gfortran python-dev python-sphinx python-numpy libbullet-dev
+    ros-hydro-angles 
+    ros-hydro-common-msgs
+    ros-hydro-control-msgs
+    ros-hydro-realtime-tools
+    ros-hydro-resource-retriever
+    ros-hydro-robot-state-publisher
+    ros-hydro-ros-base
+    ros-hydro-tf
+    ros-hydro-urdf
+    ros-hydro-urdfdom-py"
+  - GIT_DEPENDENCIES="jrl-umi3218/jrl-mathtools jrl-umi3218/jrl-mal laas/abstract-robot-dynamics jrl-umi3218/jrl-dynamics
+    stack-of-tasks/dynamic-graph stack-of-tasks/dynamic-graph-python stack-of-tasks/sot-core
+    stack-of-tasks/sot-tools stack-of-tasks/sot-dynamic 
+    francois-keith/jrl_dynamics_urdf:topic/fixed_joints"
+  - ROS_VERSION=hydro
+  - ROS_GIT_DEPENDENCIES="francois-keith/dynamic_graph_bridge_msgs:catkin francois-keith/dynamic_graph_bridge:catkin laas:romeo"
+  - ALLOW_CATKINLINT_FAILURE=true
+notifications:
+  email:
+  - francois.keith@gmail.com
+branches:
+  only:
+  - master
+  - catkin
+  - travis
+script: ./.travis/run build
+after_success: ./.travis/run after_success
+after_failure: ./.travis/run after_failure
+before_install: ./.travis/run dependencies/catkin; ./.travis/run before_install
+

CMakeLists.txt

@@ -17,8 +17,8 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/lapack.cmake)
 INCLUDE(cmake/cpack.cmake)
+INCLUDE(cmake/ros.cmake)
 
 SET(PROJECT_NAME sot-romeo)
 SET(PROJECT_DESCRIPTION "dynamic-graph package for Romeo robot")
@@ -38,12 +38,28 @@ SETUP_PROJECT()
 
 # Those packages are not directly linked, but are used in the 
 #  python scripts
+ADD_REQUIRED_DEPENDENCY("dynamic-graph")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
 ADD_REQUIRED_DEPENDENCY("sot-core >= 2.7")
 ADD_REQUIRED_DEPENDENCY("sot-tools")
 ADD_REQUIRED_DEPENDENCY("sot-dynamic >= 2.6")
 
+ADD_ROSPACK_DEPENDENCY("dynamic_graph_bridge")
+# Search for dependencies.
+# Boost
+SET(BOOST_COMPONENTS filesystem system thread)
+SEARCH_FOR_BOOST()
+
+# Handle rpath necessary to handle ROS multiplace packages
+# libraries inclusion
+SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
+SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
 # Add subdirectories.
 ADD_SUBDIRECTORY(src)
+#ADD_SUBDIRECTORY(python)
 
 SETUP_PROJECT_FINALIZE()
 SETUP_PROJECT_CPACK()

src/CMakeLists.txt

@@ -17,6 +17,34 @@ INCLUDE(../cmake/python.cmake)
 FINDPYTHON()
 INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
 
+##
+FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
+  ADD_LIBRARY(${NAME} SHARED ${SOURCES})
+  SET_TARGET_PROPERTIES(${lib} PROPERTIES
+    PREFIX ""
+    SOVERSION ${PROJECT_VERSION})
+
+  PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph)
+  PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core)
+
+  INSTALL(TARGETS ${NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
+
+  # build python submodule
+  STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${NAME})
+  SET(NEW_ENTITY_CLASS ${ENTITIES})
+  DYNAMIC_GRAPH_PYTHON_MODULE("sot/romeo/${PYTHON_LIBRARY_NAME}"
+    ${NAME}
+    sot-romeo-${PYTHON_LIBRARY_NAME}-wrap
+    )
+ENDFUNCTION()
+
+# Compile plug-ins.
+COMPILE_PLUGIN(dynamic-romeo dynamic-romeo.cpp DynamicRomeo)
+
+
+## 
+CONFIG_FILES(dynamic_graph/sot/romeo/robot.py)
+
 # Install Python files.
 SET(PYTHON_MODULE_DIR
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/romeo)
@@ -29,3 +57,34 @@ PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py" )
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
 
 
+
+# Add the library to wrap the controller of Romeo.
+SET(CONTROLLER_NAME sot-romeo-controller)
+ADD_LIBRARY(${CONTROLLER_NAME}
+  SHARED
+  sot-romeo-controller.cpp
+  sot-romeo-device.cpp
+)
+
+# Link the dynamic library containing the SoT with its dependencies.
+PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph")
+PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph-python")
+PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "sot-core")
+
+ROSPACK_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic_graph_bridge")
+
+
+IF(UNIX AND NOT APPLE)
+  TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${Boost_LIBRARIES})
+ENDIF(UNIX AND NOT APPLE)
+
+INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
+
+# build python submodule
+STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${CONTROLLER_NAME})
+SET(NEW_ENTITY_CLASS ${ENTITIES})
+DYNAMIC_GRAPH_PYTHON_MODULE("sot/romeo/${PYTHON_LIBRARY_NAME}"
+  ${CONTROLLER_NAME}
+  sot-romeo-${PYTHON_LIBRARY_NAME}-wrap
+  )
+

src/dynamic-romeo.cpp

+// Copyright 2010, François Bleibel, Thomas Moulard, Olivier Stasse,
+// JRL, CNRS/AIST.
+//
+// This file is part of dynamic-graph.
+// dynamic-graph is free software: you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public License
+// as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// dynamic-graph is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <sot/core/debug.hh>
+
+#include <dynamic-graph/factory.h>
+#include "dynamic-romeo.hh"
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace romeo
+    {
+
+      DynamicRomeo::DynamicRomeo (const std::string & name)
+	: Dynamic (name, false)
+      {
+	sotDEBUGIN(15);
+	DynamicRomeo::buildModel ();
+	sotDEBUGOUT(15);
+      }
+
+      DynamicRomeo::~DynamicRomeo ()
+      {
+	sotDEBUGINOUT(5);
+	return;
+      }
+
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DynamicRomeo,"DynamicRomeo");
+    } // end of namespace romeo.
+  } // end of namespace sot.
+} // end of namespace dynamicgraph.

src/dynamic-romeo.hh

+// Copyright 2010, François Bleibel, Thomas Moulard, Olivier Stasse,
+// JRL, CNRS/AIST.
+//
+// This file is part of sot-romeo.
+// sot-romeo is free software: you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public License
+// as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// sot-romeo is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// sot-romeo. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SOT_ROMEO_DYNAMIC_HH
+# define SOT_ROMEO_DYNAMIC_HH
+# include <sot-dynamic/dynamic.h>
+
+# if defined (WIN32)
+#   if defined (dynamic_romeo_EXPORTS)
+#     define DYNAMICROMEO_EXPORT __declspec(dllexport)
+#   else
+#     define DYNAMICROMEO_EXPORT __declspec(dllimport)
+#   endif
+# else
+#   define DYNAMICROMEO_EXPORT
+# endif
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace romeo
+    {
+      class DYNAMICROMEO_EXPORT DynamicRomeo : public Dynamic
+      {
+	DYNAMIC_GRAPH_ENTITY_DECL ();
+      public:
+	explicit DynamicRomeo (const std::string& name);
+	virtual ~DynamicRomeo ();
+      };
+    } // end of namespace romeo.
+  } // end of namespace sot.
+} // end of namespace dynamicgraph.
+
+
+#endif //! SOT_ROMEO_DYNAMIC_HH

src/sot-romeo-controller.cpp

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of romeoController.
+ */
+
+#include <sot/core/debug.hh>
+#include <sot/core/exception-abstract.hh>
+#include <dynamic_graph_bridge/ros_init.hh>
+#include <dynamic_graph_bridge/ros_interpreter.hh>
+
+#include "sot-romeo-controller.hh"
+
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
+const std::string SoTRomeoController::LOG_PYTHON="/tmp/RomeoController_python.out";
+
+using namespace std;
+
+boost::condition_variable cond;
+boost::mutex mut;
+bool data_ready;
+
+#define ROBOTNAME std::string("ROMEO")
+
+void workThread(SoTRomeoController *aSoTRomeo)
+{
+  
+  dynamicgraph::Interpreter aLocalInterpreter(dynamicgraph::rosInit(false,true));
+
+  aSoTRomeo->interpreter_ = 
+    boost::make_shared<dynamicgraph::Interpreter>(aLocalInterpreter);
+  std::cout << "Going through the thread." << std::endl;
+  {
+    boost::lock_guard<boost::mutex> lock(mut);
+    data_ready=true;
+  }
+  cond.notify_all();
+  ros::waitForShutdown();
+}
+
+SoTRomeoController::SoTRomeoController():
+  device_(ROBOTNAME)
+{
+
+  std::cout << "Going through SoTRomeoController." << std::endl;
+  boost::thread thr(workThread,this);
+  sotDEBUG(25) << __FILE__ << ":" 
+	       << __FUNCTION__ <<"(#" 
+	       << __LINE__ << " )" << std::endl;
+
+  boost::unique_lock<boost::mutex> lock(mut);
+  cond.wait(lock);
+
+  startupPython();
+  interpreter_->startRosService ();
+}
+
+SoTRomeoController::~SoTRomeoController()
+{
+}
+
+void SoTRomeoController::
+setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
+{
+  device_.setupSetSensors(SensorsIn);
+}
+
+
+void SoTRomeoController::
+nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
+{
+  device_.nominalSetSensors(SensorsIn);
+}
+
+void SoTRomeoController::
+cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
+{
+  device_.cleanupSetSensors(SensorsIn);
+}
+
+
+void SoTRomeoController::
+getControl(map<string,dgsot::ControlValues> &controlOut)
+{
+  try 
+    {
+      sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
+      device_.getControl(controlOut);
+      sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
+    }
+  catch ( dynamicgraph::sot::ExceptionAbstract & err)
+    {
+
+      std::cout << __FILE__ << " " 
+		<< __FUNCTION__ << " (" 
+		<< __LINE__ << ") " 
+		<< err.getStringMessage() 
+		<<  endl;
+      throw err;
+    }
+}
+
+void SoTRomeoController::
+runPython(std::ostream& file,
+	  const std::string& command,
+	  dynamicgraph::Interpreter& interpreter)
+{
+  file << ">>> " << command << std::endl;
+  std::string lerr(""),lout(""),lres("");
+  interpreter.runCommand(command,lres,lout,lerr);
+  if (lres != "None")
+    {
+      if (lres=="<NULL>")
+	{
+	  file << lout << std::endl;
+	  file << "------" << std::endl;
+	  file << lerr << std::endl;
+	}
+      else
+	file << lres << std::endl;
+    }
+}
+
+void SoTRomeoController::
+startupPython()
+{
+  std::ofstream aof(LOG_PYTHON.c_str());
+  runPython (aof, "import sys, os", *interpreter_);
+  runPython (aof, "pythonpath = os.environ['PYTHONPATH']", *interpreter_);
+  runPython (aof, "path = []", *interpreter_);
+  runPython (aof,
+	     "for p in pythonpath.split(':'):\n"
+	     "  if p not in sys.path:\n"
+	     "    path.append(p)", *interpreter_);
+  runPython (aof, "path.extend(sys.path)", *interpreter_);
+  runPython (aof, "sys.path = path", *interpreter_);
+
+  // Calling again rosInit here to start the spinner. It will
+  // deal with topics and services callbacks in a separate, non
+  // real-time thread. See roscpp documentation for more
+  // information.
+  dynamicgraph::rosInit (true);
+
+  runPython
+    (aof,
+     "from dynamic_graph.sot.romeo.prologue import robot",
+     *interpreter_);
+  aof.close();
+}
+
+
+extern "C" 
+{
+  dgsot::AbstractSotExternalInterface * createSotExternalInterface()
+  {
+    return new SoTRomeoController;
+  }
+}
+
+extern "C"
+{
+  void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
+  {
+    delete p;
+  }
+}
+

src/sot-romeo-controller.hh

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of RomeoController.
+ */
+
+#ifndef _SOT_RomeoController_H_
+#define _SOT_RomeoController_H_
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/device.hh>
+#include <sot/core/abstract-sot-external-interface.hh>
+
+#include "sot-romeo-device.hh"
+
+namespace dgsot=dynamicgraph::sot;
+
+class SoTRomeoController: public 
+  dgsot::AbstractSotExternalInterface
+{
+ public:
+
+  static const std::string LOG_PYTHON;
+  
+  SoTRomeoController();
+  virtual ~SoTRomeoController();
+
+  void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
+
+  void nominalSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
+
+  void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
+
+  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
+
+  /// Embedded python interpreter accessible via Corba/ros
+  boost::shared_ptr<dynamicgraph::Interpreter> interpreter_;
+
+ protected:
+  // Update output port with the control computed from the
+  // dynamic graph.
+  void updateRobotState(std::vector<double> &anglesIn);
+  
+  void runPython(std::ostream& file,
+		 const std::string& command,
+		 dynamicgraph::Interpreter& interpreter);
+  
+  virtual void startupPython();
+    
+
+  SoTRomeoDevice device_;
+};
+
+#endif /* _SOT_RomeoController_H_ */
+

src/sot-romeo-device.cpp

+/*
+ * Copyright 2011,
+ *
+ * Olivier Stasse
+ *
+ * LAAS, CNRS
+ *
+ * This file is part of RomeoController.
+ *
+ */
+
+#include <fstream>
+#include <map>
+
+#include <sot/core/debug.hh>
+
+#include "sot-romeo-device.hh"
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/all-commands.h>
+
+using namespace std;
+
+const double SoTRomeoDevice::TIMESTEP_DEFAULT = 0.005;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTRomeoDevice,"Device");
+
+SoTRomeoDevice::SoTRomeoDevice(std::string RobotName):
+  dgsot::Device(RobotName),
+  timestep_(TIMESTEP_DEFAULT),
+  previousState_ (),
+  robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
+  baseff_ ()
+{
+  sotDEBUGIN(25) ;
+  signalRegistration (robotState_);
+  ml::Vector data (3); data.setZero ();
+  baseff_.resize(12);
+
+  using namespace dynamicgraph::command;
+  std::string docstring;
+  /* Command increment. */
+  docstring =
+    "\n"
+    "    Integrate dynamics for time step provided as input\n"
+    "\n"
+    "      take one floating point number as input\n"
+    "\n";
+  addCommand("increment",
+	     makeCommandVoid1((Device&)*this,
+			      &Device::increment, docstring));
+  
+  sotDEBUGOUT(25);
+}
+
+SoTRomeoDevice::~SoTRomeoDevice()
+{ }
+
+void SoTRomeoDevice::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
+{
+  sotDEBUGIN(25) ;
+  map<string,dgsot::SensorValues>::iterator it;
+  int t = stateSOUT.getTime () + 1;
+/*
+  it = SensorsIn.find("forces");
+  if (it!=SensorsIn.end())
+    {
+
+      // Implements force recollection.
+      const vector<double>& forcesIn = it->second.getValues();
+      for(int i=0;i<4;++i)
+        {
+          for(int j=0;j<6;++j)
+            mlforces(j) = forcesIn[i*6+j];
+          forcesSOUT[i]->setConstant(mlforces);
+          forcesSOUT[i]->setTime (t);
+        }
+    }
+
+  it = SensorsIn.find("attitude");
+  if (it!=SensorsIn.end())
+    {
+      const vector<double>& attitude = it->second.getValues ();
+      for (unsigned int i = 0; i < 3; ++i)
+        for (unsigned int j = 0; j < 3; ++j)
+          pose (i, j) = attitude [i * 3 + j];
+      attitudeSOUT.setConstant (pose);
+      attitudeSOUT.setTime (t);
+    }
+*/
+  it = SensorsIn.find("joints");
+  if (it!=SensorsIn.end())
+    {
+      const vector<double>& anglesIn = it->second.getValues();
+      mlRobotState.resize (anglesIn.size () + 6);
+      for (unsigned i = 0; i < 6; ++i)
+        mlRobotState (i) = 0.;
+      updateRobotState(anglesIn);
+    }
+/*
+  it = SensorsIn.find("accelerometer_0");
+  if (it!=SensorsIn.end())
+    {
+      const vector<double>& accelerometer = 
+        SensorsIn ["accelerometer_0"].getValues ();
+      for (std::size_t i=0; i<3; ++i) 
+        accelerometer_ (i) = accelerometer [i];
+      accelerometerSOUT_.setConstant (accelerometer_);
+    }
+
+  it = SensorsIn.find("gyrometer_0");
+  if (it!=SensorsIn.end())
+    {
+      const vector<double>& gyrometer = SensorsIn ["gyrometer_0"].getValues ();
+      for (std::size_t i=0; i<3; ++i) 
+        gyrometer_ (i) = gyrometer [i];
+      gyrometerSOUT_.setConstant (gyrometer_);
+    }
+*/
+  sotDEBUGOUT(25);
+}
+
+void SoTRomeoDevice::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
+{
+  setSensors (SensorsIn);
+}
+
+
+void SoTRomeoDevice::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
+{
+  setSensors (SensorsIn);
+}
+
+void SoTRomeoDevice::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
+{
+  setSensors (SensorsIn);