final_state_constraints.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #ifndef SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_CONSTRAINTS_H_
26 #define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_CONSTRAINTS_H_
27 
29 
33 
34 #include <memory>
35 
36 namespace corbo {
37 
39 {
40  public:
41  using Ptr = std::shared_ptr<TerminalBall>;
42  using ConstPtr = std::shared_ptr<const TerminalBall>;
43 
44  TerminalBall() = default;
45 
47  {
48  setWeightS(S);
49  setGamma(gamma);
50  }
51 
52  FinalStageConstraint::Ptr getInstance() const override { return std::make_shared<TerminalBall>(); }
53 
54  bool isEqualityConstraint() const override { return false; }
55 
56  int getNonIntegralStateTermDimension(int k) const override { return 1; }
57 
60  const Eigen::MatrixXd& getWeightS() const { return _S; }
61 
62  void setGamma(double gamma) { _gamma = gamma; }
63  double getGamma() { return _gamma; }
64 
66 
68  bool single_dt, const Eigen::VectorXd& x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor,
69  const std::vector<double>& dts, const DiscretizationGridInterface* /*grid*/) override
70  {
71  _x_ref = &xref;
72 
74 
75  return false;
76  }
77 
78  bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream* issues) const override;
79 
80 #ifdef MESSAGE_SUPPORT
81  bool fromMessage(const messages::FinalStageConstraints& message, std::stringstream* issues) override;
82  void toMessage(messages::FinalStageConstraints& message) const override;
83 #endif
84 
85  protected:
86  Eigen::MatrixXd _S;
88  double _gamma = 0.0;
89 
91  bool _zero_x_ref = false;
92 
93  bool _diagonal_mode = false;
95 };
97 
99 {
100  public:
101  using Ptr = std::shared_ptr<TerminalBallInheritFromCost>;
102  using ConstPtr = std::shared_ptr<const TerminalBallInheritFromCost>;
103 
104  TerminalBallInheritFromCost() = default;
105 
106  FinalStageConstraint::Ptr getInstance() const override { return std::make_shared<TerminalBallInheritFromCost>(); }
107 
108  int getNonIntegralStateTermDimension(int k) const override { return 1; }
109 
110  void setWeightS(const Eigen::Ref<const Eigen::MatrixXd>& S) = delete;
111  bool setWeightS(const Eigen::DiagonalMatrix<double, -1>& S) = delete;
112 
114  bool single_dt, const Eigen::VectorXd& x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor,
115  const std::vector<double>& dts, const DiscretizationGridInterface* /*grid*/) override;
116 
118 
119  bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream* issues) const override;
120 
121 #ifdef MESSAGE_SUPPORT
122  bool fromMessage(const messages::FinalStageConstraints& message, std::stringstream* issues) override;
123  void toMessage(messages::FinalStageConstraints& message) const override;
124 #endif
125 
126  double _gamma = 0.0;
127 };
129 
131 {
132  public:
133  using Ptr = std::shared_ptr<TerminalEqualityConstraint>;
134  using ConstPtr = std::shared_ptr<const TerminalEqualityConstraint>;
135 
136  TerminalEqualityConstraint() = default;
137 
139 
140  FinalStageConstraint::Ptr getInstance() const override { return std::make_shared<TerminalEqualityConstraint>(); }
141 
142  bool isEqualityConstraint() const override { return true; }
143 
144  int getNonIntegralStateTermDimension(int k) const override { return _xref.size(); }
145 
146  void setXRef(const Eigen::Ref<const Eigen::VectorXd>& xref) { _xref = xref; }
147  const Eigen::VectorXd& getXRef() const { return _xref; }
148 
150  {
151  assert(x_k.size() == _xref.size());
152  assert(cost.size() == _xref.size());
153  cost = x_k - _xref;
154  }
155 
157  bool single_dt, const Eigen::VectorXd& x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor,
158  const std::vector<double>& dts, const DiscretizationGridInterface* /*grid*/) override
159  {
160  return false;
161  }
162 
163  bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream* issues) const override
164  {
165  if (state_dim != _xref.size())
166  {
167  if (issues)
168  *issues << "TerminalEqualityConstraint: Dimension of xref (" << _xref.size() << ") does not coincide with state dimension ("
169  << state_dim << ")." << std::endl;
170  }
171  return true;
172  }
173 
174 #ifdef MESSAGE_SUPPORT
175  bool fromMessage(const messages::TerminalEqualityConstraint& message, std::stringstream* issues)
176  {
177  _xref = Eigen::Map<const Eigen::VectorXd>(message.x_ref().data(), message.x_ref_size());
178  return true;
179  }
180  void toMessage(messages::TerminalEqualityConstraint& message) const
181  {
182  message.mutable_x_ref()->Resize(_xref.size(), 0);
183  Eigen::Map<Eigen::VectorXd>(message.mutable_x_ref()->mutable_data(), _xref.size()) = _xref;
184  }
185 
186  bool fromMessage(const messages::FinalStageConstraints& message, std::stringstream* issues) override
187  {
188  return fromMessage(message.terminal_equality_constraint(), issues);
189  }
190  void toMessage(messages::FinalStageConstraints& message) const override { toMessage(*message.mutable_terminal_equality_constraint()); }
191 #endif
192 
193  protected:
194  Eigen::VectorXd _xref;
195 };
197 
199 {
200  public:
201  using Ptr = std::shared_ptr<TerminalPartialEqualityConstraint>;
202  using ConstPtr = std::shared_ptr<const TerminalPartialEqualityConstraint>;
203 
206  {
207  setActiveComponents(active_components);
208  }
209 
210  FinalStageConstraint::Ptr getInstance() const override { return std::make_shared<TerminalPartialEqualityConstraint>(); }
211 
212  bool isEqualityConstraint() const override { return true; }
213 
214  int getNonIntegralStateTermDimension(int k) const override { return _num_active; }
215 
216  void setActiveComponents(const Eigen::Ref<const Eigen::Matrix<bool, -1, 1>>& active_components)
217  {
218  _active_components = active_components;
219  _num_active = _active_components.count();
220  }
221 
223  {
224  _xref = xref;
225  setActiveComponents(xfixed);
226  }
227  const Eigen::VectorXd& getXRef() const { return _xref; }
228  const Eigen::Matrix<bool, -1, 1>& getXFixed() const { return _active_components; }
229 
231  {
232  assert(x_k.size() == _xref.size());
233  assert(cost.size() == _num_active);
234  assert(_active_components.size() == _xref.size());
235  assert(_num_active == _active_components.count());
236  int idx = 0;
237  for (int i = 0; i < _active_components.size(); ++i)
238  {
239  if (_active_components[i])
240  {
241  cost[idx] = x_k[i] - _xref[i];
242  ++idx;
243  }
244  }
245  assert(idx == _num_active);
246  }
247 
249  bool single_dt, const Eigen::VectorXd& x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor,
250  const std::vector<double>& dts, const DiscretizationGridInterface* /*grid*/) override
251  {
252  return false;
253  }
254 
255  bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream* issues) const override
256  {
257  if (state_dim != _xref.size())
258  {
259  if (issues)
260  *issues << "TerminalEqualityConstraint: Dimension of xref (" << _xref.size() << ") does not coincide with state dimension ("
261  << state_dim << ")." << std::endl;
262  }
263  if (state_dim != _active_components.size())
264  {
265  if (issues)
266  *issues << "TerminalEqualityConstraint: Dimension of active_components (" << _active_components.size()
267  << ") does not coincide with state dimension (" << state_dim << ")." << std::endl;
268  }
269  return true;
270  }
271 
272 #ifdef MESSAGE_SUPPORT
273  bool fromMessage(const messages::TerminalPartialEqualityConstraint& message, std::stringstream* issues)
274  {
275  _xref = Eigen::Map<const Eigen::VectorXd>(message.x_ref().data(), message.x_ref_size());
276 
277  setActiveComponents(Eigen::Map<const Eigen::Matrix<bool, -1, 1>>(message.active_components().data(), message.active_components_size()));
278  return true;
279  }
280  void toMessage(messages::TerminalPartialEqualityConstraint& message) const
281  {
282  message.mutable_x_ref()->Resize(_xref.size(), 0);
283  Eigen::Map<Eigen::VectorXd>(message.mutable_x_ref()->mutable_data(), _xref.size()) = _xref;
284 
285  // xf fixed states
286  if (_active_components.size() > 0)
287  {
288  message.mutable_active_components()->Resize(_active_components.size(), false);
289  Eigen::Map<Eigen::Matrix<bool, -1, 1>>(message.mutable_active_components()->mutable_data(), _active_components.size()) =
290  _active_components;
291  }
292  }
293 
294  bool fromMessage(const messages::FinalStageConstraints& message, std::stringstream* issues) override
295  {
296  return fromMessage(message.terminal_partial_eq_constr(), issues);
297  }
298  void toMessage(messages::FinalStageConstraints& message) const override { toMessage(*message.mutable_terminal_partial_eq_constr()); }
299 #endif
300 
301  protected:
303  int _num_active = 0;
304 };
306 
307 } // namespace corbo
308 
309 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_CONSTRAINTS_H_
const ReferenceTrajectoryInterface * _x_ref
bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream *issues) const override
const Eigen::Matrix< bool, -1, 1 > & getXFixed() const
int getNonIntegralStateTermDimension(int k) const override
#define FACTORY_REGISTER_FINAL_STAGE_CONSTRAINT(type)
int getNonIntegralStateTermDimension(int k) const override
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream *issues) const override
Generic interface class for discretization grids.
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
std::shared_ptr< const FinalStageCost > ConstPtr
std::shared_ptr< const TerminalEqualityConstraint > ConstPtr
std::shared_ptr< const TerminalBall > ConstPtr
void setGamma(double gamma)
TerminalPartialEqualityConstraint(const Eigen::Ref< const Eigen::Matrix< bool, -1, 1 >> &active_components)
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *) override
const Eigen::VectorXd & getXRef() const
TerminalBall()=default
std::shared_ptr< FinalStageConstraint > Ptr
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
std::shared_ptr< TerminalBall > Ptr
int getNonIntegralStateTermDimension(int k) const override
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *) override
bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream *issues) const override
TerminalEqualityConstraint(const Eigen::Ref< const Eigen::VectorXd > &xref)
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
bool isEqualityConstraint() const override
void setXRef(const Eigen::Ref< const Eigen::VectorXd > &xref)
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *) override
int getNonIntegralStateTermDimension(int k) const override
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
void setActiveComponents(const Eigen::Ref< const Eigen::Matrix< bool, -1, 1 >> &active_components)
Interface class for reference trajectories.
void setXRef(const Eigen::Ref< const Eigen::VectorXd > &xref, const Eigen::Ref< const Eigen::Matrix< bool, -1, 1 >> &xfixed)
const Eigen::MatrixXd & getWeightS() const
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
std::shared_ptr< StagePreprocessor > Ptr
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
bool setWeightS(const Eigen::Ref< const Eigen::MatrixXd > &S)
TerminalBall(const Eigen::Ref< const Eigen::MatrixXd > &S, double gamma)
std::shared_ptr< TerminalEqualityConstraint > Ptr
Eigen::DiagonalMatrix< double, -1 > _S_diag
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override


control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:06:51