Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • ostasse/sot-core
  • gsaurel/sot-core
  • stack-of-tasks/sot-core
3 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 2240 additions and 0 deletions
include/sot-core/sequencer.h → include/sot/core/sequencer.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: Sequencer.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_SOTSEQUENCER_H__
#define __SOT_SOTSEQUENCER_H__
......@@ -27,33 +15,34 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/task-abstract.h>
#include <dynamic-graph/entity.h>
#include <sot/core/task-abstract.hh>
/* STD */
#include <string>
#include <map>
#include <list>
#include <map>
#include <string>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sequencer_EXPORTS)
# define SOTSEQUENCER_EXPORT __declspec(dllexport)
# else
# define SOTSEQUENCER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(sequencer_EXPORTS)
#define SOTSEQUENCER_EXPORT __declspec(dllexport)
#else
# define SOTSEQUENCER_EXPORT
#define SOTSEQUENCER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSEQUENCER_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
......@@ -62,79 +51,61 @@ namespace sot {
class Sot;
class SOTSEQUENCER_EXPORT Sequencer
:public dynamicgraph::Entity
{
public:
static const std::string CLASS_NAME;
class SOTSEQUENCER_EXPORT Sequencer : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
class sotEventAbstract
{
public:
enum sotEventType
{
EVENT_ADD
,EVENT_RM
,EVENT_CMD
};
protected:
std::string name;
void setName( const std::string& name_ ) { name = name_; }
int eventType;
public:
sotEventAbstract( const std::string & name ) : name(name) {};
virtual ~sotEventAbstract( void ) {}
virtual const std::string& getName() const { return name; }
int getEventType( ) const { return eventType; }
virtual void operator() ( Sot* sotPtr ) = 0;
virtual void display( std::ostream& os ) const { os << name; }
};
class sotEventAbstract {
public:
enum sotEventType { EVENT_ADD, EVENT_RM, EVENT_CMD };
protected:
std::string name;
void setName(const std::string &name_) { name = name_; }
int eventType;
public:
sotEventAbstract(const std::string &name) : name(name){};
virtual ~sotEventAbstract(void) {}
virtual const std::string &getName() const { return name; }
int getEventType() const { return eventType; }
virtual void operator()(Sot *sotPtr) = 0;
virtual void display(std::ostream &os) const { os << name; }
};
protected:
Sot* sotPtr;
typedef std::list< sotEventAbstract* > TaskList;
typedef std::map< unsigned int,TaskList > TaskMap;
Sot *sotPtr;
typedef std::list<sotEventAbstract *> TaskList;
typedef std::map<unsigned int, TaskList> TaskMap;
TaskMap taskMap;
/* All the events are counting wrt to this t0. If t0 is -1, it
* is set to the first time of trig. */
int timeInit;
int timeInit;
bool playMode;
std::ostream* outputStreamPtr;
std::ostream *outputStreamPtr;
bool noOutput; /*! if true, display nothing standard output on except errors*/
public: /* --- CONSTRUCTION --- */
Sequencer( const std::string& name );
virtual ~Sequencer( void );
Sequencer(const std::string &name);
virtual ~Sequencer(void);
public: /* --- TASK MANIP --- */
void setSotRef( Sot* sot ) { sotPtr = sot; }
void addTask( sotEventAbstract* task,const unsigned int time );
void rmTask( int eventType, const std::string& name,const unsigned int time );
void clearAll( );
void setSotRef(Sot *sot) { sotPtr = sot; }
void addTask(sotEventAbstract *task, const unsigned int time);
void rmTask(int eventType, const std::string &name, const unsigned int time);
void clearAll();
public: /* --- SIGNAL --- */
dynamicgraph::SignalTimeDependent<int,int> triggerSOUT;
dynamicgraph::SignalTimeDependent<int, int> triggerSOUT;
public: /* --- FUNCTIONS --- */
int& trigger( int& dummy,const int& time );
public: /* --- PARAMS --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
int &trigger(int &dummy, const int &time);
public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
};
} // namespace sot
} // namespace dynamicgraph
} // namespace dynamicgraph
#endif // #ifndef __SOT_SOTSEQUENCER_H__
#endif // #ifndef __SOT_SOTSEQUENCER_H__
include/sot/core/smooth-reach.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_SMOOTHREACH_H_H
#define __SOT_SMOOTHREACH_H_H
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(com_freezer_EXPORTS)
#define SOTSMOOTHREACH_EXPORT __declspec(dllexport)
#else
#define SOTSMOOTHREACH_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSMOOTHREACH_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTSMOOTHREACH_EXPORT SmoothReach : public dynamicgraph::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName() const { return CLASS_NAME; }
private:
dynamicgraph::Vector start, goal;
int startTime, lengthTime;
bool isStarted, isParam;
int smoothMode;
double smoothParam;
double smoothFunction(double x);
public: /* --- CONSTRUCTION --- */
SmoothReach(const std::string &name);
virtual ~SmoothReach(void){};
public: /* --- SIGNAL --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> startSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> goalSOUT;
public: /* --- FUNCTION --- */
dynamicgraph::Vector &goalSOUT_function(dynamicgraph::Vector &goal,
const int &time);
void set(const dynamicgraph::Vector &goal, const int &length);
const dynamicgraph::Vector &getGoal(void);
const int &getLength(void);
const int &getStart(void);
void setSmoothing(const int &mode, const double &param);
public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
void initCommands(void);
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SMOOTHREACH_H_H */
include/sot/core/sot-loader.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2016,
* Olivier Stasse,
*
* CNRS
*
*/
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#ifndef _SOT_LOADER_HH_
#define _SOT_LOADER_HH_
// STL includes
#include <map>
// Dynamic Graph embeded python interpreter.
#include <dynamic-graph/python/interpreter.hh>
// Sot Framework includes
#include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/debug.hh>
#include <sot/core/device.hh>
namespace dynamicgraph {
namespace sot {
/**
* @brief This class is loading the control part of the Stack-Of-Tasks.
* - 1/ It loads dynamically the graph interface.
* - 2/ It loads the python interpreter.
* - 3/ It loads the Device entity C++ pointer inside the python interpreter.
* - 4/ It provides the user interface to the graph:
* - 4.1/ starts and stop the graph executtion.
* - 4.2/ run a python command/script inside the embeded python interpreter.
* - 4.3/ execute one iteration of the graph.
*
* In order to Use this class you need to provide a dynamic library containing
* an implementation of the AbstractSotExternalInterface class.
*
* Then you can either inherite from this class an initialize and use the
* sensors_in_ and control_values_ objects.
* Or you can create you own outside of this class.
* And then use the oneIteration to execute the graph.
*/
class SotLoader {
protected:
/// \brief Check if the dynamic graph is running or not.
bool dynamic_graph_stopped_;
/// \brief The interface between the device and the robot driver.
AbstractSotExternalInterface *sot_external_interface_;
/// \brief Name of the dynamic library containing the
/// dgs::AbstractSotExternalInterface object.
std::string sot_dynamic_library_filename_;
/// \brief Handle on the SoT library.
void *sot_dynamic_library_;
/// \brief Embeded python interpreter.
python::Interpreter embeded_python_interpreter_;
/// \brief Map of sensor readings
std::map<std::string, SensorValues> sensors_in_;
/// \brief Map of control values
std::map<std::string, ControlValues> control_values_;
/// \brief Device entity created and loaded, so we deregister it as the Pool
/// is not responsible for it's life time.
std::string device_name_;
public:
/// \brief Default constructor.
SotLoader();
/// \brief Default destructor.
~SotLoader();
/// \brief Read user input to extract the path of the SoT dynamic library.
int parseOptions(int argc, char *argv[]);
/// \brief Prepare the SoT framework.
bool initialization();
/// \brief Unload the library which handles the robot device.
void cleanUp();
/// \brief Get Status of dg.
inline bool isDynamicGraphStopped() { return dynamic_graph_stopped_; }
/// \brief Get Status of dg.
inline void startDG() { dynamic_graph_stopped_ = false; }
/// \brief Get Status of dg.
inline void stopDG() { dynamic_graph_stopped_ = true; }
/// \brief Specify the name of the dynamic library.
inline void setDynamicLibraryName(std::string &afilename) {
sot_dynamic_library_filename_ = afilename;
}
/// \brief Run a python command inside the embeded python interpreter.
void runPythonCommand(const std::string &command, std::string &result,
std::string &out, std::string &err);
/// \brief Run a python script inside the embeded python interpreter.
inline void runPythonFile(std::string ifilename, std::string &err) {
embeded_python_interpreter_.runPythonFile(ifilename, err);
}
/// \brief Run a python script inside the embeded python interpreter.
inline void runPythonFile(std::string ifilename) {
embeded_python_interpreter_.runPythonFile(ifilename);
}
/// \brief Compute one iteration of control.
/// Basically executes fillSensors, the SoT and the readControl.
void oneIteration(std::map<std::string, SensorValues> &sensors_in,
std::map<std::string, ControlValues> &control_values);
/// \brief Load the Device entity in the python global scope.
void loadDeviceInPython(const std::string &device_name);
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* _SOT_LOADER_HH_ */
include/sot-core/sot-qr.h → include/sot/core/sot.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: sotSotQr.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_SOTQR_HH
#define __SOT_SOTQR_HH
#ifndef __SOT_SOT_HH
#define __SOT_SOT_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* Classes standards. */
#include <list> /* Classe std::list */
#include <list> /* Classe std::list */
/* SOT */
#include <sot-core/task-abstract.h>
#include <sot-core/flags.h>
#include <dynamic-graph/entity.h>
#include <sot-core/constraint.h>
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sot_qr_EXPORTS)
# define SOTSOTQR_EXPORT __declspec(dllexport)
# else
# define SOTSOTQR_EXPORT __declspec(dllimport)
# endif
#ifndef SOTSOT_CORE_EXPORT
#if defined(WIN32)
#if defined(sot_EXPORTS)
#define SOTSOT_CORE_EXPORT __declspec(dllexport)
#else
# define SOTSOTQR_EXPORT
#define SOTSOT_CORE_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSOT_CORE_EXPORT
#endif
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
/*! @ingroup stackoftasks
\brief This class implements the Stack of Task.
It allows to deal with the priority of the controllers
through the shell. The controllers can be either constraints
either tasks.
through the shell.
*/
namespace sot {
namespace dg = dynamicgraph;
class SOTSOTQR_EXPORT SotQr
:public dg::Entity
{
class SOTSOT_CORE_EXPORT Sot : public Entity {
public:
/*! \brief Specify the name of the class entity. */
static const std::string CLASS_NAME;
/*! \brief Returns the name of this class. */
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
protected:
public:
/*! \brief Returns the name of this class. */
virtual const std::string &getClassName() const { return CLASS_NAME; }
/*! \brief Defines a type for a list of tasks */
typedef std::list<TaskAbstract*> StackType;
typedef std::list<TaskAbstract *> StackType;
protected:
/*! \brief This field is a list of controllers
managed by the stack of tasks. */
StackType stack;
/*! \brief Defines a type for a list of constraints */
typedef std::list<Constraint*> ConstraintListType;
/*! \brief This field is a list of constraints
managed by the stack of tasks. */
ConstraintListType constraintList;
/*! \brief Defines an interval in the state vector of the robot
which is the free flyer. */
unsigned int ffJointIdFirst,ffJointIdLast;
/*! \brief Defines a default joint. */
static const unsigned int FF_JOINT_ID_DEFAULT = 0;
/* double directionalThreshold; */
/* bool useContiInverse; */
StackType stack;
/*! \brief Store the number of joints to be used in the
command computed by the stack of tasks. */
unsigned int nbJoints;
/*! \brief Store a pointer to compute the gradient */
TaskAbstract* taskGradient;
/*! \brief Option to disable the computation of the SVD for the last task
if this task is a Task with a single FeaturePosture */
bool enablePostureTaskAcceleration;
/*! Projection used to compute the control law. */
ml::Matrix Proj;
/*! \brief Maximum allowed squared norm of control increment.
A task whose control increment is above this value is discarded.
It defaults to \c std::numeric_limits<double>::max().
\warning This is a security feature and is **not** a good way of adding a
proper constraint on the control generated by SoT.
*/
double maxControlIncrementSquaredNorm;
public:
/*! \brief Threshold to compute the dumped pseudo inverse. */
static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
/* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
/* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
/*! \brief Number of joints by default. */
static const unsigned int NB_JOINTS_DEFAULT; // = 48;
static void taskVectorToMlVector(const VectorMultiBound &taskVector,
Vector &err);
public:
/*! \brief Default constructor */
SotQr( const std::string& name );
Sot(const std::string &name);
~Sot(void) { /* TODO!! */
}
/*! \name Methods to handle the stack.
@{
*/
*/
virtual const StackType &tasks() const { return stack; }
/*! \brief Push the task in the stack.
It has a lowest priority than the previous ones.
If this is the first task, then it has the highest
priority. */
void push( TaskAbstract& task );
virtual void push(TaskAbstract &task);
/*! \brief Pop the task from the stack.
This method removes the task with the smallest
priority in the task. The other are projected
in the null-space of their predecessors. */
TaskAbstract& pop( void );
virtual TaskAbstract &pop(void);
/*! \brief This method allows to know if a task exists or not */
bool exist( const TaskAbstract& task );
virtual bool exist(const TaskAbstract &task);
/*! \brief Remove a task regardless to its position in the stack.
It removes also the signals connected to the output signal of this stack.*/
void remove( const TaskAbstract& task );
It removes also the signals connected to the output signal of this
stack.*/
virtual void remove(const TaskAbstract &task);
/*! \brief This method removes the output signals depending on this task. */
void removeDependency( const TaskAbstract& key );
/*! \brief This method removes the output signals depending on
this task. */
virtual void removeDependency(const TaskAbstract &key);
/*! \brief This method makes the task to swap with the task having the
immediate superior priority. */
void up( const TaskAbstract& task );
virtual void up(const TaskAbstract &task);
/*! \brief This method makes the task to swap with the task having the
immediate inferior priority. */
void down( const TaskAbstract& task );
virtual void down(const TaskAbstract &task);
/*! \brief Remove all the tasks from the stack. */
void clear( void );
virtual void clear(void);
/*! @} */
/*! \name Methods to handle the constraints.
@{
*/
/*! \brief Add a constraint to the stack with the current level
of priority. */
void addConstraint( Constraint& constraint );
/*! \brief Remove a constraint from the stack. */
void removeConstraint( const Constraint& constraint );
/*! \brief Remove all the constraints from the stack. */
void clearConstraint( void );
/*! @} */
/*! \brief This method defines the part of the state vector which correspond
to the free flyer of the robot. */
void defineFreeFloatingJoints( const unsigned int& jointIdFirst,
const unsigned int& jointIdLast = -1 );
/*! \brief This method defines the part of the state vector
which correspond to the free flyer of the robot. */
virtual void defineNbDof(const unsigned int &nbDof);
virtual const unsigned int &getNbDof() const { return nbJoints; }
/*! @} */
public: /* --- CONTROL --- */
/*! \name Methods to compute the control law following the
recursive definition of the stack of tasks.
@{
recursive definition of the stack of tasks.
@{
*/
/*! \brief Compute the control law. */
ml::Vector& computeControlLaw( ml::Vector& control,const int& time );
/*! \brief Compute the projector of the constraint. */
ml::Matrix& computeConstraintProjector( ml::Matrix& Proj, const int& time );
virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control,
const int &time);
/*! @} */
/* double getDirectionalThreshold( void ){ return directionalThreshold; } */
/* void setdirectionalThreshold( double th ){ directionalThreshold = th; } */
/* bool getUseContiInverse( void ){ return useContiInverse; } */
/* void setUseContiInverse( bool b ){ useContiInverse = b; } */
public: /* --- DISPLAY --- */
/*! \name Methods to display the stack of tasks.
@{
*/
*/
/*! Display the stack of tasks in text mode as a tree. */
virtual void display( std::ostream& os ) const;
virtual void display(std::ostream &os) const;
/*! Wrap the previous method around an operator. */
SOTSOTQR_EXPORT friend std::ostream& operator<< ( std::ostream& os,const SotQr& sot );
SOTSOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
const Sot &sot);
/*! @} */
public: /* --- SIGNALS --- */
/*! \name Methods to handle signals
@{
*/
*/
/*! \brief Intrinsec velocity of the robot, that is used to initialized
* the recurence of the SOT (e.g. velocity comming from the other
* the recurence of the SOT (e.g. velocity coming from the other
* OpenHRP plugins).
*/
dg::SignalPtr<ml::Vector,int> q0SIN;
/*! \brief This signal allow to change the threshold for the damped pseudo-inverse
on-line */
dg::SignalPtr<double,int> inversionThresholdSIN;
/*! \brief This signal allow to get the result of the Constraint projector. */
dg::SignalTimeDependent<ml::Matrix,int> constraintSOUT;
/*! \brief This signal allow to get the result of the computed control law. */
dg::SignalTimeDependent<ml::Vector,int> controlSOUT;
SignalPtr<dynamicgraph::Vector, int> q0SIN;
/*! \brief A matrix K whose columns are a base of the desired velocity.
* In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free
* parameter to be computed.
* \note K should be an orthonormal matrix.
*/
SignalPtr<dynamicgraph::Matrix, int> proj0SIN;
/*! \brief This signal allow to change the threshold for the
damped pseudo-inverse on-line */
SignalPtr<double, int> inversionThresholdSIN;
/*! \brief Allow to get the result of the computed control law. */
SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
/*! @} */
public: /* --- COMMANDS --- */
/*! \brief This method deals with the command line.
The command given in argument is send to the stack of tasks by the shell.
The command understood by sot are:
<ul>
<li> Tasks
<ul>
<li> push <task> : Push a task in the stack (FILO).
<li> pop : Remove the task push in the stack.
<li> down <task> : Make the task have a higher priority, i.e.
swap with the task immediatly superior in priority.
<li> up <task> : Make the task have a lowest priority, i.e.
swap with the task immediatly inferior in priority.
<li> rm <task> : Remove the task from the stack.
</ul>
<li> Constraints
<ul>
<li> addConstraint <constraint> : Add the constraint in the stack (FILO).
<li> rmConstraint <constraint> : Remove the constraint.
<li> clearConstraint : Remove all the constraints.
<li> printConstraint :
</ol>
</ul>
*/
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
/*! \brief This method write the priority between tasks in the output stream os. */
virtual std::ostream & writeGraph(std::ostream & os) const;
/*! \brief This method write the priority between tasks in the output stream
* os. */
virtual std::ostream &writeGraph(std::ostream &os) const;
};
} // namespace sot
} // namespace dynamicgraph
} // namespace sot
#endif /* #ifndef __SOT_SOTQR_HH */
#endif /* #ifndef __SOT_SOT_HH */
include/sot/core/stop-watch.hh 0 → 100644
View file @ 1c36e63b
/*
Copyright (c) 2010-2013 Tommaso Urli
Tommaso Urli tommaso.urli@uniud.it University of Udine
Permission is hereby granted, free of charge, to any person obtaining
a copy of this software and associated documentation files (the
"Software"), to deal in the Software without restriction, including
without limitation the rights to use, copy, modify, merge, publish,
distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so, subject to
the following conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef __sot_core_stopwatch_H__
#define __sot_core_stopwatch_H__
#include <ctime>
#include <iostream>
#include <map>
#include <sstream>
#ifndef WIN32
/* The classes below are exported */
#pragma GCC visibility push(default)
#endif
// Generic stopwatch exception class
struct StopwatchException {
public:
StopwatchException(std::string error) : error(error) {}
std::string error;
};
enum StopwatchMode {
NONE = 0, // Clock is not initialized
CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
REAL_TIME = 2 // Clock calculates time by asking the operating system how
// much real time passed
};
#define STOP_WATCH_MAX_NAME_LENGTH 80
/**
@brief A class representing a stopwatch.
@code
Stopwatch swatch();
@endcode
The Stopwatch class can be used to measure execution time of code,
algorithms, etc., // TODO: he Stopwatch can be initialized in two
time-taking modes, CPU time and real time:
@code
swatch.set_mode(REAL_TIME);
@endcode
CPU time is the time spent by the processor on a certain piece of code,
while real time is the real amount of time taken by a certain piece of
code to execute (i.e. in general if you are doing hard work such as
image or video editing on a different process the measured time will
probably increase).
How does it work? Basically, one wraps the code to be measured with the
following method calls:
@code
swatch.start("My astounding algorithm");
// Hic est code
swatch.stop("My astounding algorithm");
@endcode
A string representing the code ID is provided so that nested portions of
code can be profiled separately:
@code
swatch.start("My astounding algorithm");
swatch.start("My astounding algorithm - Super smart init");
// Initialization
swatch.stop("My astounding algorithm - Super smart init");
swatch.start("My astounding algorithm - Main loop");
// Loop
swatch.stop("My astounding algorithm - Main loop");
swatch.stop("My astounding algorithm");
@endcode
Note: ID strings can be whatever you like, in the previous example I have
used "My astounding algorithm - *" only to enforce the fact that the
measured code portions are part of My astounding algorithm, but there's no
connection between the three measurements.
If the code for a certain task is scattered through different files or
portions of the same file one can use the start-pause-stop method:
@code
swatch.start("Setup");
// First part of setup
swatch.pause("Setup");
swatch.start("Main logic");
// Main logic
swatch.stop("Main logic");
swatch.start("Setup");
// Cleanup (part of the setup)
swatch.stop("Setup");
@endcode
Finally, to report the results of the measurements just run:
@code
swatch.report("Code ID");
@endcode
Thou can also provide an additional std::ostream& parameter to report() to
redirect the logging on a different output. Also, you can use the
get_total/min/max/average_time() methods to get the individual numeric data,
without all the details of the logging. You can also extend Stopwatch to
implement your own logging syntax.
To report all the measurements:
@code
swatch.report_all();
@endcode
Same as above, you can redirect the output by providing a std::ostream&
parameter.
*/
class Stopwatch {
public:
/** Constructor */
Stopwatch(StopwatchMode _mode = NONE);
/** Destructor */
~Stopwatch();
/** Tells if a performance with a certain ID exists */
bool performance_exists(std::string perf_name);
/** Initialize stopwatch to use a certain time taking mode */
void set_mode(StopwatchMode mode);
/** Start the stopwatch related to a certain piece of code */
void start(std::string perf_name);
/** Stops the stopwatch related to a certain piece of code */
void stop(std::string perf_name);
/** Stops the stopwatch related to a certain piece of code */
void pause(std::string perf_name);
/** Reset a certain performance record */
void reset(std::string perf_name);
/** Resets all the performance records */
void reset_all();
/** Dump the data of a certain performance record */
void report(std::string perf_name, int precision = 2,
std::ostream &output = std::cout);
/** Dump the data of all the performance records */
void report_all(int precision = 2, std::ostream &output = std::cout);
/** Returns total execution time of a certain performance */
long double get_total_time(std::string perf_name);
/** Returns average execution time of a certain performance */
long double get_average_time(std::string perf_name);
/** Returns minimum execution time of a certain performance */
long double get_min_time(std::string perf_name);
/** Returns maximum execution time of a certain performance */
long double get_max_time(std::string perf_name);
/** Return last measurement of a certain performance */
long double get_last_time(std::string perf_name);
/** Return the time since the start of the last measurement of a given
performance. */
long double get_time_so_far(std::string perf_name);
/** Turn off clock, all the Stopwatch::* methods return without doing
anything after this method is called. */
void turn_off();
/** Turn on clock, restore clock operativity after a turn_off(). */
void turn_on();
/** Take time, depends on mode */
long double take_time();
protected:
/** Struct to hold the performance data */
struct PerformanceData {
PerformanceData()
: clock_start(0),
total_time(0),
min_time(0),
max_time(0),
last_time(0),
paused(false),
stops(0) {}
/** Start time */
long double clock_start;
/** Cumulative total time */
long double total_time;
/** Minimum time */
long double min_time;
/** Maximum time */
long double max_time;
/** Last time */
long double last_time;
/** Tells if this performance has been paused, only for internal use */
bool paused;
/** How many cycles have been this stopwatch executed? */
int stops;
};
/** Flag to hold the clock's status */
bool active;
/** Time taking mode */
StopwatchMode mode;
/** Pointer to the dynamic structure which holds the collection of performance
data */
std::map<std::string, PerformanceData> *records_of;
};
Stopwatch &getProfiler();
#ifndef WIN32
#pragma GCC visibility pop
#endif
#endif
include/sot/core/switch.hh 0 → 100644
View file @ 1c36e63b
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
#ifndef __SOT_SWITCH_H__
#define __SOT_SWITCH_H__
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal.h>
#include <sot/core/config.hh>
#include <sot/core/variadic-op.hh>
namespace dynamicgraph {
namespace sot {
/// Switch
template <typename Value, typename Time = int>
class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value, Value, Time> {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
typedef VariadicAbstract<Value, Value, Time> Base;
Switch(const std::string &name)
: Base(name, CLASS_NAME),
selectionSIN(NULL, "Switch(" + name + ")::input(int)::selection"),
boolSelectionSIN(NULL,
"Switch(" + name + ")::input(bool)::boolSelection") {
this->signalRegistration(selectionSIN << boolSelectionSIN);
this->SOUT.setFunction(boost::bind(&Switch::signal, this, _1, _2));
this->SOUT.addDependency(selectionSIN);
this->SOUT.addDependency(boolSelectionSIN);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set number of input signals\n";
this->addCommand(
"setSignalNumber",
makeCommandVoid1(*(Base *)this, &Base::setSignalNumber, docstring));
docstring =
"\n"
" Get number of input signals\n";
this->addCommand("getSignalNumber",
new command::Getter<Base, int>(
*this, &Base::getSignalNumber, docstring));
}
~Switch() {}
/// Header documentation of the python class
virtual std::string getDocString() const {
return "Dynamically select a given signal based on a input information.\n";
}
SignalPtr<int, Time> selectionSIN;
SignalPtr<bool, Time> boolSelectionSIN;
private:
Value &signal(Value &ret, const Time &time) {
int sel;
if (selectionSIN.isPlugged()) {
sel = selectionSIN(time);
} else {
const bool &b = boolSelectionSIN(time);
sel = b ? 1 : 0;
}
if (sel < 0 || sel >= int(this->signalsIN.size()))
throw std::runtime_error("Signal selection is out of range.");
ret = this->signalsIN[sel]->access(time);
return ret;
}
};
} // namespace sot
} // namespace dynamicgraph
#endif // __SOT_SWITCH_H__
include/sot-core/task-abstract.h → include/sot/core/task-abstract.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: task-abstract.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TASKABSTRACT_H__
#define __SOT_TASKABSTRACT_H__
......@@ -27,78 +15,74 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <MatrixAbstractLayer/boostMatrixSvd.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
#include <Eigen/SVD>
/* STD */
#include <string>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/multi-bound.h>
#include <sot-core/sot-core-api.h>
#include <dynamic-graph/entity.h>
#include <sot/core/multi-bound.hh>
#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOT_CORE_EXPORT TaskAbstract
: public dg::Entity
{
/// Hierarchical element of the stack of tasks.
///
/// A task computes a value and a Jacobian as output signals.
/// Once stacked into a solver, the solver will compute the control
/// vector that makes the task values converge toward zero in the
/// order defined by the priority levels.
///
/// \image html pictures/task.png "Task diagram: Task types derive from
/// TaskAbstract. The value and Jacobian of a Task are computed from the
/// features that are stored in the task.
class SOT_CORE_EXPORT TaskAbstract : public dynamicgraph::Entity {
public:
/* Use a derivative of this class to store computational memory. */
class MemoryTaskAbstract
{
public:
class MemoryTaskAbstract {
public:
int timeLastChange;
public:
MemoryTaskAbstract( void ) : timeLastChange(0) {};
virtual ~MemoryTaskAbstract( void ) {};
public:
virtual void commandLine( const std::string& cmdLine
,std::istringstream& cmdArgs
,std::ostream& os ) = 0;
virtual void display( std::ostream& os ) const = 0;
friend std::ostream&
operator<<( std::ostream& os,const MemoryTaskAbstract& tcm )
{tcm.display(os); return os;}
public:
MemoryTaskAbstract(void) : timeLastChange(0){};
virtual ~MemoryTaskAbstract(void){};
public:
virtual void display(std::ostream &os) const = 0;
friend std::ostream &operator<<(std::ostream &os,
const MemoryTaskAbstract &tcm) {
tcm.display(os);
return os;
}
};
public:
MemoryTaskAbstract * memoryInternal;
MemoryTaskAbstract *memoryInternal;
protected:
void taskRegistration( void );
void taskRegistration(void);
public:
TaskAbstract( const std::string& n );
TaskAbstract(const std::string &n);
public: /* --- SIGNALS --- */
dg::SignalTimeDependent< VectorMultiBound,int > taskSOUT;
dg::SignalTimeDependent< ml::Matrix,int > jacobianSOUT;
dg::SignalTimeDependent< ml::Vector,int > featureActivationSOUT;
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine
,std::istringstream& cmdArgs
,std::ostream& os ) ;
public:
dynamicgraph::SignalTimeDependent<VectorMultiBound, int> taskSOUT;
dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASKABSTRACT_H__ */
include/sot-core/task-conti.h → include/sot/core/task-conti.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: task-conti.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TASKCONTI_H__
#define __SOT_TASKCONTI_H__
......@@ -27,87 +15,69 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* STD */
#include <string>
/* SOT */
#include <sot-core/feature-abstract.h>
#include <sot-core/flags.h>
#include <sot-core/task.h>
#include <sot-core/exception-task.h>
#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (task_conti_EXPORTS)
# define SOTTASKCONTI_EXPORT __declspec(dllexport)
# else
# define SOTTASKCONTI_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(task_conti_EXPORTS)
#define SOTTASKCONTI_EXPORT __declspec(dllexport)
#else
#define SOTTASKCONTI_EXPORT __declspec(dllimport)
#endif
#else
# define SOTTASKCONTI_EXPORT
#define SOTTASKCONTI_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOTTASKCONTI_EXPORT TaskConti
: public Task
{
class SOTTASKCONTI_EXPORT TaskConti : public Task {
protected:
enum TimeRefValues
{
TIME_REF_UNSIGNIFICANT = -1
,TIME_REF_TO_BE_SET =-2
};
enum TimeRefValues { TIME_REF_UNSIGNIFICANT = -1, TIME_REF_TO_BE_SET = -2 };
int timeRef;
double mu;
ml::Vector q0;
dynamicgraph::Vector q0;
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public:
TaskConti( const std::string& n );
TaskConti(const std::string &n);
void referenceTime( const unsigned int & t ) { timeRef = t; }
const int & referenceTime( void ) { return timeRef; }
void referenceTime(const unsigned int &t) { timeRef = t; }
const int &referenceTime(void) { return timeRef; }
/* --- COMPUTATION --- */
VectorMultiBound& computeContiDesiredVelocity( VectorMultiBound &task,
const int & time );
VectorMultiBound &computeContiDesiredVelocity(VectorMultiBound &task,
const int &time);
/* --- SIGNALS ------------------------------------------------------------ */
public:
dg::SignalPtr< ml::Vector,int > controlPrevSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> controlPrevSIN;
/* --- DISPLAY ------------------------------------------------------------ */
void display( std::ostream& os ) const;
/* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
void display(std::ostream &os) const;
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASKCONTI_H__ */
include/sot-core/task-pd.h → include/sot/core/task-pd.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: task-pd.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TASK_PD_H__
#define __SOT_TASK_PD_H__
......@@ -26,67 +14,55 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#include <sot-core/task.h>
#include <sot/core/task.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (task_pd_EXPORTS)
# define SOTTASKPD_EXPORT __declspec(dllexport)
# else
# define SOTTASKPD_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(task_pd_EXPORTS)
#define SOTTASKPD_EXPORT __declspec(dllexport)
#else
#define SOTTASKPD_EXPORT __declspec(dllimport)
#endif
#else
# define SOTTASKPD_EXPORT
#define SOTTASKPD_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOTTASKPD_EXPORT TaskPD
: public Task
{
class SOTTASKPD_EXPORT TaskPD : public Task {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
ml::Vector previousError;
dynamicgraph::Vector previousError;
double beta;
public:
TaskPD( const std::string& n );
TaskPD(const std::string &n);
/* --- COMPUTATION --- */
ml::Vector& computeErrorDot( ml::Vector& error,int time );
VectorMultiBound& computeTaskModif( VectorMultiBound& error,int time );
dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &error, int time);
VectorMultiBound &computeTaskModif(VectorMultiBound &error, int time);
/* --- SIGNALS ------------------------------------------------------------ */
public:
dg::SignalTimeDependent< ml::Vector,int > errorDotSOUT;
dg::SignalPtr< ml::Vector,int > errorDotSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> errorDotSOUT;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> errorDotSIN;
/* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine
,std::istringstream& cmdArgs
,std::ostream& os );
void initCommand(void);
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASK_PD_H__ */
include/sot-core/task-unilateral.h → include/sot/core/task-unilateral.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet Gepetto, LAAS-CNRS, 2009
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: task-unilateral.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TASKUNILATERAL_H__
#define __SOT_TASKUNILATERAL_H__
......@@ -29,73 +15,64 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* STD */
#include <string>
/* SOT */
#include <sot-core/feature-abstract.h>
#include <sot-core/flags.h>
#include <sot-core/task.h>
#include <sot-core/exception-task.h>
#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (task_unilateral_EXPORTS)
# define SOTTASKUNILATERAL_EXPORT __declspec(dllexport)
# else
# define SOTTASKUNILATERAL_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(task_unilateral_EXPORTS)
#define SOTTASKUNILATERAL_EXPORT __declspec(dllexport)
#else
# define SOTTASKUNILATERAL_EXPORT
#define SOTTASKUNILATERAL_EXPORT __declspec(dllimport)
#endif
#else
#define SOTTASKUNILATERAL_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOTTASKUNILATERAL_EXPORT TaskUnilateral
: public Task
{
class SOTTASKUNILATERAL_EXPORT TaskUnilateral : public Task {
protected:
std::list< FeatureAbstract* > featureList;
std::list<FeatureAbstract *> featureList;
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public:
TaskUnilateral( const std::string& n );
TaskUnilateral(const std::string &n);
/* --- COMPUTATION --- */
VectorMultiBound& computeTaskUnilateral( VectorMultiBound& res,int time );
VectorMultiBound &computeTaskUnilateral(VectorMultiBound &res, int time);
/* --- SIGNALS ------------------------------------------------------------ */
public:
dg::SignalPtr< ml::Vector,int > positionSIN;
dg::SignalPtr< ml::Vector,int > referenceInfSIN;
dg::SignalPtr< ml::Vector,int > referenceSupSIN;
dg::SignalPtr< double,int > dtSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> referenceInfSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> referenceSupSIN;
dynamicgraph::SignalPtr<double, int> dtSIN;
/* --- DISPLAY ------------------------------------------------------------ */
void display( std::ostream& os ) const;
void display(std::ostream &os) const;
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASKUNILATERAL_H__ */
include/sot-core/task.h → include/sot/core/task.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: task.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TASK_H__
#define __SOT_TASK_H__
......@@ -29,31 +15,29 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* STD */
#include <string>
/* SOT */
#include <sot-core/feature-abstract.h>
#include <sot-core/flags.h>
#include <sot-core/task-abstract.h>
#include <sot-core/exception-task.h>
#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined task_EXPORTS)
# define SOTTASK_EXPORT __declspec(dllexport)
# else
# define SOTTASK_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined task_EXPORTS
#define SOTTASK_EXPORT __declspec(dllexport)
#else
# define SOTTASK_EXPORT
#define SOTTASK_EXPORT __declspec(dllimport)
#endif
#else
#define SOTTASK_EXPORT
#endif
/* --------------------------------------------------------------------- */
......@@ -61,11 +45,13 @@ namespace ml = maal::boost;
/* --------------------------------------------------------------------- */
/*!
@ingroup tasks
This class defines the basic elements of a task.
@class dynamicgraph::sot::Task task.hh "Definition"
@brief Class that defines the basic elements of a task.
A task is defined as \f$ {\bf s} ={\bf e}({\bf q}) \f$
where \f${\bf s} \f$ is a set of features and \f${\bf q}\f$ the
actuated joints of the robot. <br>
It is assume that \f$ {\bf e} = - \lambda \dot{\bf e} \f$.
It is assumes that \f$ \dot{\bf e} = - \lambda {\bf e} \f$.
Moreover as it assumed that this task can provide:
\f$ {\bf J} = \frac{\delta f}{\delta {\bf q}} \f$
It then possible to compute
......@@ -78,68 +64,62 @@ namespace ml = maal::boost;
This class makes also possible to select some of the
listed of features to compute the control law through setControlSelection,
addControlSelection, clearControlSelection.
*/
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class SOTTASK_EXPORT Task
: public TaskAbstract
{
class SOTTASK_EXPORT Task : public TaskAbstract {
public:
typedef std::list<FeatureAbstract *> FeatureList_t;
protected:
std::list< FeatureAbstract* > featureList;
FeatureList_t featureList;
bool withDerivative;
public:
private: //HACK
static const std::string CLASS_NAME;
public: //HACK
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
DYNAMIC_GRAPH_ENTITY_DECL();
public:
Task( const std::string& n );
Task(const std::string &n);
void initCommands(void);
void addFeature( FeatureAbstract& s );
void clearFeatureList( void );
void addFeature(FeatureAbstract &s);
void addFeatureFromName(const std::string &name);
void clearFeatureList(void);
FeatureList_t &getFeatureList(void) { return featureList; }
void setControlSelection( const Flags& act );
void addControlSelection( const Flags& act );
void clearControlSelection( void );
void setControlSelection(const Flags &act);
void addControlSelection(const Flags &act);
void clearControlSelection(void);
/* --- COMPUTATION --- */
ml::Vector& computeError( ml::Vector& error,int time );
VectorMultiBound&
computeTaskExponentialDecrease( VectorMultiBound& errorRef,int time );
ml::Matrix& computeJacobian( ml::Matrix& J,int time );
ml::Vector& computeFeatureActivation( ml::Vector& h,int time );
void setWithDerivative(const bool &s);
bool getWithDerivative(void);
/* --- COMPUTATION --- */
dynamicgraph::Vector &computeError(dynamicgraph::Vector &error, int time);
VectorMultiBound &computeTaskExponentialDecrease(VectorMultiBound &errorRef,
int time);
dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &J, int time);
dynamicgraph::Vector &computeErrorTimeDerivative(dynamicgraph::Vector &res,
int time);
/* --- SIGNALS ------------------------------------------------------------ */
public:
dg::SignalPtr< double,int > controlGainSIN;
dg::SignalPtr< double,int > dampingGainSINOUT;
dg::SignalPtr< Flags,int > controlSelectionSIN; // At the task level or at the feature level?
public:
dg::SignalTimeDependent< ml::Vector,int > errorSOUT;
dynamicgraph::SignalPtr<double, int> controlGainSIN;
dynamicgraph::SignalPtr<double, int> dampingGainSINOUT;
dynamicgraph::SignalPtr<Flags, int> controlSelectionSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> errorSOUT;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
errorTimeDerivativeSOUT;
/* --- DISPLAY ------------------------------------------------------------ */
void display( std::ostream& os ) const;
// friend std::ostream& operator<< ( std::ostream& os,const Task& t );
void display(std::ostream &os) const;
/* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine
,std::istringstream& cmdArgs
,std::ostream& os );
/* --- Writing graph --- */
virtual std::ostream& writeGraph( std::ostream& os ) const;
virtual std::ostream &writeGraph(std::ostream &os) const;
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASK_H__ */
include/sot-core/time-stamp.h → include/sot/core/time-stamp.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: TimeStamp.h
* Project: SOT
* Author: Paul Evrard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TIME_STAMP__HH
#define __SOT_TIME_STAMP__HH
......@@ -26,92 +14,78 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/linear-algebra.h>
/* Classes standards. */
#ifndef WIN32
#include <sys/time.h>
#else /*WIN32*/
#include <Winsock2.h>
#include <sot/utils-windows.h>
#include <sot/core/utils-windows.hh>
#endif /*WIN32*/
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/debug.h>
#include <dynamic-graph/entity.h>
#include <sot/core/debug.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (TimeStamp_EXPORTS)
# define TimeStamp_EXPORT __declspec(dllexport)
# else
# define TimeStamp_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(time_stamp_EXPORTS)
#define TimeStamp_EXPORT __declspec(dllexport)
#else
#define TimeStamp_EXPORT __declspec(dllimport)
#endif
#else
# define TimeStamp_EXPORT
#define TimeStamp_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
class TimeStamp_EXPORT TimeStamp
:public dg::Entity
{
class TimeStamp_EXPORT TimeStamp : public dynamicgraph::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
protected:
struct timeval val;
unsigned int offsetValue;
bool offsetSet;
public:
/* --- CONSTRUCTION --- */
TimeStamp( const std::string& name );
TimeStamp(const std::string &name);
public: /* --- DISPLAY --- */
virtual void display( std::ostream& os ) const;
virtual void display(std::ostream &os) const;
public: /* --- SIGNALS --- */
/* These signals can be called several time per period, given
/* These signals can be called several time per period, given
* each time a different results. Useful for chronos. */
dg::Signal<ml::Vector,int> timeSOUT;
dg::Signal<double,int> timeDoubleSOUT;
dynamicgraph::Signal<dynamicgraph::Vector, int> timeSOUT;
dynamicgraph::Signal<double, int> timeDoubleSOUT;
/* These signals can be called several time per period, but give
* always the same results different results. Useful for synchro. */
dg::SignalTimeDependent<ml::Vector,int> timeOnceSOUT;
dg::SignalTimeDependent<double,int> timeOnceDoubleSOUT;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> timeOnceSOUT;
dynamicgraph::SignalTimeDependent<double, int> timeOnceDoubleSOUT;
protected: /* --- SIGNAL FUNCTIONS --- */
ml::Vector& getTimeStamp( ml::Vector& res,const int& time );
double& getTimeStampDouble( const ml::Vector& vect,double& res );
public: /* --- COMMANDS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
dynamicgraph::Vector &getTimeStamp(dynamicgraph::Vector &res,
const int &time);
double &getTimeStampDouble(const dynamicgraph::Vector &vect, double &res);
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SOT_HH */
include/sot-core/timer.h → include/sot/core/timer.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: Timer.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_TIMER_HH
#define __SOT_TIMER_HH
......@@ -27,172 +15,145 @@
/* --------------------------------------------------------------------- */
/* Classes standards. */
#include <list> /* Classe std::list */
#include <list> /* Classe std::list */
#ifndef WIN32
#include <sys/time.h>
#else /*WIN32*/
#else /*WIN32*/
// When including Winsock2.h, the MAL must be included first
#include <MatrixAbstractLayer/boost.h>
#include <sot-core/utils-windows.h>
#include <Winsock2.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/utils-windows.hh>
#endif /*WIN32*/
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/debug.h>
#include <dynamic-graph/interpreter.h>
#include <dynamic-graph/entity.h>
#include <sot/core/debug.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (timer_EXPORTS)
# define Timer_EXPORT __declspec(dllexport)
# else
# define Timer_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(timer_EXPORTS)
#define Timer_EXPORT __declspec(dllexport)
#else
#define Timer_EXPORT __declspec(dllimport)
#endif
#else
# define Timer_EXPORT
#define Timer_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dg = dynamicgraph;
template< class T >
class Timer_EXPORT Timer
:public dg::Entity
{
template <class T>
class Timer_EXPORT Timer : public dynamicgraph::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
protected:
struct timeval t0,t1;
struct timeval t0, t1;
clock_t c0, c1;
double dt;
public:
/* --- CONSTRUCTION --- */
Timer( const std::string& name );
Timer(const std::string &name);
public: /* --- DISPLAY --- */
virtual void display( std::ostream& os ) const;
Timer_EXPORT friend std::ostream& operator<< ( std::ostream& os,const Timer<T>& timer )
{ timer.display(os); return os; }
virtual void display(std::ostream &os) const;
Timer_EXPORT friend std::ostream &operator<<(std::ostream &os,
const Timer<T> &timer) {
timer.display(os);
return os;
}
public: /* --- SIGNALS --- */
dg::SignalPtr<T,int> sigSIN;
dg::SignalTimeDependent<T,int> sigSOUT;
dg::Signal<double,int> timerSOUT;
dynamicgraph::SignalPtr<T, int> sigSIN;
dynamicgraph::SignalTimeDependent<T, int> sigSOUT;
dynamicgraph::SignalTimeDependent<T, int> sigClockSOUT;
dynamicgraph::Signal<double, int> timerSOUT;
protected: /* --- SIGNAL FUNCTIONS --- */
void plug( dg::Signal<T,int> &sig )
{
sigSIN = &sig; dt=0.;
void plug(dynamicgraph::Signal<T, int> &sig) {
sigSIN = &sig;
dt = 0.;
}
template <bool UseClock>
T &compute(T &t, const int &time) {
sotDEBUGIN(15);
if (UseClock) {
c0 = clock();
sotDEBUG(15) << "t0: " << c0 << std::endl;
} else {
gettimeofday(&t0, NULL);
sotDEBUG(15) << "t0: " << t0.tv_sec << " - " << t0.tv_usec << std::endl;
}
T& compute( T& t,const int& time )
{
sotDEBUGIN(15);
gettimeofday(&t0,NULL);
sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
t = sigSIN( time );
gettimeofday(&t1,NULL);
dt = ( (t1.tv_sec-t0.tv_sec) * 1000.
+ (t1.tv_usec-t0.tv_usec+0.) / 1000. );
sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
timerSOUT = dt;
sotDEBUGOUT(15);
return t;
t = sigSIN(time);
if (UseClock) {
c1 = clock();
sotDEBUG(15) << "t1: " << c0 << std::endl;
dt = ((double)(c1 - c0) * 1000) / CLOCKS_PER_SEC;
} else {
gettimeofday(&t1, NULL);
dt = ((static_cast<double>(t1.tv_sec) - static_cast<double>(t0.tv_sec)) *
1000. +
(static_cast<double>(t1.tv_usec) - static_cast<double>(t0.tv_usec) +
0.) /
1000.);
sotDEBUG(15) << "t1: " << t1.tv_sec << " - " << t1.tv_usec << std::endl;
}
double& getDt( double& res,const int& time )
{
res=dt;
return res;
}
public: /* --- COMMANDS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
timerSOUT = dt;
timerSOUT.setTime(time);
sotDEBUGOUT(15);
return t;
}
double &getDt(double &res, const int & /*time*/) {
res = dt;
return res;
}
};
void cmdChrono( const std::string& cmd,
std::istringstream& args,
std::ostream& os );
void cmdChrono(const std::string &cmd, std::istringstream &args,
std::ostream &os);
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
template< class T >
Timer<T>::
Timer( const std::string& name )
:Entity(name)
,t0(),t1()
,dt(0.)
,sigSIN( NULL,"Timer("+name+")::output(T)::in" )
,sigSOUT( boost::bind(&Timer::compute,this,_1,_2),
sigSIN,
"Timer("+name+")::output(T)::out" )
,timerSOUT( "Timer("+name+")::output(double)::timer" )
{
template <class T>
Timer<T>::Timer(const std::string &name)
: Entity(name),
dt(0.),
sigSIN(NULL, "Timer(" + name + ")::input(T)::sin"),
sigSOUT(boost::bind(&Timer::compute<false>, this, _1, _2), sigSIN,
"Timer(" + name + ")::output(T)::sout"),
sigClockSOUT(boost::bind(&Timer::compute<true>, this, _1, _2), sigSIN,
"Timer(" + name + ")::output(T)::clockSout"),
timerSOUT("Timer(" + name + ")::output(double)::timer") {
sotDEBUGIN(15);
timerSOUT.setFunction( boost::bind(&Timer::getDt,this,_1,_2) );
signalRegistration( sigSIN<<sigSOUT<<timerSOUT );
timerSOUT.setFunction(boost::bind(&Timer::getDt, this, _1, _2));
signalRegistration(sigSIN << sigSOUT << sigClockSOUT << timerSOUT);
sotDEBUGOUT(15);
}
/* --- DISPLAY --------------------------------------------------------- */
template< class T >
void Timer<T>::
display( std::ostream& os ) const
{
os << "Timer <"<< sigSIN << "> : " << dt << "ms." << std::endl;
template <class T>
void Timer<T>::display(std::ostream &os) const {
os << "Timer <" << sigSIN << "> : " << dt << "ms." << std::endl;
}
/* --- COMMAND --------------------------------------------------------- */
template< class T >
void Timer<T>::
commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUGIN(15);
if( cmdLine == "help")
{
os << "Timer: "<<std::endl;
Entity::commandLine( cmdLine,cmdArgs,os );
}
else
Entity::commandLine( cmdLine,cmdArgs,os );
sotDEBUGOUT(15);
}
#endif /* #ifndef __SOT_SOT_HH */
include/sot/core/trajectory.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2013,
* Olivier Stasse,
*
* CNRS
*
*/
#ifndef SOT_TRAJECTORY_H__
#define SOT_TRAJECTORY_H__
// Matrix
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-caster.h>
#include <boost/array.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/regex.hpp>
#include <sot/core/api.hh>
namespace dg = dynamicgraph;
namespace ba = boost::assign;
namespace dynamicgraph {
namespace sot {
class Trajectory;
class RulesJointTrajectory {
protected:
Trajectory &TrajectoryToFill_;
public:
unsigned int dbg_level;
/// \brief Strings specifying the grammar of the structure.
std::string float_str_re, seq_str_re, timestamp_str_re, frame_id_str_re,
header_str_re, joint_name_str_re, list_of_jn_str_re, point_value_str_re,
list_of_pv_str_re, bg_pt_str_re, end_pt_str_re, comma_pt_str_re,
bg_liste_of_pts_str_re;
/// \brief Boost regular expressions implementing the grammar.
boost::regex header_re, list_of_jn_re, list_of_pv_re, bg_pt_re, end_pt_re,
comma_pt_re, bg_liste_of_pts_re;
std::vector<std::string> joint_names;
/// \brief Constructor TrajectoryToFill is the structure where to store the
/// parsed information.
RulesJointTrajectory(Trajectory &TrajectoryToFill);
/// \brief parse_string will fill TrajectoryToFill with string atext.
void parse_string(std::string &atext);
protected:
/// \brief General parsing method of text with regexp e. The results are given
/// in what. The remaining text is left in sub_text.
bool search_exp_sub_string(
std::string &text,
boost::match_results<std::string::const_iterator> &what, boost::regex &e,
std::string &sub_text);
/// \brief Find and store the header.
/// This method is looking for:
/// unsigned int seq.
/// unsigned int sec, unsigned int nsec.
/// string format_id
void parse_header(std::string &text, std::string &sub_text1);
/// \brief Understand joint_names.
/// Extract a list of strings.
void parse_joint_names(std::string &text, std::string &sub_text1,
std::vector<std::string> &joint_names);
/// \brief Extract a sequence of doubles.
/// To be used for position, velocities, accelerations and effort.
bool parse_seq(std::string &text, std::string &sub_text1,
std::vector<double> &seq);
/// \brief Extract a point description.
bool parse_point(std::string &trajectory, std::string &sub_text1);
/// \brief Extract a sequence of points.
bool parse_points(std::string &trajectory, std::string &sub_text1);
};
class SOT_CORE_EXPORT timestamp {
public:
unsigned long int secs_;
unsigned long int nsecs_;
timestamp() : secs_(0), nsecs_(0) {}
timestamp(const timestamp &ats) {
secs_ = ats.secs_;
nsecs_ = ats.nsecs_;
}
timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
secs_ = lsecs;
nsecs_ = lnsecs;
}
bool operator==(const timestamp &other) const {
if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_)) return false;
return true;
}
friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
return stream;
}
};
class SOT_CORE_EXPORT Header {
public:
unsigned int seq_;
timestamp stamp_;
std::string frame_id_;
Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
};
class SOT_CORE_EXPORT JointTrajectoryPoint {
public:
std::vector<double> positions_;
std::vector<double> velocities_;
std::vector<double> accelerations_;
std::vector<double> efforts_;
typedef std::vector<double> vec_ref;
void display(std::ostream &os) const {
boost::array<std::string, 4> names = boost::assign::list_of("Positions")(
"Velocities")("Accelerations")("Effort");
const std::vector<double> *points = 0;
for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
switch (arrayId) {
case (0):
points = &positions_;
break;
case (1):
points = &velocities_;
break;
case (2):
points = &accelerations_;
break;
case (3):
points = &efforts_;
break;
default:
assert(0);
}
std::vector<double>::const_iterator it_db;
os << names[arrayId] << std::endl << "---------" << std::endl;
for (it_db = points->begin(); it_db != points->end(); it_db++) {
os << *it_db << std::endl;
}
}
}
void transfer(const std::vector<double> &src, unsigned int vecId) {
switch (vecId) {
case (0):
positions_ = src;
break;
case (1):
velocities_ = src;
break;
case (2):
accelerations_ = src;
break;
case (3):
efforts_ = src;
break;
default:
assert(0);
}
}
};
class SOT_CORE_EXPORT Trajectory {
public:
Trajectory();
Trajectory(const Trajectory &copy);
virtual ~Trajectory();
std::vector<std::string> joint_names_;
Header header_;
double time_from_start_;
std::vector<JointTrajectoryPoint> points_;
int deserialize(std::istringstream &is);
void display(std::ostream &) const;
};
} // namespace sot
template <>
struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {};
} // namespace dynamicgraph
#endif /* #ifndef SOT_TRAJECTORY_H__ */
include/sot-core/unary-op.h → include/sot/core/unary-op.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: unary-op.h
* Project: SOT
* Author: Nicolas Mansard
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
#ifndef __SOT_BINARYOP_H__
#define __SOT_BINARYOP_H__
#ifndef SOT_CORE_UNARYOP_HH
#define SOT_CORE_UNARYOP_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* SOT */
#include <sot-core/flags.h>
#include <dynamic-graph/entity.h>
#include <sot-core/pool.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/all-signals.h>
/* STD */
#include <string>
#include <boost/function.hpp>
#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
template< class Tin,class Tout,typename Operator >
class UnaryOp
:public dg::Entity
{
template <typename Operator>
class UnaryOp : public Entity {
Operator op;
typedef typename Operator::Tin Tin;
typedef typename Operator::Tout Tout;
typedef UnaryOp<Operator> Self;
public: /* --- CONSTRUCTION --- */
static std::string getTypeInName( void ) { return "UnknownIn"; }
static std::string getTypeOutName( void ) { return "UnknownOut"; }
static std::string getTypeInName(void) { return Operator::nameTypeIn(); }
static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
static const std::string CLASS_NAME;
UnaryOp( const std::string& name )
: dg::Entity(name)
,SIN(NULL,UnaryOp::CLASS_NAME+"("+name+")::input("+getTypeInName()+")::in")
,SOUT( boost::bind(&UnaryOp<Tin,Tout,Operator>::computeOperation,this,_1,_2),
SIN,CLASS_NAME+"("+name+")::output("+getTypeOutName()+")::out")
{
signalRegistration( SIN<<SOUT );
}
virtual const std::string &getClassName() const { return CLASS_NAME; }
std::string getDocString() const { return op.getDocString(); }
virtual ~UnaryOp( void ) {};
UnaryOp(const std::string &name)
: Entity(name),
SIN(NULL, Self::CLASS_NAME + "(" + name + ")::input(" +
Self::getTypeInName() + ")::sin"),
SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN,
Self::CLASS_NAME + "(" + name + ")::output(" +
Self::getTypeOutName() + ")::sout") {
signalRegistration(SIN << SOUT);
op.addSpecificCommands(*this, commandMap);
}
public: /* --- SIGNAL --- */
virtual ~UnaryOp(void){};
dg::SignalPtr<Tin,int> SIN;
dg::SignalTimeDependent<Tout,int> SOUT;
public: /* --- SIGNAL --- */
SignalPtr<Tin, int> SIN;
SignalTimeDependent<Tout, int> SOUT;
protected:
Tout& computeOperation( Tout& res,int time )
{
const Tin &x1 = SIN(time);
op(x1,res);
return res;
}
Tout &computeOperation(Tout &res, int time) {
const Tin &x1 = SIN(time);
op(x1, res);
return res;
}
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os ) ;
};
} /* namespace sot */
} /* namespace dynamicgraph */
} // namespace sot
#endif // #ifndef __SOT_BINARYOP_H__
#endif // #ifndef SOT_CORE_UNARYOP_HH
include/sot-core/utils-windows.h → include/sot/core/utils-windows.hh
View file @ 1c36e63b
/*
* utils-windows.h
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
* Created on: Jun 25, 2010
* Author: blue
*/
#ifndef UTILSWINDOWS_H_
......@@ -10,14 +12,15 @@
#ifdef WIN32
#include <sot-core/sot-core-api.h>
#include < time.h >
struct SOT_CORE_EXPORT timezone
{
int tz_minuteswest; /* minutes W of Greenwich */
int tz_dsttime; /* type of dst correction */
#include "sot/core/api.hh"
#define NOMINMAX
#include <Winsock2.h>
struct SOT_CORE_EXPORT timezone {
int tz_minuteswest; /* minutes W of Greenwich */
int tz_dsttime; /* type of dst correction */
};
int SOT_CORE_EXPORT gettimeofday(struct timeval *tv, struct timezone *tz);
......
include/sot/core/variadic-op.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2018,
* Mirabel Joseph
*
* CNRS/AIST
*
*/
#ifndef SOT_CORE_VARIADICOP_HH
#define SOT_CORE_VARIADICOP_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>
/* STD */
#include <boost/function.hpp>
#include <string>
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template <typename Tin, typename Tout, typename Time>
class VariadicAbstract : public Entity {
public: /* --- CONSTRUCTION --- */
static std::string getTypeInName(void);
static std::string getTypeOutName(void);
VariadicAbstract(const std::string &name, const std::string &className)
: Entity(name),
SOUT(className + "(" + name + ")::output(" + getTypeOutName() +
")::sout"),
baseSigname(className + "(" + name + ")::input(" + getTypeInName() +
")::") {
signalRegistration(SOUT);
}
virtual ~VariadicAbstract(void) {
for (std::size_t i = 0; i < signalsIN.size(); ++i) {
_removeSignal(i);
}
};
public: /* --- SIGNAL --- */
typedef SignalPtr<Tin, int> signal_t;
SignalTimeDependent<Tout, int> SOUT;
std::size_t addSignal() {
std::ostringstream oss;
oss << "sin" << signalsIN.size();
return addSignal(oss.str());
}
std::size_t addSignal(const std::string &name) {
signal_t *sig = new signal_t(NULL, baseSigname + name);
try {
_declareSignal(sig);
signalsIN.push_back(sig);
// names.push_back (name);
return signalsIN.size() - 1;
} catch (const ExceptionAbstract &) {
delete sig;
throw;
}
}
void removeSignal() {
assert(signalsIN.size() > 0);
_removeSignal(signalsIN.size() - 1);
// names.pop_back();
signalsIN.pop_back();
}
void setSignalNumber(const int &n) {
assert(n >= 0);
const std::size_t oldSize = signalsIN.size();
for (std::size_t i = n; i < oldSize; ++i) _removeSignal(i);
signalsIN.resize(n, NULL);
// names.resize(n);
for (std::size_t i = oldSize; i < (std::size_t)n; ++i) {
assert(signalsIN[i] == NULL);
std::ostringstream oss;
oss << baseSigname << "sin" << i;
// names[i] = oss.str();
// signal_t* s = new signal_t (NULL,names[i]);
signal_t *s = new signal_t(NULL, oss.str());
signalsIN[i] = s;
_declareSignal(s);
}
updateSignalNumber(n);
}
int getSignalNumber() const { return (int)signalsIN.size(); }
signal_t *getSignalIn(int i) {
if (i < 0 || i >= (int)signalsIN.size())
throw std::out_of_range("Wrong signal index");
return signalsIN[i];
}
protected:
std::vector<signal_t *> signalsIN;
// Use signal->shortName instead
// std::vector< std::string > names;
virtual void updateSignalNumber(int n) { (void)n; };
private:
void _removeSignal(const std::size_t i) {
// signalDeregistration(names[i]);
signalDeregistration(signalsIN[i]->shortName());
SOUT.removeDependency(*signalsIN[i]);
delete signalsIN[i];
}
void _declareSignal(signal_t *s) {
signalRegistration(*s);
SOUT.addDependency(*s);
}
const std::string baseSigname;
};
template <typename Operator>
class VariadicOp : public VariadicAbstract<typename Operator::Tin,
typename Operator::Tout, int> {
typedef typename Operator::Tin Tin;
typedef typename Operator::Tout Tout;
typedef VariadicOp<Operator> Self;
public: /* --- CONSTRUCTION --- */
Operator op;
typedef VariadicAbstract<Tin, Tout, int> Base;
// static std::string getTypeInName ( void ) { return Operator::nameTypeIn ();
// } static std::string getTypeOutName( void ) { return
// Operator::nameTypeOut(); }
static const std::string CLASS_NAME;
virtual const std::string &getClassName() const { return CLASS_NAME; }
std::string getDocString() const { return op.getDocString(); }
VariadicOp(const std::string &name) : Base(name, CLASS_NAME) {
this->SOUT.setFunction(boost::bind(&Self::computeOperation, this, _1, _2));
op.initialize(this, this->commandMap);
}
virtual ~VariadicOp(void){};
protected:
Tout &computeOperation(Tout &res, int time) {
std::vector<const Tin *> in(this->signalsIN.size());
for (std::size_t i = 0; i < this->signalsIN.size(); ++i) {
const Tin &x = this->signalsIN[i]->access(time);
in[i] = &x;
}
op(in, res);
return res;
}
inline void updateSignalNumber(int n) { op.updateSignalNumber(n); }
};
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef SOT_CORE_VARIADICOP_HH
include/sot/core/vector-constant.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
#define DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
namespace command {
namespace vectorConstant {
class Resize;
}
} // namespace command
class VectorConstant : public Entity {
friend class command::vectorConstant::Resize;
int rows;
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
VectorConstant(const std::string &name);
virtual ~VectorConstant(void) {}
SignalTimeDependent<dynamicgraph::Vector, int> SOUT;
/// \brief Set value of vector (and therefore of output signal)
void setValue(const dynamicgraph::Vector &inValue);
};
} // namespace sot
} // namespace dynamicgraph
#endif // DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
include/sot/core/vector-to-rotation.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOTVECTORTOMATRIX_HH
#define __SOTVECTORTOMATRIX_HH
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/matrix-geometry.hh>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* STD */
#include <vector>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(vector_to_rotation_EXPORTS)
#define SOTVECTORTOROTATION_EXPORT __declspec(dllexport)
#else
#define SOTVECTORTOROTATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTVECTORTOROTATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
class [[deprecated(
"use RPYToMatrix")]] SOTVECTORTOROTATION_EXPORT VectorToRotation
: public dynamicgraph::Entity {
enum sotAxis { AXIS_X, AXIS_Y, AXIS_Z };
unsigned int size;
std::vector<sotAxis> axes;
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
VectorToRotation(const std::string &name);
virtual ~VectorToRotation(void) {}
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> SIN;
dynamicgraph::SignalTimeDependent<MatrixRotation, int> SOUT;
MatrixRotation &computeRotation(const dynamicgraph::Vector &angles,
MatrixRotation &res);
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif // #ifndef __SOTVECTORTOMATRIX_HH
include/sot/core/visual-point-projecter.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2011, Nicolas Mansard, LAAS-CNRS
*
*/
#ifndef __sot_core_VisualPointProjecter_H__
#define __sot_core_VisualPointProjecter_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(visual_point_projecter_EXPORTS)
#define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllexport)
#else
#define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTVISUALPOINTPROJECTER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/matrix-geometry.hh>
/* SOT */
#include <dynamic-graph/entity-helper.h>
#include <dynamic-graph/signal-helper.h>
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTVISUALPOINTPROJECTER_EXPORT VisualPointProjecter
: public ::dynamicgraph::Entity,
public ::dynamicgraph::EntityHelper<VisualPointProjecter> {
public: /* --- CONSTRUCTOR ---- */
VisualPointProjecter(const std::string &name);
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(point3D, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(transfo, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(depth, double);
DECLARE_SIGNAL_OUT(point2D, dynamicgraph::Vector);
}; // class VisualPointProjecter
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_core_VisualPointProjecter_H__