ORCA: Optimization-based framework for Robotic Control Applications
RegularisationTask.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 
39 #include "orca/task/GenericTask.h"
40 
41 namespace orca
42 {
43 namespace task
44 {
45  template<optim::ControlVariable C> class RegularisationTask : public GenericTask
46  {
47  public:
48  using Ptr = std::shared_ptr<RegularisationTask<C> >;
49 
50  const double DefaultWeight = 1.e-4;
51 
52  RegularisationTask(const std::string& name)
53  : GenericTask(name,C)
54  {
55  this->setRampDuration(0);
56  this->setWeight(DefaultWeight);
57  }
58  protected:
59  void onActivation()
60  {}
61  void onUpdateAffineFunction(double current_time, double dt)
62  {}
64  {}
65  void onResize()
66  {
67  const int sizeofvar = this->getProblem()->getSize(this->getControlVariable());
68  int old_cols = euclidianNorm().cols();
69 
70  if(old_cols != sizeofvar)
71  {
72  euclidianNorm().resize(sizeofvar,sizeofvar);
73  euclidianNorm().A().setIdentity();
74  euclidianNorm().b().setZero();
76  }
77  }
78  };
79 
86 } // namespace task
87 } // namespace orca
88 
90 ORCA_REGISTER_CLASS(orca::task::RegularisationTask<orca::optim::ControlVariable::GeneralisedAcceleration>,1)
91 ORCA_REGISTER_CLASS(orca::task::RegularisationTask<orca::optim::ControlVariable::JointAcceleration>,2)
92 ORCA_REGISTER_CLASS(orca::task::RegularisationTask<orca::optim::ControlVariable::GeneralisedTorque>,3)
93 ORCA_REGISTER_CLASS(orca::task::RegularisationTask<orca::optim::ControlVariable::JointTorque>,4)
94 ORCA_REGISTER_CLASS(orca::task::RegularisationTask<orca::optim::ControlVariable::ExternalWrench>,5)
void onDeactivation()
Definition: RegularisationTask.h:63
void onUpdateAffineFunction(double current_time, double dt)
Definition: RegularisationTask.h:61
optim::ControlVariable getControlVariable() const
Definition: TaskBase.cc:280
void onActivation()
Definition: RegularisationTask.h:59
Eigen::MatrixXd & A()
Definition: AffineFunction.cc:52
RegularisationTask< optim::ControlVariable::JointTorque > JointTorqueRegularisationTask
Definition: RegularisationTask.h:83
void resize(int rows, int cols)
Definition: WeightedEuclidianNormFunction.cc:117
void computeQuadraticCost()
Definition: WeightedEuclidianNormFunction.cc:91
Definition: GenericTask.h:50
RegularisationTask< optim::ControlVariable::JointAcceleration > JointAccelerationRegularisationTask
Definition: RegularisationTask.h:81
int cols() const
Definition: AffineFunction.cc:67
std::shared_ptr< ConfigurableOrcaObject > Ptr
Definition: ConfigurableOrcaObject.h:17
RegularisationTask< optim::ControlVariable::ExternalWrench > WrenchRegularisationTask
Definition: RegularisationTask.h:84
std::shared_ptr< const optim::Problem > getProblem() const
Definition: TaskBase.cc:554
Eigen::VectorXd & b()
Definition: AffineFunction.cc:57
void onResize()
Definition: RegularisationTask.h:65
const double DefaultWeight
Definition: RegularisationTask.h:50
RegularisationTask< optim::ControlVariable::GeneralisedTorque > GeneralisedTorqueRegularisationTask
Definition: RegularisationTask.h:82
#define ORCA_REGISTER_CLASS(CLASSNAME,...)
Definition: Factory.h:74
void setWeight(double weight)
Definition: GenericTask.cc:36
void setRampDuration(double ramp_time)
Definition: TaskBase.cc:148
math::WeightedEuclidianNormFunction & euclidianNorm()
Definition: GenericTask.cc:61
Definition: CartesianAccelerationPID.h:44
ControlVariable
Definition: ControlVariable.h:44
RegularisationTask< optim::ControlVariable::X > GlobalRegularisationTask
Definition: RegularisationTask.h:85
Definition: RegularisationTask.h:45
RegularisationTask(const std::string &name)
Definition: RegularisationTask.h:52
RegularisationTask< optim::ControlVariable::GeneralisedAcceleration > GeneralisedAccelerationRegularisationTask
Definition: RegularisationTask.h:80