ORCA: Optimization-based framework for Robotic Control Applications
ProblemData.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/math/Utils.h"
39 #include <iostream>
40 
41 namespace orca
42 {
43 namespace optim
44 {
45 
47 {
48  void resize(int nvar, int nconstr)
49  {
50  H_.conservativeResizeLike(Eigen::MatrixXd::Zero(nvar,nvar));
51  g_.conservativeResizeLike(Eigen::VectorXd::Zero(nvar));
52 
53  A_.conservativeResizeLike(Eigen::MatrixXd::Zero(nconstr,nvar));
54 
55  lbA_.conservativeResizeLike( Eigen::VectorXd::Constant(nconstr, - math::Infinity) );
56  lb_.conservativeResizeLike( Eigen::VectorXd::Constant(nvar, - math::Infinity) );
57 
58  ubA_.conservativeResizeLike( Eigen::VectorXd::Constant(nconstr, math::Infinity) );
59  ub_.conservativeResizeLike( Eigen::VectorXd::Constant(nvar, math::Infinity) );
60 
61  primal_solution_.conservativeResizeLike(Eigen::VectorXd::Zero(nvar));
62  }
63 
64  void reset()
65  {
66  H_.setZero();
67  g_.setZero();
68  lb_.setConstant( - math::Infinity );
69  ub_.setConstant( math::Infinity );
70  A_.setZero();
71  lbA_.setConstant( - math::Infinity );
72  ubA_.setConstant( math::Infinity );
73  primal_solution_.setZero();
74  }
75 
76  void print() const
77  {
78  std::cout << "=========================================================================================================================================" << std::endl;
79  std::cout << "H" << std::endl;
80  std::cout << H_ << "\n\n";
81  std::cout << "g" << std::endl;
82  std::cout << g_ << "\n\n";
83  std::cout << "A" << std::endl;
84  std::cout << A_ << "\n\n";
85  std::cout << "lbA" << std::endl;
86  std::cout << lbA_ << "\n\n";
87  std::cout << "ubA" << std::endl;
88  std::cout << ubA_ << "\n\n";
89  std::cout << "lb" << std::endl;
90  std::cout << lb_ << "\n\n";
91  std::cout << "ub" << std::endl;
92  std::cout << ub_ << "\n\n";
93  std::cout << " - Nvars (Hrows) : " << H_.rows() << std::endl;
94  std::cout << " - NConstr (Arows) : " << A_.rows() << std::endl;
95  std::cout << "=========================================================================================================================================" << std::endl;
96  }
97 
98  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> H_;
99  Eigen::VectorXd g_;
100  Eigen::VectorXd lb_;
101  Eigen::VectorXd ub_;
102  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A_;
103  Eigen::VectorXd lbA_;
104  Eigen::VectorXd ubA_;
105  Eigen::VectorXd primal_solution_;
106 };
107 
108 } // namespace optim
109 } // namespace orca
Eigen::VectorXd ub_
Definition: ProblemData.h:101
Eigen::VectorXd lb_
Definition: ProblemData.h:100
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > A_
Definition: ProblemData.h:102
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > H_
Definition: ProblemData.h:98
void reset()
Definition: ProblemData.h:64
void print() const
Definition: ProblemData.h:76
Eigen::VectorXd lbA_
Definition: ProblemData.h:103
Definition: CartesianAccelerationPID.h:44
void resize(int nvar, int nconstr)
Definition: ProblemData.h:48
Definition: ProblemData.h:46
static const double Infinity
Definition: Utils.h:48
Eigen::VectorXd g_
Definition: ProblemData.h:99
Eigen::VectorXd primal_solution_
Definition: ProblemData.h:105
Eigen::VectorXd ubA_
Definition: ProblemData.h:104