ORCA: Optimization-based framework for Robotic Control Applications
TaskBase.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 
38 #pragma once
39 
41 #include "orca/robot/RobotModel.h"
43 #include "orca/optim/Problem.h"
44 #include "orca/common/Mutex.h"
45 
46 
47 namespace orca
48 {
49 namespace common
50 {
51  //class Wrench;
60  {
61  friend optim::Problem;
62  public:
63  using Ptr = std::shared_ptr<TaskBase>;
67  enum State {
68  Init = 0
75  };
76 
77  TaskBase(const std::string& name, optim::ControlVariable control_var);
78  virtual ~TaskBase();
79  bool isActivated() const;
80  bool isComputing() const;
82 
84 
85  virtual bool activate();
86  virtual void update(double current_time, double dt);
87  virtual bool deactivate();
88  virtual void print() const;
89 
90  virtual bool setProblem(std::shared_ptr< const orca::optim::Problem > problem);
91 
92  bool hasProblem() const;
93  bool hasRobot() const;
94 
95  bool dependsOnProblem() const;
96  bool dependsOnWrench() const;
97  bool dependsOnRobotJoints() const;
98  bool dependsOnFloatingBase() const;
99 
100  bool hasWrench() const;
101  bool isRobotInitialized() const;
102  State getState() const;
103  void setRampDuration(double ramp_time);
104  double getRampDuration() const;
105  double getCurrentRampValue() const;
106 
107  double getStartTime() const;
108  double getStopTime() const;
109 
110  std::shared_ptr<const optim::Problem> getProblem()const;
111  std::shared_ptr<const common::Wrench> getWrench() const;
112  std::shared_ptr<const robot::RobotModel> getRobot() const;
113 
117  void addChild(TaskBase::Ptr e);
121  bool hasParent() const;
125  bool hasChildren() const;
129  const std::string& getParentName() const;
133  const std::string& getPrintableName() const;
134 
135  void onResizedCallback(std::function<void(void)> cb);
136  void onActivationCallback(std::function<void(void)> cb);
137  void onActivatedCallback(std::function<void(void)> cb);
138  void onComputeBeginCallback(std::function<void(double,double)> cb);
139  void onComputeEndCallback(std::function<void(double,double)> cb);
140  void onDeactivationCallback(std::function<void(void)> cb);
141  void onDeactivatedCallback(std::function<void(void)> cb);
147  protected:
148  virtual void resize();
150  std::shared_ptr<Wrench> wrench();
151 
152  virtual void onResize() = 0;
153  virtual void onResized() {};
154  virtual void onActivation() {};
155  virtual void onActivated() {};
156  virtual bool rampUp(double time_since_start);
157  void setRampValue(double new_val);
158  virtual void onCompute(double current_time, double dt) = 0;
159  virtual bool rampDown(double time_since_stop);
160  virtual void onDeactivation() {};
161  virtual void onDeactivated() {};
162  private:
163  common::Parameter<double> ramp_duration_ = 0;
164  std::shared_ptr<Wrench> wrench_;
165  private:
166  void setParentName(const std::string& parent_name);
167  void checkIfUpdatable() const;
168  bool is_activated_ = true;
169  State state_ = Init;
170  double start_time_ = 0;
171  double stop_time_ = 0;
172  double ramp_value_ = 0;
173  bool activation_requested_ = false;
174  bool deactivation_requested_ = false;
175 
176  std::shared_ptr<const optim::Problem> problem_;
177  std::shared_ptr<robot::RobotModel> robot_;
178 
179  optim::ControlVariable control_var_;
180  std::list< TaskBase::Ptr > children_;
181 
182  std::function<void(void)> on_resized_cb_;
183  std::function<void(void)> on_activation_cb_;
184  std::function<void(void)> on_activated_cb_;
185  std::function<void(double,double)> on_update_begin_cb_;
186  std::function<void(double,double)> on_update_end_cb_;
187  std::function<void(void)> on_deactivation_cb_;
188  std::function<void(void)> on_deactivated_cb_;
189  //unsigned int getHierarchicalLevel() const;
190  //void getHierarchicalLevel(unsigned int level);
191  //unsigned int hierarchical_level = 0;
192  std::string parent_name_;
193  std::string printable_name_;
194  double current_time_ = -1;
195  double current_dt_ = 0;
196  };
197 
198 
199  inline ::std::ostream& operator<<(::std::ostream& os, const TaskBase::State& s)
200  {
201  switch (s)
202  {
203  case TaskBase::Init: os << "Init"; break;
204  case TaskBase::Resized: os << "Resized"; break;
205  case TaskBase::Activating: os << "Activating"; break;
206  case TaskBase::Activated: os << "Activated"; break;
207  case TaskBase::Deactivating: os << "Deactivating"; break;
208  case TaskBase::Deactivated: os << "Deactivated"; break;
209  case TaskBase::Error: os << "Error"; break;
210  default: break;
211  }
212  return os;
213  }
214 } // namespace common
215 } // namespace orca
216 
217 // This header needs to have TaskBase
218 // TODO: figure out if forward declaring is possible in that case
220 #include "orca/common/Wrench.h"
bool dependsOnProblem() const
Definition: TaskBase.cc:217
void onResizedCallback(std::function< void(void)> cb)
Definition: TaskBase.cc:463
Definition: TaskBase.h:74
Represents a set of parameters that can be loaded from a YAML file.
Definition: ConfigurableOrcaObject.h:14
virtual void print() const
Definition: TaskBase.cc:116
virtual void onActivated()
Definition: TaskBase.h:155
virtual void onActivation()
Definition: TaskBase.h:154
optim::ControlVariable getControlVariable() const
Definition: TaskBase.cc:280
bool isActivated() const
Definition: TaskBase.cc:106
double getRampDuration() const
Definition: TaskBase.cc:160
double getCurrentRampValue() const
Definition: TaskBase.cc:143
virtual bool rampDown(double time_since_stop)
Definition: TaskBase.cc:245
void onDeactivationCallback(std::function< void(void)> cb)
Definition: TaskBase.cc:476
virtual bool activate()
Definition: TaskBase.cc:308
bool hasWrench() const
Definition: TaskBase.cc:597
std::shared_ptr< Wrench > wrench()
Definition: TaskBase.cc:297
const std::string & getParentName() const
Returns the parent name if it exists, empty otherwise.
Definition: TaskBase.cc:60
virtual void onResized()
Definition: TaskBase.h:153
Definition: Mutex.h:66
void onDeactivatedCallback(std::function< void(void)> cb)
Definition: TaskBase.cc:482
Definition: TaskBase.h:73
void onActivationCallback(std::function< void(void)> cb)
Definition: TaskBase.cc:445
bool hasChildren() const
Returns true if the task has children.
Definition: TaskBase.cc:50
virtual bool deactivate()
Definition: TaskBase.cc:488
void onComputeEndCallback(std::function< void(double, double)> cb)
Definition: TaskBase.cc:470
void onComputeBeginCallback(std::function< void(double, double)> cb)
Definition: TaskBase.cc:457
bool dependsOnWrench() const
Definition: TaskBase.cc:211
const std::string & getPrintableName() const
Returns the name that include parent name if present.
Definition: TaskBase.cc:65
std::shared_ptr< RobotModel > Ptr
Definition: RobotModel.h:111
std::shared_ptr< const optim::Problem > getProblem() const
Definition: TaskBase.cc:554
virtual void resize()
Definition: TaskBase.cc:250
inline::std::ostream & operator<<(::std::ostream &os, const TaskBase::State &s)
Definition: TaskBase.h:199
bool setRobotModel(robot::RobotModel::Ptr robot)
Definition: TaskBase.cc:180
bool dependsOnRobotJoints() const
Definition: TaskBase.cc:224
double getStopTime() const
Definition: TaskBase.cc:170
virtual void onCompute(double current_time, double dt)=0
State getState() const
Definition: TaskBase.cc:175
virtual void update(double current_time, double dt)
Definition: TaskBase.cc:331
virtual ~TaskBase()
Definition: TaskBase.cc:47
TaskBase(const std::string &name, optim::ControlVariable control_var)
Definition: TaskBase.cc:21
robot::RobotModel::Ptr robot()
Definition: TaskBase.cc:285
virtual void onDeactivation()
Definition: TaskBase.h:160
bool hasProblem() const
Definition: TaskBase.cc:587
Definition: TaskBase.h:69
bool isRobotInitialized() const
Definition: TaskBase.cc:133
void setRampDuration(double ramp_time)
Definition: TaskBase.cc:148
std::shared_ptr< const robot::RobotModel > getRobot() const
Definition: TaskBase.cc:291
void setRampValue(double new_val)
Definition: TaskBase.cc:138
double getStartTime() const
Definition: TaskBase.cc:165
bool hasRobot() const
Definition: TaskBase.cc:592
Definition: CartesianAccelerationPID.h:44
Definition: TaskBase.h:72
ControlVariable
Definition: ControlVariable.h:44
virtual void onDeactivated()
Definition: TaskBase.h:161
Definition: TaskBase.h:68
void addChild(TaskBase::Ptr e)
Add a child/slave task that will be updated BEFORE the parent task.
Definition: TaskBase.cc:89
bool dependsOnFloatingBase() const
Definition: TaskBase.cc:231
State
Represents the internal state of the task.
Definition: TaskBase.h:67
void onActivatedCallback(std::function< void(void)> cb)
Definition: TaskBase.cc:451
Definition: TaskBase.h:70
bool hasParent() const
Returns true if the task owned by a parent task.
Definition: TaskBase.cc:55
Definition: Problem.h:73
orca::common::MutexRecursive mutex
The recursive mutex that is locked during the update function. It is up to the external user to lock ...
Definition: TaskBase.h:146
std::shared_ptr< const common::Wrench > getWrench() const
Definition: TaskBase.cc:602
virtual bool setProblem(std::shared_ptr< const orca::optim::Problem > problem)
Definition: TaskBase.cc:511
The common base class for tasks and constraints.
Definition: TaskBase.h:59
virtual void onResize()=0
bool isComputing() const
Definition: TaskBase.cc:111
std::shared_ptr< TaskBase > Ptr
Definition: TaskBase.h:63
virtual bool rampUp(double time_since_start)
Definition: TaskBase.cc:240
Definition: TaskBase.h:71