GCC Code Coverage Report

Directory: ./
File: include/sot/core/sot.hh
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 0 4 0.0%
Branches: 0 0 -%
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 */
207