Commit bc826e50 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Format

parent 43892dd4
Pipeline #8584 failed with stage
in 1 minute and 6 seconds
/* Copyright 2018, CNRS /* Copyright 2018, CNRS
* Author: O. Stasse * Author: O. Stasse
* *
BSD 2-Clause License BSD 2-Clause License
Copyright (c) 2017, Stack Of Tasks development team Copyright (c) 2017, Stack Of Tasks development team
...@@ -33,16 +33,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -33,16 +33,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
\mainpage \mainpage
\section sec_intro Introduction \section sec_intro Introduction
This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the Stack-Of-Tasks. This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the
A derived class is used for the first prototype of this line of humanoid robot, Pyrene. Stack-Of-Tasks. A derived class is used for the first prototype of this line of humanoid robot, Pyrene.
This class is implementing a perception-action loop. This class is implementing a perception-action loop.
To achieve this goal it is providing a Hardware Abstract Layer to communicate with the SoT control infra-structure and the robot or a simulator. To achieve this goal it is providing a Hardware Abstract Layer to communicate with the SoT control infra-structure and
It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or with the Gazebo simulator. the robot or a simulator. It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or
with the Gazebo simulator.
The sot-talos package contains also the class sot-talos-controller. This class is used to start the multithreading environment which is handling ROS request, starts The sot-talos package contains also the class sot-talos-controller. This class is used to start the multithreading
the python interpreter to control the SoT and finally handle environment which is handling ROS request, starts the python interpreter to control the SoT and finally handle the
the control states: initialization, nominal usage, stopping or cancellation. control states: initialization, nominal usage, stopping or cancellation.
It also provides python scripts to be used to interact with the robot. It also provides python scripts to be used to interact with the robot.
...@@ -57,6 +58,6 @@ It also provides python scripts to be used to interact with the robot. ...@@ -57,6 +58,6 @@ It also provides python scripts to be used to interact with the robot.
<li> motor_angles </li> <li> motor_angles </li>
<li> p_gains </li> <li> p_gains </li>
<li> d_gains</li> <li> d_gains</li>
\section sot-talos-controller \section sot-talos-controller
*/ */
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
* LAAS, CNRS * LAAS, CNRS
* *
* This file is part of TALOSController. * This file is part of TALOSController.
* TALOSController is a free software, * TALOSController is a free software,
* *
*/ */
...@@ -17,43 +17,26 @@ ...@@ -17,43 +17,26 @@
#include "sot-pyrene-controller.hh" #include "sot-pyrene-controller.hh"
const std::string SoTPyreneController::LOG_PYTHON_PYRENE="/tmp/PyreneController_python.out"; const std::string SoTPyreneController::LOG_PYTHON_PYRENE = "/tmp/PyreneController_python.out";
SoTPyreneController::SoTPyreneController(): SoTPyreneController::SoTPyreneController() : SoTTalosController(ROBOTNAME) {
SoTTalosController(ROBOTNAME)
{
startupPython(); startupPython();
interpreter_->startRosService (); interpreter_->startRosService();
} }
void SoTPyreneController::startupPython() void SoTPyreneController::startupPython() {
{
SoTTalosController::startupPython(); SoTTalosController::startupPython();
std::ofstream aof(LOG_PYTHON_PYRENE.c_str()); std::ofstream aof(LOG_PYTHON_PYRENE.c_str());
runPython runPython(aof, "from dynamic_graph.sot.pyrene.prologue import makeRobot", *interpreter_);
(aof, runPython(aof, "robot = makeRobot ()", *interpreter_);
"from dynamic_graph.sot.pyrene.prologue import makeRobot",
*interpreter_);
runPython
(aof,
"robot = makeRobot ()",
*interpreter_);
aof.close(); aof.close();
} }
extern "C" extern "C" {
{ dgsot::AbstractSotExternalInterface *createSotExternalInterface() { return new SoTPyreneController; }
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
return new SoTPyreneController;
}
} }
extern "C" extern "C" {
{ void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p) { delete p; }
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
{
delete p;
}
} }
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
* LAAS, CNRS * LAAS, CNRS
* *
* This file is part of TALOSController. * This file is part of TALOSController.
* TALOSController is a free software, * TALOSController is a free software,
* *
* *
*/ */
...@@ -15,22 +15,17 @@ ...@@ -15,22 +15,17 @@
#define _SOT_PYRENE_Controller_H_ #define _SOT_PYRENE_Controller_H_
#include "sot-talos-controller.hh" #include "sot-talos-controller.hh"
namespace dgsot=dynamicgraph::sot; namespace dgsot = dynamicgraph::sot;
class SoTPyreneController: public SoTTalosController class SoTPyreneController : public SoTTalosController {
{
public: public:
static const std::string LOG_PYTHON_PYRENE; static const std::string LOG_PYTHON_PYRENE;
SoTPyreneController(); SoTPyreneController();
virtual ~SoTPyreneController() {}; virtual ~SoTPyreneController(){};
protected: protected:
virtual void startupPython(); virtual void startupPython();
}; };
#endif /* _SOTTalosController_H_ */ #endif /* _SOTTalosController_H_ */
...@@ -23,143 +23,96 @@ ...@@ -23,143 +23,96 @@
#include <ros/console.h> #include <ros/console.h>
const std::string SoTTalosController::LOG_PYTHON="/tmp/TalosController_python.out"; const std::string SoTTalosController::LOG_PYTHON = "/tmp/TalosController_python.out";
using namespace std; using namespace std;
SoTTalosController::SoTTalosController(std::string RobotName): SoTTalosController::SoTTalosController(std::string RobotName) : device_(new SoTTalosDevice(RobotName)) { init(); }
device_(new SoTTalosDevice(RobotName))
{
init();
}
SoTTalosController::SoTTalosController(const char robotName[]): SoTTalosController::SoTTalosController(const char robotName[]) : device_(new SoTTalosDevice(robotName)) { init(); }
device_(new SoTTalosDevice(robotName))
{
init();
}
void SoTTalosController::init() void SoTTalosController::init() {
{
std::cout << "Going through SoTTalosController." << std::endl; std::cout << "Going through SoTTalosController." << std::endl;
// rosInit is called here only to initialize ros. // rosInit is called here only to initialize ros.
// No spinner is initialized. // No spinner is initialized.
ros::NodeHandle &nh = dynamicgraph::rosInit(false,false); ros::NodeHandle &nh = dynamicgraph::rosInit(false, false);
interpreter_ = boost::shared_ptr<dynamicgraph::Interpreter>( interpreter_ = boost::shared_ptr<dynamicgraph::Interpreter>(new dynamicgraph::Interpreter(nh));
new dynamicgraph::Interpreter (nh));
sotDEBUG(25) << __FILE__ << ":" sotDEBUG(25) << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << " )" << std::endl;
<< __FUNCTION__ <<"(#"
<< __LINE__ << " )" << std::endl;
double ts = ros::param::param<double> ("/sot_controller/dt", SoTTalosDevice::TIMESTEP_DEFAULT); double ts = ros::param::param<double>("/sot_controller/dt", SoTTalosDevice::TIMESTEP_DEFAULT);
device_->timeStep(ts); device_->timeStep(ts);
} }
SoTTalosController::~SoTTalosController() SoTTalosController::~SoTTalosController() {
{
// device_ will be deleted by dynamicgraph::PoolStorage::destroy() // device_ will be deleted by dynamicgraph::PoolStorage::destroy()
} }
void SoTTalosController:: void SoTTalosController::setupSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
device_->setupSetSensors(SensorsIn); device_->setupSetSensors(SensorsIn);
} }
void SoTTalosController::nominalSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
void SoTTalosController::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
device_->nominalSetSensors(SensorsIn); device_->nominalSetSensors(SensorsIn);
} }
void SoTTalosController:: void SoTTalosController::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
device_->cleanupSetSensors(SensorsIn); device_->cleanupSetSensors(SensorsIn);
} }
void SoTTalosController::getControl(map<string, dgsot::ControlValues> &controlOut) {
void SoTTalosController:: try {
getControl(map<string,dgsot::ControlValues> &controlOut) sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
{ device_->getControl(controlOut);
try sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
{ } catch (dynamicgraph::sot::ExceptionAbstract &err) {
sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl; std::cout << __FILE__ << " " << __FUNCTION__ << " (" << __LINE__ << ") " << err.getStringMessage() << endl;
device_->getControl(controlOut); throw err;
sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl; }
}
catch ( dynamicgraph::sot::ExceptionAbstract & err)
{
std::cout << __FILE__ << " "
<< __FUNCTION__ << " ("
<< __LINE__ << ") "
<< err.getStringMessage()
<< endl;
throw err;
}
} }
void SoTTalosController:: void SoTTalosController::setNoIntegration(void) { device_->setNoIntegration(); }
setNoIntegration(void)
{
device_->setNoIntegration();
}
void SoTTalosController:: void SoTTalosController::setSecondOrderIntegration(void) { device_->setSecondOrderIntegration(); }
setSecondOrderIntegration(void)
{
device_->setSecondOrderIntegration();
}
void SoTTalosController:: void SoTTalosController::runPython(std::ostream &file, const std::string &command,
runPython(std::ostream& file, dynamicgraph::Interpreter &interpreter) {
const std::string& command,
dynamicgraph::Interpreter& interpreter)
{
file << ">>> " << command << std::endl; file << ">>> " << command << std::endl;
std::string lres(""),lout(""),lerr(""); std::string lres(""), lout(""), lerr("");
interpreter.runCommand(command,lres,lout,lerr); interpreter.runCommand(command, lres, lout, lerr);
if (lres != "None") if (lres != "None") {
{ if (lres == "<NULL>") {
if (lres=="<NULL>") file << lout << std::endl;
{ file << "------" << std::endl;
file << lout << std::endl; file << lerr << std::endl;
file << "------" << std::endl; ROS_INFO(lout.c_str());
file << lerr << std::endl; ROS_ERROR(lerr.c_str());
ROS_INFO(lout.c_str()); } else {
ROS_ERROR(lerr.c_str()); file << lres << std::endl;
} ROS_INFO(lres.c_str());
else
{
file << lres << std::endl;
ROS_INFO(lres.c_str());
}
} }
}
} }
void SoTTalosController:: void SoTTalosController::startupPython() {
startupPython()
{
std::ofstream aof(LOG_PYTHON.c_str()); std::ofstream aof(LOG_PYTHON.c_str());
runPython (aof, "import sys, os", *interpreter_); runPython(aof, "import sys, os", *interpreter_);
runPython (aof, "pythonpath = os.environ.get('PYTHONPATH', '')", *interpreter_); runPython(aof, "pythonpath = os.environ.get('PYTHONPATH', '')", *interpreter_);
runPython (aof, "path = []", *interpreter_); runPython(aof, "path = []", *interpreter_);
runPython (aof, runPython(aof,
"for p in pythonpath.split(':'):\n" "for p in pythonpath.split(':'):\n"
" if p not in sys.path:\n" " if p not in sys.path:\n"
" path.append(p)", *interpreter_); " path.append(p)",
runPython (aof, "path.extend(sys.path)", *interpreter_); *interpreter_);
runPython (aof, "sys.path = path", *interpreter_); runPython(aof, "path.extend(sys.path)", *interpreter_);
runPython(aof, "sys.path = path", *interpreter_);
// Calling again rosInit here to start the spinner. It will // Calling again rosInit here to start the spinner. It will
// deal with topics and services callbacks in a separate, non // deal with topics and services callbacks in a separate, non
// real-time thread. See roscpp documentation for more // real-time thread. See roscpp documentation for more
// information. // information.
dynamicgraph::rosInit (true); dynamicgraph::rosInit(true);
aof.close(); aof.close();
} }
...@@ -22,13 +22,10 @@ ...@@ -22,13 +22,10 @@
#include "sot-talos-device.hh" #include "sot-talos-device.hh"
#include <dynamic_graph_bridge/ros_interpreter.hh> #include <dynamic_graph_bridge/ros_interpreter.hh>
namespace dgsot=dynamicgraph::sot; namespace dgsot = dynamicgraph::sot;
class SoTTalosController: public class SoTTalosController : public dgsot::AbstractSotExternalInterface {
dgsot::AbstractSotExternalInterface
{
public: public:
static const std::string LOG_PYTHON; static const std::string LOG_PYTHON;
SoTTalosController(); SoTTalosController();
...@@ -36,9 +33,9 @@ class SoTTalosController: public ...@@ -36,9 +33,9 @@ class SoTTalosController: public
SoTTalosController(std::string robotName); SoTTalosController(std::string robotName);
virtual ~SoTTalosController(); virtual ~SoTTalosController();
void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn); void setupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
void nominalSetSensors(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 cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
...@@ -59,16 +56,13 @@ class SoTTalosController: public ...@@ -59,16 +56,13 @@ class SoTTalosController: public
void updateRobotState(std::vector<double> &anglesIn); void updateRobotState(std::vector<double> &anglesIn);
/// Run a python command /// Run a python command
void runPython(std::ostream& file, void runPython(std::ostream &file, const std::string &command, dynamicgraph::Interpreter &interpreter);
const std::string& command,
dynamicgraph::Interpreter& interpreter);
virtual void startupPython(); virtual void startupPython();
void init(); void init();
SoTTalosDevice* device_; SoTTalosDevice *device_;
}; };
#endif /* _SOT_TalosController_H_ */ #endif /* _SOT_TalosController_H_ */
...@@ -24,19 +24,26 @@ ...@@ -24,19 +24,26 @@
#define DBGFILE "/tmp/sot-talos-device.txt" #define DBGFILE "/tmp/sot-talos-device.txt"
#if 0 #if 0
#define RESETDEBUG5() { std::ofstream DebugFile; \ #define RESETDEBUG5() \
DebugFile.open(DBGFILE,std::ofstream::out); \ { \
DebugFile.close();} std::ofstream DebugFile; \
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \ DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.open(DBGFILE,std::ofstream::app); \ DebugFile.close(); \
DebugFile << __FILE__ << ":" \ }
<< __FUNCTION__ << "(#" \ #define ODEBUG5FULL(x) \
<< __LINE__ << "):" << x << std::endl; \ { \
DebugFile.close();} std::ofstream DebugFile; \
#define ODEBUG5(x) { std::ofstream DebugFile; \ DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile.open(DBGFILE,std::ofstream::app); \ DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << "):" << x << std::endl; \
DebugFile << x << std::endl; \ DebugFile.close(); \
DebugFile.close();} }
#define ODEBUG5(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close(); \
}
#else #else
// Void the macro // Void the macro
...@@ -55,41 +62,40 @@ using namespace std; ...@@ -55,41 +62,40 @@ using namespace std;
const double SoTTalosDevice::TIMESTEP_DEFAULT = 0.001; const double SoTTalosDevice::TIMESTEP_DEFAULT = 0.001;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice,"DeviceTalos"); DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice, "DeviceTalos");
SoTTalosDevice::SoTTalosDevice(std::string RobotName): SoTTalosDevice::SoTTalosDevice(std::string RobotName)
dgsot::Device(RobotName), : dgsot::Device(RobotName),
previousState_ (), previousState_(),
baseff_ (), baseff_(),
accelerometerSOUT_("Device(" + RobotName + ")::output(vector)::accelerometer"), accelerometerSOUT_("Device(" + RobotName + ")::output(vector)::accelerometer"),
gyrometerSOUT_ ("Device(" + RobotName + ")::output(vector)::gyrometer"), gyrometerSOUT_("Device(" + RobotName + ")::output(vector)::gyrometer"),
currentsSOUT_ ("Device(" + RobotName + ")::output(vector)::currents"), currentsSOUT_("Device(" + RobotName + ")::output(vector)::currents"),
joint_anglesSOUT_ ("Device(" + RobotName + ")::output(vector)::joint_angles"), joint_anglesSOUT_("Device(" + RobotName + ")::output(vector)::joint_angles"),
motor_anglesSOUT_ ("Device(" + RobotName + ")::output(vector)::motor_angles"), motor_anglesSOUT_("Device(" + RobotName + ")::output(vector)::motor_angles"),
p_gainsSOUT_ ("Device(" + RobotName + ")::output(vector)::p_gains"), p_gainsSOUT_("Device(" + RobotName + ")::output(vector)::p_gains"),
d_gainsSOUT_ ("Device(" + RobotName + ")::output(vector)::d_gains"), d_gainsSOUT_("Device(" + RobotName + ")::output(vector)::d_gains"),
dgforces_ (6), dgforces_(6),
accelerometer_ (3), accelerometer_(3),
gyrometer_ (3) gyrometer_(3) {
{
RESETDEBUG5(); RESETDEBUG5();
timestep_=TIMESTEP_DEFAULT; timestep_ = TIMESTEP_DEFAULT;
sotDEBUGIN(25) ; sotDEBUGIN(25);
for( int i=0;i<4;++i ) { withForceSignals[i] = true; } for (int i = 0; i < 4; ++i) {
signalRegistration (accelerometerSOUT_ << gyrometerSOUT_ withForceSignals[i] = true;
<< currentsSOUT_ }
<< joint_anglesSOUT_ signalRegistration(accelerometerSOUT_ << gyrometerSOUT_ << currentsSOUT_ << joint_anglesSOUT_ << motor_anglesSOUT_
<< motor_anglesSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
<< p_gainsSOUT_ << d_gainsSOUT_); dg::Vector data(3);
dg::Vector data (3); data.setZero (); data.setZero();
accelerometerSOUT_.setConstant (data); accelerometerSOUT_.setConstant(data);
gyrometerSOUT_.setConstant (data); gyrometerSOUT_.setConstant(data);
baseff_.resize(7); baseff_.resize(7);
dg::Vector dataForces(6); dataForces.setZero (); dg::Vector dataForces(6);
for(int i=0;i<4;i++) dataForces.setZero();
forcesSOUT[i]->setConstant(dataForces); for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);
using namespace dynamicgraph::command; using namespace dynamicgraph::command;
std::string docstring; std::string docstring;
/* Command increment. */ /* Command increment. */
...@@ -99,97 +105,77 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName): ...@@ -99,97 +105,77 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName):
"\n" "\n"
" take one floating point number as input\n" " take one floating point number as input\n"
"\n"; "\n";
addCommand("increment", addCommand("increment", makeCommandVoid1((Device&)*this, &Device::increment, docstring));
makeCommandVoid1((Device&)*this,
&Device::increment, docstring));
sotDEBUGOUT(25); sotDEBUGOUT(25);
} }
SoTTalosDevice::~SoTTalosDevice() SoTTalosDevice::~SoTTalosDevice() {}
{ }
void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t) void SoTTalosDevice::setSensorsForce(map<string, dgsot::SensorValues>& SensorsIn, int t) {
{ int map_sot_2_urdf[4] = {2, 0, 3, 1};
int map_sot_2_urdf[4] = { 2, 0, 3, 1};
sotDEBUGIN(15);