Commit 23041e0a authored by olivier stasse's avatar olivier stasse
Browse files

Merge pull request #2 from francois-keith/master

Enable to create a controller for Romeo.
parents 5866b2dc d26b3a6c
[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
Subproject commit 52a6b185463163784cf6b17f5b92438cc53aeddf
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
......@@ -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()
......@@ -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
)
// 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.
// 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
/*
* 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;
}
}
/*
* 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_ */
/*
* 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);