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:
52 
54  {
55  public:
56  void resize(int A_or_b_rows);
57 
58  Size getSize() const;
59 
60  int rows() const;
61 
62  int cols() const;
63 
64  void computeQuadraticCost(const Eigen::VectorXd& SelectionVector
65  , const Eigen::MatrixXd& Weight
66  , const Eigen::MatrixXd& A
67  , const Eigen::VectorXd& b);
68 
69  void computeHessian(const Eigen::VectorXd& SelectionVector
70  , const Eigen::MatrixXd& Weight
71  , const Eigen::MatrixXd& A);
72 
73  void computeGradient(const Eigen::VectorXd& SelectionVector
74  , const Eigen::MatrixXd& Weight
75  , const Eigen::MatrixXd& A
76  , const Eigen::VectorXd& b);
77 
78  const Eigen::MatrixXd& getHessian() const;
79  const Eigen::VectorXd& getGradient() const;
80 
81  private:
82  Eigen::MatrixXd Hessian_;
83  Eigen::VectorXd Gradient_;
84  };
85 
86  void print() const;
87 
88  void setWeight(const Eigen::MatrixXd& weight);
89 
90  void setWeight(double weight);
91 
92  void computeQuadraticCost();
93 
94  const Eigen::VectorXd& getSelectionVector() const;
95 
96  void setSelectionVector(const Eigen::VectorXd& s);
97 
98  const Eigen::MatrixXd& getWeight() const;
99 
100  const QuadraticCost& getQuadraticCost() const;
101 
102  void resize(int rows,int cols);
103 
104 private:
105  Eigen::MatrixXd Weight_;
106  Eigen::VectorXd SelectionVector_;
107  QuadraticCost quadCost_;
108 };
109 
110 }
111 }
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
const Eigen::MatrixXd & getWeight() const
Definition: WeightedEuclidianNormFunction.cc:107
void print() const
Definition: WeightedEuclidianNormFunction.cc:65
Definition: AffineFunction.h:47
const Eigen::MatrixXd & getHessian() const
Definition: WeightedEuclidianNormFunction.cc:55
WeightedEuclidianNormFunction()
Definition: WeightedEuclidianNormFunction.cc:5
Eigen::VectorXd & b()
Definition: AffineFunction.cc:57
void setSelectionVector(const Eigen::VectorXd &s)
Definition: WeightedEuclidianNormFunction.cc:101
const QuadraticCost & getQuadraticCost() const
Definition: WeightedEuclidianNormFunction.cc:112
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
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:53
const Eigen::VectorXd & getGradient() const
Definition: WeightedEuclidianNormFunction.cc:60
int rows() const
Definition: WeightedEuclidianNormFunction.cc:21
Definition: Utils.h:94
const Eigen::VectorXd & getSelectionVector() const
Definition: WeightedEuclidianNormFunction.cc:96