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_ */
This diff is collapsed.
......@@ -6,7 +6,7 @@
* LAAS, CNRS
*
* This file is part of TalosController.
* TalosController is not a free software,
* TalosController is not a free software,
* it contains information related to Talos which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
......@@ -18,7 +18,6 @@
#ifndef _SOT_TalosDevice_H_
#define _SOT_TalosDevice_H_
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
......@@ -27,76 +26,69 @@
#include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/matrix-geometry.hh>
namespace dgsot=dynamicgraph::sot;
namespace dg=dynamicgraph;
namespace dgsot = dynamicgraph::sot;
namespace dg = dynamicgraph;
class SoTTalosDevice: public
dgsot::Device
{
class SoTTalosDevice : public dgsot::Device {
public:
static const std::string CLASS_NAME;
static const double TIMESTEP_DEFAULT;
virtual const std::string& getClassName () const
{
return CLASS_NAME;
}
virtual const std::string &getClassName() const { return CLASS_NAME; }
SoTTalosDevice(std::string RobotName);
virtual ~SoTTalosDevice();
void setSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void setSensors(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 getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
void timeStep(double ts) { timestep_ =ts;}
protected:
void timeStep(double ts) { timestep_ = ts; }
protected:
/// \brief Previous robot configuration.
dg::Vector previousState_;
/// Intermediate variables to avoid allocation during control
std::vector<double> baseff_;
/// Accelerations measured by accelerometers
dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_;
dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers
dynamicgraph::Signal <dg::Vector, int> gyrometerSOUT_;
dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
/// motor currents
dynamicgraph::Signal <dg::Vector, int> currentsSOUT_;
dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
/// joint angles
dynamicgraph::Signal <dg::Vector, int> joint_anglesSOUT_;
dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
/// motor angles
dynamicgraph::Signal <dg::Vector, int> motor_anglesSOUT_;
dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
/// proportional and derivative position-control gains
dynamicgraph::Signal <dg::Vector, int> p_gainsSOUT_;
dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
dynamicgraph::Signal <dg::Vector, int> d_gainsSOUT_;
dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
/// Protected methods for internal variables filling
void setSensorsForce(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsIMU(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsEncoders(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsVelocities(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsTorquesCurrents(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsGains(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
void setSensorsVelocities(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
void setSensorsTorquesCurrents(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
/// Intermediate variables to avoid allocation during control
dg::Vector dgforces_;
dg::Vector dgRobotState_; // motor-angles
dg::Vector joint_angles_; // joint-angles
dg::Vector motor_angles_; // motor-angles
dg::Vector dgRobotVelocity_; // motor velocities
dg::Vector velocities_; // motor velocities
dg::Vector dgRobotState_; // motor-angles
dg::Vector joint_angles_; // joint-angles
dg::Vector motor_angles_; // motor-angles
dg::Vector dgRobotVelocity_; // motor velocities
dg::Vector velocities_; // motor velocities
dgsot::MatrixRotation pose;
dg::Vector accelerometer_;
dg::Vector gyrometer_;
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment