ORCA: Optimization-based framework for Robotic Control Applications
Controller.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 #pragma once
38 #include "orca/utils/Utils.h"
39 #include "orca/common/Mutex.h"
40 #include "orca/utils/Logger.h"
41 #include "orca/task/GenericTask.h"
43 #include "orca/optim/Problem.h"
45 #include "orca/optim/QPSolver.h"
46 #include "orca/robot/RobotModel.h"
50 
51 namespace orca
52 {
53 namespace optim
54 {
56 {
57 public:
58  Controller(const std::string& name);
59  Controller(const std::string& name
60  , std::shared_ptr<robot::RobotModel> robot
61  ,ResolutionStrategy resolution_strategy
62  ,QPSolverImplType solver_type);
63 
64  void print() const;
65 
66  void setPrintLevel(int level);
67 
68  std::shared_ptr<robot::RobotModel> robot();
69 
70  void setRobotModel(std::shared_ptr<robot::RobotModel> robot);
71 
72  bool update(double current_time, double dt);
73 
75 
76  bool addTask(std::shared_ptr<task::GenericTask> task);
77 
78  bool addTaskFromString(const std::string& task_description);
79 
80  bool addConstraintFromString(const std::string& task_description);
81 
82  template<class T>
83  std::shared_ptr<T> addTask(const std::string& name)
84  {
85  auto t = std::make_shared<T>(name);
86  if(this->addTask(t))
87  return t;
88  return nullptr;
89  }
90 
91  std::shared_ptr<task::GenericTask> getTask(const std::string& name, int level = 0);
92  std::shared_ptr<task::GenericTask> getTask(unsigned int index, int level = 0);
93 
94  bool addConstraint(std::shared_ptr<constraint::GenericConstraint> cstr);
95 
96  template<class C>
97  std::shared_ptr<C> addConstraint(const std::string& name)
98  {
99  auto c = std::make_shared<C>(name);
100  if(this->addConstraint(c))
101  return c;
102  return nullptr;
103  }
104 
105  bool solutionFound() const;
106 
107  const Eigen::VectorXd& getSolution();
108 
109  const Eigen::VectorXd& getJointTorqueCommand(bool remove_gravity_torques = false
110  , bool remove_coriolis_torques = false);
111 
112  const Eigen::VectorXd& computeKKTTorques();
113 
114  const Eigen::VectorXd& getJointAccelerationCommand();
115 
117  void activateTasks();
118  void activateConstraints();
119 
121  void deactivateTasks();
122  void deactivateConstraints();
123 
125 
126  std::shared_ptr<task::RegularisationTask<ControlVariable::X> > globalRegularization(int level = 0);
127  void setUpdateCallback(std::function<void(double,double)> update_cb);
128 
129  void removeGravityTorquesFromSolution(bool do_remove);
130  void removeCoriolisTorquesFromSolution(bool do_remove);
136 
138 private:
139  bool isProblemDry(std::shared_ptr<const optim::Problem> problem);
140  std::shared_ptr<Problem> getProblemAtLevel(int level);
141  void insertNewLevel();
142  void updateTasks(double current_time, double dt);
143  void updateConstraints(double current_time, double dt);
144 private:
147  common::Parameter<bool> remove_gravity_torques_ = false;
148  common::Parameter<bool> remove_coriolis_torques_ = true;
152 private:
153  std::function<void(double,double)> update_cb_;
154 
155  std::list< Problem::Ptr > problems_;
156 
157  Eigen::VectorXd joint_torque_command_;
158  Eigen::VectorXd kkt_torques_;
159  Eigen::VectorXd joint_acceleration_command_;
160  Eigen::VectorXd __fix_warnings__;
161 
162  bool solution_found_ = false;
163 };
164 } // namespace optim
165 } //namespace orca
void deactivateTasksAndConstraints()
Definition: Controller.cc:348
Represents a set of parameters that can be loaded from a YAML file.
Definition: ConfigurableOrcaObject.h:14
void setUpdateCallback(std::function< void(double, double)> update_cb)
Definition: Controller.cc:92
ReturnCode
Definition: ReturnCode.h:45
void activateTasks()
Definition: Controller.cc:380
bool solutionFound() const
Definition: Controller.cc:148
Definition: Controller.h:55
void removeCoriolisTorquesFromSolution(bool do_remove)
Definition: Controller.cc:428
Definition: Mutex.h:66
void activateConstraints()
Definition: Controller.cc:391
void deactivateConstraints()
Definition: Controller.cc:367
common::MutexRecursive mutex
The recursive mutex to protect the update function.
Definition: Controller.h:135
std::shared_ptr< task::GenericTask > getTask(const std::string &name, int level=0)
Definition: Controller.cc:225
const Eigen::VectorXd & getJointTorqueCommand(bool remove_gravity_torques=false, bool remove_coriolis_torques=false)
Definition: Controller.cc:280
QPSolverImplType
Definition: QPSolverImplType.h:44
std::shared_ptr< C > addConstraint(const std::string &name)
Definition: Controller.h:97
std::shared_ptr< task::RegularisationTask< ControlVariable::X > > globalRegularization(int level=0)
Definition: Controller.cc:446
std::shared_ptr< T > addTask(const std::string &name)
Definition: Controller.h:83
ResolutionStrategy getResolutionStrategy() const
Definition: Controller.cc:153
bool addTaskFromString(const std::string &task_description)
Definition: Controller.cc:170
void deactivateTasks()
Definition: Controller.cc:354
const Eigen::VectorXd & getSolution()
Definition: Controller.cc:267
void removeGravityTorquesFromSolution(bool do_remove)
Definition: Controller.cc:424
void activateTasksAndConstraints()
Definition: Controller.cc:342
bool update(double current_time, double dt)
Definition: Controller.cc:122
bool tasksAndConstraintsDeactivated()
Definition: Controller.cc:402
Controller(const std::string &name)
Definition: Controller.cc:15
const Eigen::VectorXd & getJointAccelerationCommand()
Definition: Controller.cc:328
std::shared_ptr< robot::RobotModel > robot()
Definition: Controller.cc:78
bool addTask(std::shared_ptr< task::GenericTask > task)
Definition: Controller.cc:201
const Eigen::VectorXd & computeKKTTorques()
Definition: Controller.cc:322
ResolutionStrategy
Definition: ResolutionStrategy.h:44
Definition: CartesianAccelerationPID.h:44
bool addConstraintFromString(const std::string &task_description)
Definition: Controller.cc:185
void setRobotModel(std::shared_ptr< robot::RobotModel > robot)
Definition: Controller.cc:85
void setPrintLevel(int level)
Definition: Controller.cc:70
common::ReturnCode getReturnCode() const
Definition: Controller.cc:158
void print() const
Definition: Controller.cc:62
bool addConstraint(std::shared_ptr< constraint::GenericConstraint > cstr)
Definition: Controller.cc:246