Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/sot/core/sot.hh	Lines:	0	4	0.0 %
Date:	2023-03-13 12:09:37	Branches:	0	0	- %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_SOT_HH
#define __SOT_SOT_HH

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* Classes standards. */
#include <list> /* Classe std::list   */

/* SOT */
#include <dynamic-graph/entity.h>

#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#ifndef SOTSOT_CORE_EXPORT
#if defined(WIN32)
#if defined(sot_EXPORTS)
#define SOTSOT_CORE_EXPORT __declspec(dllexport)
#else
#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.
*/
class SOTSOT_CORE_EXPORT Sot : public Entity {
 public:
  /*! \brief Specify the name of the class entity. */
  static const std::string CLASS_NAME;

 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;

 protected:
  /*! \brief This field is a list of controllers
    managed by the stack of tasks. */
  StackType stack;

  /*! \brief Store the number of joints to be used in the
    command computed by the stack of tasks. */
  unsigned int nbJoints;

  /*! \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;

  /*! \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 DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
  /*   static const bool USE_CONTI_INVERSE_DEFAULT = false; */

  /*! \brief Number of joints by default. */
  static void taskVectorToMlVector(const VectorMultiBound &taskVector,
                                   Vector &err);

 public:
  /*! \brief Default constructor */
  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. */
  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. */
  virtual TaskAbstract &pop(void);

  /*! \brief This method allows to know if a task exists or not */
  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.*/
  virtual void remove(const TaskAbstract &task);

  /*! \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. */
  virtual void up(const TaskAbstract &task);

  /*! \brief This method makes the task to swap with the task having the
    immediate inferior priority. */
  virtual void down(const TaskAbstract &task);

  /*! \brief Remove all the tasks from the stack. */
  virtual void clear(void);
  /*! @} */

  /*! @} */

  /*! \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.
    @{
  */

  /*! \brief Compute the control law. */
  virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control,
                                                  const int &time);

  /*! @} */

 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;
  /*! Wrap the previous method around an operator. */
  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 coming from the other
   * OpenHRP plugins).
   */
  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;
  /*! @} */

  /*! \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

#endif /* #ifndef __SOT_SOT_HH */


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			#ifndef __SOT_SOT_HH
11			#define __SOT_SOT_HH
12
13			/* --------------------------------------------------------------------- */
14			/* --- INCLUDE --------------------------------------------------------- */
15			/* --------------------------------------------------------------------- */
16
17			/* Matrix */
18			#include <dynamic-graph/linear-algebra.h>
19
20			/* Classes standards. */
21			#include <list> /* Classe std::list */
22
23			/* SOT */
24			#include <dynamic-graph/entity.h>
25
26			#include <sot/core/flags.hh>
27			#include <sot/core/task-abstract.hh>
28
29			/* --------------------------------------------------------------------- */
30			/* --- API ------------------------------------------------------------- */
31			/* --------------------------------------------------------------------- */
32
33			#ifndef SOTSOT_CORE_EXPORT
34			#if defined(WIN32)
35			#if defined(sot_EXPORTS)
36			#define SOTSOT_CORE_EXPORT __declspec(dllexport)
37			#else
38			#define SOTSOT_CORE_EXPORT __declspec(dllimport)
39			#endif
40			#else
41			#define SOTSOT_CORE_EXPORT
42			#endif
43			#endif
44
45			/* --------------------------------------------------------------------- */
46			/* --- CLASS ----------------------------------------------------------- */
47			/* --------------------------------------------------------------------- */
48
49			namespace dynamicgraph {
50			namespace sot {
51
52			/*! @ingroup stackoftasks
53			\brief This class implements the Stack of Task.
54			It allows to deal with the priority of the controllers
55			through the shell.
56			*/
57			class SOTSOT_CORE_EXPORT Sot : public Entity {
58			public:
59			/! \brief Specify the name of the class entity. /
60			static const std::string CLASS_NAME;
61
62			public:
63			/! \brief Returns the name of this class. /
64			virtual const std::string &getClassName() const { return CLASS_NAME; }
65
66			/! \brief Defines a type for a list of tasks /
67			typedef std::list<TaskAbstract *> StackType;
68
69			protected:
70			/*! \brief This field is a list of controllers
71			managed by the stack of tasks. */
72			StackType stack;
73
74			/*! \brief Store the number of joints to be used in the
75			command computed by the stack of tasks. */
76			unsigned int nbJoints;
77
78			/*! \brief Option to disable the computation of the SVD for the last task
79			if this task is a Task with a single FeaturePosture */
80			bool enablePostureTaskAcceleration;
81
82			/*! \brief Maximum allowed squared norm of control increment.
83			A task whose control increment is above this value is discarded.
84			It defaults to \c std::numeric_limits<double>::max().
85			\warning This is a security feature and is not a good way of adding a
86			proper constraint on the control generated by SoT.
87			*/
88			double maxControlIncrementSquaredNorm;
89
90			public:
91			/! \brief Threshold to compute the dumped pseudo inverse. /
92			static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
93
94			/* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
95			/* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
96
97			/! \brief Number of joints by default. /
98			static void taskVectorToMlVector(const VectorMultiBound &taskVector,
99			Vector &err);
100
101			public:
102			/! \brief Default constructor /
103			Sot(const std::string &name);
104			~Sot(void) { /* TODO!! */
105			}
106
107			/*! \name Methods to handle the stack.
108			@{
109			*/
110			virtual const StackType &tasks() const { return stack; }
111
112			/*! \brief Push the task in the stack.
113			It has a lowest priority than the previous ones.
114			If this is the first task, then it has the highest
115			priority. */
116			virtual void push(TaskAbstract &task);
117			/*! \brief Pop the task from the stack.
118			This method removes the task with the smallest
119			priority in the task. The other are projected
120			in the null-space of their predecessors. */
121			virtual TaskAbstract &pop(void);
122
123			/! \brief This method allows to know if a task exists or not /
124			virtual bool exist(const TaskAbstract &task);
125
126			/*! \brief Remove a task regardless to its position in the stack.
127			It removes also the signals connected to the output signal of this
128			stack.*/
129			virtual void remove(const TaskAbstract &task);
130
131			/*! \brief This method removes the output signals depending on
132			this task. */
133			virtual void removeDependency(const TaskAbstract &key);
134
135			/*! \brief This method makes the task to swap with the task having the
136			immediate superior priority. */
137			virtual void up(const TaskAbstract &task);
138
139			/*! \brief This method makes the task to swap with the task having the
140			immediate inferior priority. */
141			virtual void down(const TaskAbstract &task);
142
143			/! \brief Remove all the tasks from the stack. /
144			virtual void clear(void);
145			/! @} /
146
147			/! @} /
148
149			/*! \brief This method defines the part of the state vector
150			which correspond to the free flyer of the robot. */
151			virtual void defineNbDof(const unsigned int &nbDof);
152			virtual const unsigned int &getNbDof() const { return nbJoints; }
153
154			/! @} /
155			public: /* --- CONTROL --- */
156			/*! \name Methods to compute the control law following the
157			recursive definition of the stack of tasks.
158			@{
159			*/
160
161			/! \brief Compute the control law. /
162			virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control,
163			const int &time);
164
165			/! @} /
166
167			public: /* --- DISPLAY --- */
168			/*! \name Methods to display the stack of tasks.
169			@{
170			*/
171			/! Display the stack of tasks in text mode as a tree. /
172			virtual void display(std::ostream &os) const;
173			/! Wrap the previous method around an operator. /
174			SOTSOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
175			const Sot &sot);
176			/! @} /
177			public: /* --- SIGNALS --- */
178			/*! \name Methods to handle signals
179			@{
180			*/
181			/*! \brief Intrinsec velocity of the robot, that is used to initialized
182			* the recurence of the SOT (e.g. velocity coming from the other
183			* OpenHRP plugins).
184			*/
185			SignalPtr<dynamicgraph::Vector, int> q0SIN;
186			/*! \brief A matrix K whose columns are a base of the desired velocity.
187			* In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free
188			* parameter to be computed.
189			* \note K should be an orthonormal matrix.
190			*/
191			SignalPtr<dynamicgraph::Matrix, int> proj0SIN;
192			/*! \brief This signal allow to change the threshold for the
193			damped pseudo-inverse on-line */
194			SignalPtr<double, int> inversionThresholdSIN;
195			/! \brief Allow to get the result of the computed control law. /
196			SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
197			/! @} /
198
199			/*! \brief This method write the priority between tasks in the output stream
200			* os. */
201			virtual std::ostream &writeGraph(std::ostream &os) const;
202			};
203			} // namespace sot
204			} // namespace dynamicgraph
205
206			#endif /* #ifndef __SOT_SOT_HH */