ORCA: Optimization-based framework for Robotic Control Applications
Problem.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/common/ReturnCode.h"
39 #include "orca/utils/Utils.h"
42 #include "orca/optim/ProblemData.h"
43 #include "orca/optim/QPSolver.h"
44 #include "orca/robot/RobotModel.h"
45 
46 namespace orca
47 {
48  namespace common
49  {
50  class Wrench;
51  class TaskBase;
52  }
53  namespace task
54  {
55  class GenericTask;
56  class WrenchTask;
57  }
58  namespace constraint
59  {
60  class GenericConstraint;
61  }
62  namespace robot
63  {
64  class RobotModel;
65  }
66 }
67 
68 namespace orca
69 {
70 namespace optim
71 {
72 
73 class Problem
74 {
75 public:
76  using Ptr = std::shared_ptr<Problem>;
77 
78  Problem();
79 
80  virtual ~Problem();
81 
82  bool setRobotModel(std::shared_ptr<robot::RobotModel> robot);
83  bool setImplementationType(QPSolverImplType solver_type);
84 
85  void build();
86  bool solve();
87  common::ReturnCode getReturnCode() const;
88 
89  bool addConstraint(std::shared_ptr< constraint::GenericConstraint> cstr);
90  bool constraintExists(std::shared_ptr< constraint::GenericConstraint> cstr);
91  bool addTask(std::shared_ptr< task::GenericTask> task);
92  bool taskExists(std::shared_ptr< task::GenericTask> task);
93  const std::list< std::shared_ptr< const common::Wrench> >& getWrenches() const;
94  const std::list< std::shared_ptr< task::GenericTask> >& getTasks() const;
95  const std::list< std::shared_ptr< constraint::GenericConstraint> >& getConstraints() const;
96 
97  unsigned int getIndex(ControlVariable var) const;
98  unsigned int getSize(ControlVariable var) const;
99  unsigned int getTotalSize() const;
100  const std::map<ControlVariable, unsigned int >& getIndexMap() const;
101  const std::map<ControlVariable, unsigned int >& getSizeMap() const;
102  void print() const;
103  Eigen::VectorXd getSolution(ControlVariable var) const;
104  const Eigen::VectorXd& getSolution() const;
105  std::shared_ptr<QPSolver> qpSolver();
106 protected:
107  std::list< std::shared_ptr< const common::Wrench > > wrenches_;
108  std::list< std::shared_ptr< task::GenericTask > > tasks_;
109  std::list< std::shared_ptr< constraint::GenericConstraint > > constraints_;
110 
111 protected:
113 
114  std::shared_ptr<robot::RobotModel> robot_;
115  QPSolver::Ptr qpsolver_ = std::make_shared<QPSolver>();
116 
118  unsigned int number_of_variables_ = 0;
119  unsigned int number_of_constraints_rows_ = 0;
120 private:
121  void resize();
122  bool addWrench(std::shared_ptr< const common::Wrench > wrench);
123  unsigned int computeNumberOfConstraintRows() const;
124  unsigned int generateVariableMapping();
125  void resizeProblemData(int nvar, int nconstr);
126  void resizeSolver(int nvar,int nconstr);
127 };
128 
129 } // namespace optim
130 } // namespace orca
ReturnCode
Definition: ReturnCode.h:45
ControlVariableMapping mapping_
Definition: Problem.h:112
std::shared_ptr< Problem > Ptr
Definition: Problem.h:76
Definition: GenericTask.h:50
QPSolverImplType
Definition: QPSolverImplType.h:44
std::shared_ptr< robot::RobotModel > robot_
Definition: Problem.h:114
Definition: ControlVariableMapping.h:48
ProblemData data_
Definition: Problem.h:117
The robot model class allow to make kinematics and dynamics computations.
Definition: RobotModel.h:108
std::shared_ptr< QPSolver > Ptr
Definition: QPSolver.h:54
std::list< std::shared_ptr< constraint::GenericConstraint > > constraints_
Definition: Problem.h:109
Definition: CartesianAccelerationPID.h:44
ControlVariable
Definition: ControlVariable.h:44
Definition: ProblemData.h:46
Definition: Problem.h:73
std::list< std::shared_ptr< const common::Wrench > > wrenches_
Definition: Problem.h:107
Definition: WrenchTask.h:48
std::list< std::shared_ptr< task::GenericTask > > tasks_
Definition: Problem.h:108