Commit bc826e50 authored by Guilhem Saurel's avatar Guilhem Saurel

Format

parent 43892dd4
Pipeline #8584 failed with stage
in 1 minute and 6 seconds
/* Copyright 2018, CNRS
* Author: O. Stasse
*
*
BSD 2-Clause License
Copyright (c) 2017, Stack Of Tasks development team
......@@ -33,16 +33,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
\mainpage
\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.
A derived class is used for the first prototype of this line of humanoid robot, Pyrene.
This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the
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.
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.
It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or with the Gazebo simulator.
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. 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 python interpreter to control the SoT and finally handle
the control states: initialization, nominal usage, stopping or cancellation.
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 python interpreter to control the SoT and finally handle the
control states: initialization, nominal usage, stopping or cancellation.
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> p_gains </li>
<li> d_gains</li>
\section sot-talos-controller
\section sot-talos-controller
*/
......@@ -6,7 +6,7 @@
* LAAS, CNRS
*
* This file is part of TALOSController.
* TALOSController is a free software,
* TALOSController is a free software,
*
*/
......@@ -17,43 +17,26 @@
#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():
SoTTalosController(ROBOTNAME)
{
SoTPyreneController::SoTPyreneController() : SoTTalosController(ROBOTNAME) {
startupPython();
interpreter_->startRosService ();
interpreter_->startRosService();
}
void SoTPyreneController::startupPython()
{
void SoTPyreneController::startupPython() {
SoTTalosController::startupPython();
std::ofstream aof(LOG_PYTHON_PYRENE.c_str());
runPython
(aof,
"from dynamic_graph.sot.pyrene.prologue import makeRobot",
*interpreter_);
runPython
(aof,
"robot = makeRobot ()",
*interpreter_);
runPython(aof, "from dynamic_graph.sot.pyrene.prologue import makeRobot", *interpreter_);
runPython(aof, "robot = makeRobot ()", *interpreter_);
aof.close();
}
extern "C"
{
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
return new SoTPyreneController;
}
extern "C" {
dgsot::AbstractSotExternalInterface *createSotExternalInterface() { return new SoTPyreneController; }
}
extern "C"
{
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
{
delete p;
}
extern "C" {
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p) { delete p; }
}
......@@ -6,7 +6,7 @@
* LAAS, CNRS
*
* This file is part of TALOSController.
* TALOSController is a free software,
* TALOSController is a free software,
*
*
*/
......@@ -15,22 +15,17 @@
#define _SOT_PYRENE_Controller_H_
#include "sot-talos-controller.hh"
namespace dgsot=dynamicgraph::sot;
namespace dgsot = dynamicgraph::sot;
class SoTPyreneController: public SoTTalosController
{
class SoTPyreneController : public SoTTalosController {
public:
static const std::string LOG_PYTHON_PYRENE;
SoTPyreneController();
virtual ~SoTPyreneController() {};
virtual ~SoTPyreneController(){};
protected:
virtual void startupPython();
};
#endif /* _SOTTalosController_H_ */
......@@ -23,143 +23,96 @@
#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;
SoTTalosController::SoTTalosController(std::string RobotName):
device_(new SoTTalosDevice(RobotName))
{
init();
}
SoTTalosController::SoTTalosController(std::string RobotName) : device_(new SoTTalosDevice(RobotName)) { init(); }
SoTTalosController::SoTTalosController(const char robotName[]):
device_(new SoTTalosDevice(robotName))
{
init();
}
SoTTalosController::SoTTalosController(const char robotName[]) : device_(new SoTTalosDevice(robotName)) { init(); }
void SoTTalosController::init()
{
void SoTTalosController::init() {
std::cout << "Going through SoTTalosController." << std::endl;
// rosInit is called here only to initialize ros.
// No spinner is initialized.
ros::NodeHandle &nh = dynamicgraph::rosInit(false,false);
interpreter_ = boost::shared_ptr<dynamicgraph::Interpreter>(
new dynamicgraph::Interpreter (nh));
ros::NodeHandle &nh = dynamicgraph::rosInit(false, false);
interpreter_ = boost::shared_ptr<dynamicgraph::Interpreter>(new dynamicgraph::Interpreter(nh));
sotDEBUG(25) << __FILE__ << ":"
<< __FUNCTION__ <<"(#"
<< __LINE__ << " )" << std::endl;
sotDEBUG(25) << __FILE__ << ":" << __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);
}
SoTTalosController::~SoTTalosController()
{
SoTTalosController::~SoTTalosController() {
// device_ will be deleted by dynamicgraph::PoolStorage::destroy()
}
void SoTTalosController::
setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
void SoTTalosController::setupSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
device_->setupSetSensors(SensorsIn);
}
void SoTTalosController::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
void SoTTalosController::nominalSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
device_->nominalSetSensors(SensorsIn);
}
void SoTTalosController::
cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
void SoTTalosController::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
device_->cleanupSetSensors(SensorsIn);
}
void SoTTalosController::
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 SoTTalosController::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 SoTTalosController::
setNoIntegration(void)
{
device_->setNoIntegration();
}
void SoTTalosController::setNoIntegration(void) { device_->setNoIntegration(); }
void SoTTalosController::
setSecondOrderIntegration(void)
{
device_->setSecondOrderIntegration();
}
void SoTTalosController::setSecondOrderIntegration(void) { device_->setSecondOrderIntegration(); }
void SoTTalosController::
runPython(std::ostream& file,
const std::string& command,
dynamicgraph::Interpreter& interpreter)
{
void SoTTalosController::runPython(std::ostream &file, const std::string &command,
dynamicgraph::Interpreter &interpreter) {
file << ">>> " << command << std::endl;
std::string lres(""),lout(""),lerr("");
interpreter.runCommand(command,lres,lout,lerr);
if (lres != "None")
{
if (lres=="<NULL>")
{
file << lout << std::endl;
file << "------" << std::endl;
file << lerr << std::endl;
ROS_INFO(lout.c_str());
ROS_ERROR(lerr.c_str());
}
else
{
file << lres << std::endl;
ROS_INFO(lres.c_str());
}
std::string lres(""), lout(""), lerr("");
interpreter.runCommand(command, lres, lout, lerr);
if (lres != "None") {
if (lres == "<NULL>") {
file << lout << std::endl;
file << "------" << std::endl;
file << lerr << std::endl;
ROS_INFO(lout.c_str());
ROS_ERROR(lerr.c_str());
} else {
file << lres << std::endl;
ROS_INFO(lres.c_str());
}
}
}
void SoTTalosController::
startupPython()
{
void SoTTalosController::startupPython() {
std::ofstream aof(LOG_PYTHON.c_str());
runPython (aof, "import sys, os", *interpreter_);
runPython (aof, "pythonpath = os.environ.get('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_);
runPython(aof, "import sys, os", *interpreter_);
runPython(aof, "pythonpath = os.environ.get('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);
dynamicgraph::rosInit(true);
aof.close();
}
......@@ -22,13 +22,10 @@
#include "sot-talos-device.hh"
#include <dynamic_graph_bridge/ros_interpreter.hh>
namespace dgsot=dynamicgraph::sot;
namespace dgsot = dynamicgraph::sot;
class SoTTalosController: public
dgsot::AbstractSotExternalInterface
{
class SoTTalosController : public dgsot::AbstractSotExternalInterface {
public:
static const std::string LOG_PYTHON;
SoTTalosController();
......@@ -36,9 +33,9 @@ class SoTTalosController: public
SoTTalosController(std::string robotName);
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);
......@@ -59,16 +56,13 @@ class SoTTalosController: public
void updateRobotState(std::vector<double> &anglesIn);
/// Run a python command
void runPython(std::ostream& file,
const std::string& command,
dynamicgraph::Interpreter& interpreter);
void runPython(std::ostream &file, const std::string &command, dynamicgraph::Interpreter &interpreter);
virtual void startupPython();
void init();
SoTTalosDevice* device_;
SoTTalosDevice *device_;
};
#endif /* _SOT_TalosController_H_ */
......@@ -24,19 +24,26 @@
#define DBGFILE "/tmp/sot-talos-device.txt"
#if 0
#define RESETDEBUG5() { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::out); \
DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define RESETDEBUG5() \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.close(); \
}
#define ODEBUG5FULL(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << "):" << x << std::endl; \
DebugFile.close(); \
}
#define ODEBUG5(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close(); \
}
#else
// Void the macro
......@@ -55,41 +62,40 @@ using namespace std;
const double SoTTalosDevice::TIMESTEP_DEFAULT = 0.001;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice,"DeviceTalos");
SoTTalosDevice::SoTTalosDevice(std::string RobotName):
dgsot::Device(RobotName),
previousState_ (),
baseff_ (),
accelerometerSOUT_("Device(" + RobotName + ")::output(vector)::accelerometer"),
gyrometerSOUT_ ("Device(" + RobotName + ")::output(vector)::gyrometer"),
currentsSOUT_ ("Device(" + RobotName + ")::output(vector)::currents"),
joint_anglesSOUT_ ("Device(" + RobotName + ")::output(vector)::joint_angles"),
motor_anglesSOUT_ ("Device(" + RobotName + ")::output(vector)::motor_angles"),
p_gainsSOUT_ ("Device(" + RobotName + ")::output(vector)::p_gains"),
d_gainsSOUT_ ("Device(" + RobotName + ")::output(vector)::d_gains"),
dgforces_ (6),
accelerometer_ (3),
gyrometer_ (3)
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice, "DeviceTalos");
SoTTalosDevice::SoTTalosDevice(std::string RobotName)
: dgsot::Device(RobotName),
previousState_(),
baseff_(),
accelerometerSOUT_("Device(" + RobotName + ")::output(vector)::accelerometer"),
gyrometerSOUT_("Device(" + RobotName + ")::output(vector)::gyrometer"),
currentsSOUT_("Device(" + RobotName + ")::output(vector)::currents"),
joint_anglesSOUT_("Device(" + RobotName + ")::output(vector)::joint_angles"),
motor_anglesSOUT_("Device(" + RobotName + ")::output(vector)::motor_angles"),
p_gainsSOUT_("Device(" + RobotName + ")::output(vector)::p_gains"),
d_gainsSOUT_("Device(" + RobotName + ")::output(vector)::d_gains"),
dgforces_(6),
accelerometer_(3),
gyrometer_(3) {
RESETDEBUG5();
timestep_=TIMESTEP_DEFAULT;
sotDEBUGIN(25) ;
for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
signalRegistration (accelerometerSOUT_ << gyrometerSOUT_
<< currentsSOUT_
<< joint_anglesSOUT_
<< motor_anglesSOUT_
<< p_gainsSOUT_ << d_gainsSOUT_);
dg::Vector data (3); data.setZero ();
accelerometerSOUT_.setConstant (data);
gyrometerSOUT_.setConstant (data);
timestep_ = TIMESTEP_DEFAULT;
sotDEBUGIN(25);
for (int i = 0; i < 4; ++i) {
withForceSignals[i] = true;
}
signalRegistration(accelerometerSOUT_ << gyrometerSOUT_ << currentsSOUT_ << joint_anglesSOUT_ << motor_anglesSOUT_
<< p_gainsSOUT_ << d_gainsSOUT_);
dg::Vector data(3);
data.setZero();
accelerometerSOUT_.setConstant(data);
gyrometerSOUT_.setConstant(data);
baseff_.resize(7);
dg::Vector dataForces(6); dataForces.setZero ();
for(int i=0;i<4;i++)
forcesSOUT[i]->setConstant(dataForces);
dg::Vector dataForces(6);
dataForces.setZero();
for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);
using namespace dynamicgraph::command;
std::string docstring;
/* Command increment. */
......@@ -99,97 +105,77 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName):
"\n"
" take one floating point number as input\n"
"\n";
addCommand("increment",
makeCommandVoid1((Device&)*this,
&Device::increment, docstring));
addCommand("increment", makeCommandVoid1((Device&)*this, &Device::increment, docstring));
sotDEBUGOUT(25);
}
SoTTalosDevice::~SoTTalosDevice()
{ }
SoTTalosDevice::~SoTTalosDevice() {}
void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t)
{
int map_sot_2_urdf[4] = { 2, 0, 3, 1};
void SoTTalosDevice::setSensorsForce(map<string, dgsot::SensorValues>& SensorsIn, int t) {
int map_sot_2_urdf[4] = {2, 0, 3, 1};
sotDEBUGIN(15);
map<string,dgsot::SensorValues>::iterator it;
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("forces");
if (it!=SensorsIn.end())
{
if (it != SensorsIn.end()) {
// Implements force recollection.
const vector<double>& forcesIn = it->second.getValues();
for(int i=0;i<4;++i)
{
for (int i = 0; i < 4; ++i) {
sotDEBUG(15) << "Force sensor " << i << std::endl;
int idx_sensor = map_sot_2_urdf[i];
for(int j=0;j<6;++j)
{
dgforces_(j) = forcesIn[idx_sensor*6+j];
sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
}
for (int j = 0; j < 6; ++j) {
dgforces_(j) = forcesIn[idx_sensor * 6 + j];
sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
}
forcesSOUT[i]->setConstant(dgforces_);
forcesSOUT[i]->setTime (t);
forcesSOUT[i]->setTime(t);
}
}
sotDEBUGIN(15);
}
void SoTTalosDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn, int t)
{
map<string,dgsot::SensorValues>::iterator it;
//TODO: Confirm if this can be made quaternion
void SoTTalosDevice::setSensorsIMU(map<string, dgsot::SensorValues>& SensorsIn, int t) {
map<string, dgsot::SensorValues>::iterator it;
// TODO: Confirm if this can be made quaternion
it = SensorsIn.find("attitude");
if (it!=SensorsIn.end())
{
const vector<double>& attitude = it->second.getValues ();
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);
for (unsigned int j = 0; j < 3; ++j) pose(i, j) = attitude[i * 3 + j];
attitudeSOUT.setConstant(pose);
attitudeSOUT.setTime(t);
}
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_);
accelerometerSOUT_.setTime (t);
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_);
accelerometerSOUT_.setTime(t);
}
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_);
gyrometerSOUT_.setTime (t);
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_);
gyrometerSOUT_.setTime(t);
}
}
void SoTTalosDevice::setSensorsEncoders(map<string,dgsot::SensorValues> &SensorsIn, int t)
{
map<string,dgsot::SensorValues>::iterator it;
void SoTTalosDevice::setSensorsEncoders(map<string, dgsot::SensorValues>& SensorsIn, int t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("motor-angles");
if (it!=SensorsIn.end())
{
if (it != SensorsIn.end()) {
const vector<double>& anglesIn = it->second.getValues();
dgRobotState_.resize (anglesIn.size () + 6);
motor_angles_.resize(anglesIn.size ());
for (unsigned i = 0; i < 6; ++i)
dgRobotState_ (i) = 0.;
for (unsigned i = 0; i < anglesIn.size(); ++i)
{
dgRobotState_ (i + 6) = anglesIn[i];
motor_angles_(i)= anglesIn[i];
}
dgRobotState_.resize(anglesIn.size() + 6);
motor_angles_.resize(anglesIn.size());
for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
for (unsigned i = 0; i < anglesIn.size(); ++i) {
dgRobotState_(i + 6) = anglesIn[i];