ORCA: Optimization-based framework for Robotic Control Applications
WeightedEuclidianNormFunction.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 
40 #include "orca/utils/Utils.h"
42 
43 namespace orca
44 {
45 namespace math
46 {
47 
49 {
50 public:
53 
55  {
56  public:
57  virtual ~QuadraticCost() {}
58 
59  void resize(int A_or_b_rows);
60 
61  Size getSize() const;
62 
63  int rows() const;
64 
65  int cols() const;
66 
67  void computeQuadraticCost(const Eigen::VectorXd& SelectionVector
68  , const Eigen::MatrixXd& Weight
69  , const Eigen::MatrixXd& A
70  , const Eigen::VectorXd& b);
71 
72  void computeHessian(const Eigen::VectorXd& SelectionVector
73  , const Eigen::MatrixXd& Weight
74  , const Eigen::MatrixXd& A);
75 
76  void computeGradient(const Eigen::VectorXd& SelectionVector
77  , const Eigen::MatrixXd& Weight
78  , const Eigen::MatrixXd& A
79  , const Eigen::VectorXd& b);
80 
81  const Eigen::MatrixXd& getHessian() const;
82  const Eigen::VectorXd& getGradient() const;
83 
84  private:
85  Eigen::MatrixXd Hessian_;
86  Eigen::VectorXd Gradient_;
87  };
88 
89  void print() const;
90 
91  void setWeight(const Eigen::MatrixXd& weight);
92 
93  void setWeight(double weight);
94 
95  void computeQuadraticCost();
96 
97  const Eigen::VectorXd& getSelectionVector() const;
98 
99  void setSelectionVector(const Eigen::VectorXd& s);
100 
101  const Eigen::MatrixXd& getWeight() const;
102 
103  const QuadraticCost& getQuadraticCost() const;
104 
105  void resize(int rows,int cols);
106 
107 private:
108  Eigen::MatrixXd Weight_;
109  Eigen::VectorXd SelectionVector_;
110  QuadraticCost quadCost_;
111 };
112 
113 }
114 }
int rows() const
Definition: WeightedEuclidianNormFunction.cc:21
void setWeight(const Eigen::MatrixXd &weight)
Definition: WeightedEuclidianNormFunction.cc:79
int cols() const
Definition: WeightedEuclidianNormFunction.cc:26
Definition: WeightedEuclidianNormFunction.h:48
Eigen::MatrixXd & A()
Definition: AffineFunction.cc:52
Definition: AffineFunction.h:47
WeightedEuclidianNormFunction()
Definition: WeightedEuclidianNormFunction.cc:5
const Eigen::MatrixXd & getHessian() const
Definition: WeightedEuclidianNormFunction.cc:55
Eigen::VectorXd & b()
Definition: AffineFunction.cc:57
virtual ~WeightedEuclidianNormFunction()
Definition: WeightedEuclidianNormFunction.h:52
void setSelectionVector(const Eigen::VectorXd &s)
Definition: WeightedEuclidianNormFunction.cc:101
void computeHessian(const Eigen::VectorXd &SelectionVector, const Eigen::MatrixXd &Weight, const Eigen::MatrixXd &A)
Definition: WeightedEuclidianNormFunction.cc:40
Size getSize() const
Definition: WeightedEuclidianNormFunction.cc:16
void computeQuadraticCost(const Eigen::VectorXd &SelectionVector, const Eigen::MatrixXd &Weight, const Eigen::MatrixXd &A, const Eigen::VectorXd &b)
Definition: WeightedEuclidianNormFunction.cc:31
const Eigen::MatrixXd & getWeight() const
Definition: WeightedEuclidianNormFunction.cc:107
const Eigen::VectorXd & getSelectionVector() const
Definition: WeightedEuclidianNormFunction.cc:96
virtual ~QuadraticCost()
Definition: WeightedEuclidianNormFunction.h:57
void computeGradient(const Eigen::VectorXd &SelectionVector, const Eigen::MatrixXd &Weight, const Eigen::MatrixXd &A, const Eigen::VectorXd &b)
Definition: WeightedEuclidianNormFunction.cc:47
Definition: CartesianAccelerationPID.h:44
void resize(int A_or_b_rows)
Definition: WeightedEuclidianNormFunction.cc:10
Definition: WeightedEuclidianNormFunction.h:54
void print() const
Definition: WeightedEuclidianNormFunction.cc:65
Definition: Utils.h:105
const Eigen::VectorXd & getGradient() const
Definition: WeightedEuclidianNormFunction.cc:60
const QuadraticCost & getQuadraticCost() const
Definition: WeightedEuclidianNormFunction.cc:112