finite_differences_collocation_edges.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_STRUCTURED_OCP_EDGES_FINITE_DIFFERENCES_COLLOCATION_EDGES_H_
26 #define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_FINITE_DIFFERENCES_COLLOCATION_EDGES_H_
27 
29 
35 
36 #include <functional>
37 #include <memory>
38 
39 namespace corbo {
40 
41 // TODO(roesmann): if we combine StageCost / StageEqualtiy ... we can merge some of the edges below
42 
43 class FDCollocationEdge : public Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>
44 {
45  public:
46  using Ptr = std::shared_ptr<FDCollocationEdge>;
47  using UPtr = std::unique_ptr<FDCollocationEdge>;
48 
50  : Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>(x1, u1, x2, dt), _dynamics(dynamics)
51  {
52  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
53  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
54  _x2 = static_cast<const VectorVertex*>(_vertices[2]);
55  _dt = static_cast<const ScalarVertex*>(_vertices[3]);
56  }
57 
58  // implements interface method
59  int getDimension() const override
60  {
61  assert(_dynamics);
62  return _dynamics->getStateDimension();
63  }
64  // implement in child class:
65  bool isLinear() const override { return false; }
66 
67  // bool providesJacobian() const override { return false; }
68 
69  bool isLeastSquaresForm() const override { return false; }
70 
72  {
73  assert(_fd_eval);
74  assert(_dynamics);
75  assert(_x1->getDimension() == _dynamics->getStateDimension());
76  assert(_u1->getDimension() == _dynamics->getInputDimension());
77  assert(_x2->getDimension() == _dynamics->getStateDimension());
78 
79  _fd_eval->computeEqualityConstraint(_x1->values(), _u1->values(), _x2->values(), _dt->value(), *_dynamics, values);
80  }
81 
83 
84  private:
87 
88  const VectorVertex* _x1 = nullptr;
89  const VectorVertex* _u1 = nullptr;
90  const VectorVertex* _x2 = nullptr;
91  const ScalarVertex* _dt = nullptr;
92 
93  public:
95 };
96 
97 // TODO(roesmann): maybe a mixed edge for the uncompressed HS with linear control to compute uc just once.
98 class TrapezoidalIntegralCostEdge : public Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>
99 {
100  public:
101  using Ptr = std::shared_ptr<TrapezoidalIntegralCostEdge>;
102  using UPtr = std::unique_ptr<TrapezoidalIntegralCostEdge>;
103 
105  : Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>(x1, u1, x2, dt), _stage_cost(stage_cost), _k(k)
106  {
107  assert(stage_cost);
108  assert(stage_cost->getIntegralStateControlTermDimension(_k) == 1);
109 
110  _cost1.resize(1);
111  _cost2.resize(1);
112  }
113 
114  virtual ~TrapezoidalIntegralCostEdge() = default;
115 
116  // implements interface method
117  int getDimension() const override { return 1; }
118 
119  // implement in child class:
120  bool isLinear() const override { return false; }
121 
122  // bool providesJacobian() const override { return false; }
123  bool isLeastSquaresForm() const override { return false; }
124 
126  {
127  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
128  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
129  const VectorVertex* x2 = static_cast<const VectorVertex*>(_vertices[2]);
130  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[3]);
131 
132  _stage_cost->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), _cost1);
133  _stage_cost->computeIntegralStateControlTerm(_k, x2->values(), u1->values(), _cost2);
134  values[0] = 0.5 * dt->value() * (_cost1[0] + _cost2[0]);
135  }
136 
137  private:
139 
140  int _k = 0;
141 
142  Eigen::VectorXd _cost1;
143  Eigen::VectorXd _cost2;
144 
145  public:
147 };
148 
149 class TrapezoidalIntegralEqualityDynamicsEdge : public Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>
150 {
151  public:
152  using Ptr = std::shared_ptr<TrapezoidalIntegralEqualityDynamicsEdge>;
153  using UPtr = std::unique_ptr<TrapezoidalIntegralEqualityDynamicsEdge>;
154 
156  ScalarVertex& dt, StageEqualityConstraint::Ptr stage_eq, int k)
157  : Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>(x1, u1, x2, dt), _dynamics(dynamics), _stage_eq(stage_eq), _k(k)
158  {
159  _dim_dyn = _dynamics->getStateDimension();
160  _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
161  _dim_eq = _dim_dyn + _dim_int_eq;
162 
163  _eq1.resize(_dim_int_eq);
164  _eq2.resize(_dim_int_eq);
165  }
166 
167  virtual ~TrapezoidalIntegralEqualityDynamicsEdge() = default;
168 
169  // implements interface method
170  int getDimension() const override { return _dim_eq; }
171 
172  // implement in child class:
173  bool isLinear() const override { return false; }
174 
175  // bool providesJacobian() const override { return false; }
176  bool isLeastSquaresForm() const override { return false; }
177 
179  {
180  assert(_dynamics);
181  assert(_fd_eval);
182  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
183  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
184  const VectorVertex* x2 = static_cast<const VectorVertex*>(_vertices[2]);
185  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[3]);
186 
187  _fd_eval->computeEqualityConstraint(x1->values(), u1->values(), x2->values(), dt->value(), *_dynamics, values.head(_dim_dyn));
188 
189  if (_dim_int_eq > 0)
190  {
191  _stage_eq->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), _eq1);
192  _stage_eq->computeIntegralStateControlTerm(_k, x2->values(), u1->values(), _eq2);
193  values.tail(_dim_int_eq).noalias() = 0.5 * dt->value() * (_eq1 + _eq2);
194  }
195  }
196 
198 
199  private:
202 
204 
205  int _dim_eq = 0;
206  int _dim_int_eq = 0;
207  int _dim_dyn = 0;
208 
209  Eigen::VectorXd _eq1;
210  Eigen::VectorXd _eq2;
211 
212  int _k = 0;
213 
214  public:
216 };
217 
218 class TrapezoidalIntegralEqualityEdge : public Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>
219 {
220  public:
221  using Ptr = std::shared_ptr<TrapezoidalIntegralEqualityEdge>;
222  using UPtr = std::unique_ptr<TrapezoidalIntegralEqualityEdge>;
223 
225  StageEqualityConstraint::Ptr stage_eq, int k)
226  : Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>(x1, u1, x2, dt), _stage_eq(stage_eq), _k(k)
227  {
228  _dim_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
229 
230  _eq1.resize(_dim_eq);
231  _eq2.resize(_dim_eq);
232  }
233 
234  virtual ~TrapezoidalIntegralEqualityEdge() = default;
235 
236  // implements interface method
237  int getDimension() const override { return _dim_eq; }
238 
239  // implement in child class:
240  bool isLinear() const override { return false; }
241 
242  // bool providesJacobian() const override { return false; }
243  bool isLeastSquaresForm() const override { return false; }
244 
246  {
247  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
248  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
249  const VectorVertex* x2 = static_cast<const VectorVertex*>(_vertices[2]);
250  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[3]);
251 
252  _stage_eq->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), _eq1);
253  _stage_eq->computeIntegralStateControlTerm(_k, x2->values(), u1->values(), _eq2);
254  values.noalias() = 0.5 * dt->value() * (_eq1 + _eq2);
255  }
256 
257  private:
259 
260  int _dim_eq = 0;
261 
262  Eigen::VectorXd _eq1;
263  Eigen::VectorXd _eq2;
264 
265  int _k = 0;
266 
267  public:
269 };
270 
271 class TrapezoidalIntegralInequalityEdge : public Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>
272 {
273  public:
274  using Ptr = std::shared_ptr<TrapezoidalIntegralInequalityEdge>;
275  using UPtr = std::unique_ptr<TrapezoidalIntegralInequalityEdge>;
276 
278  StageInequalityConstraint::Ptr stage_ineq, int k)
279  : Edge<VectorVertex, VectorVertex, VectorVertex, ScalarVertex>(x1, u1, x2, dt), _stage_ineq(stage_ineq), _k(k)
280  {
281  _dim_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
282 
283  _ineq1.resize(_dim_ineq);
284  _ineq2.resize(_dim_ineq);
285  }
286 
287  virtual ~TrapezoidalIntegralInequalityEdge() = default;
288 
289  // implements interface method
290  int getDimension() const override { return _dim_ineq; }
291 
292  // implement in child class:
293  bool isLinear() const override { return false; }
294 
295  // bool providesJacobian() const override { return false; }
296 
297  bool isLeastSquaresForm() const override { return false; }
298 
300  {
301  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
302  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
303  const VectorVertex* x2 = static_cast<const VectorVertex*>(_vertices[2]);
304  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[3]);
305 
306  _stage_ineq->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), _ineq1);
307  _stage_ineq->computeIntegralStateControlTerm(_k, x2->values(), u1->values(), _ineq2);
308  values.noalias() = 0.5 * dt->value() * (_ineq1 + _ineq2);
309  }
310 
311  private:
313 
314  int _dim_ineq = 0;
315 
316  Eigen::VectorXd _ineq1;
317  Eigen::VectorXd _ineq2;
318 
319  int _k = 0;
320 
321  public:
323 };
324 
325 class LeftSumCostEdge : public Edge<VectorVertex, VectorVertex, ScalarVertex>
326 {
327  public:
328  using Ptr = std::shared_ptr<LeftSumCostEdge>;
329  using UPtr = std::unique_ptr<LeftSumCostEdge>;
330 
331  explicit LeftSumCostEdge(VectorVertex& x1, VectorVertex& u1, ScalarVertex& dt, StageCost::Ptr stage_cost, int k)
332  : Edge<VectorVertex, VectorVertex, ScalarVertex>(x1, u1, dt), _stage_cost(stage_cost), _k(k)
333  {
334  assert(stage_cost);
335  assert(stage_cost->getIntegralStateControlTermDimension(_k) == 1);
336  }
337 
338  virtual ~LeftSumCostEdge() = default;
339 
340  // implements interface method
341  int getDimension() const override { return 1; }
342 
343  // implement in child class:
344  bool isLinear() const override { return false; }
345 
346  // bool providesJacobian() const override { return false; }
347  bool isLeastSquaresForm() const override { return false; }
348 
350  {
351  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
352  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
353  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[2]);
354 
355  _stage_cost->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), values);
356  values *= dt->value();
357  }
358 
359  private:
361 
362  int _k = 0;
363 
364  public:
366 };
367 
368 class LeftSumEqualityEdge : public Edge<VectorVertex, VectorVertex, ScalarVertex>
369 {
370  public:
371  using Ptr = std::shared_ptr<LeftSumEqualityEdge>;
372  using UPtr = std::unique_ptr<LeftSumEqualityEdge>;
373 
375  : Edge<VectorVertex, VectorVertex, ScalarVertex>(x1, u1, dt), _stage_eq(stage_eq), _k(k)
376  {
377  _dim_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
378  }
379 
380  virtual ~LeftSumEqualityEdge() = default;
381 
382  // implements interface method
383  int getDimension() const override { return _dim_eq; }
384 
385  // implement in child class:
386  bool isLinear() const override { return false; }
387 
388  // bool providesJacobian() const override { return false; }
389  bool isLeastSquaresForm() const override { return false; }
390 
392  {
393  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
394  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
395  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[2]);
396 
397  _stage_eq->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), values);
398  values *= dt->value();
399  }
400 
401  private:
403 
404  int _dim_eq = 0;
405 
406  int _k = 0;
407 
408  public:
410 };
411 
412 class LeftSumInequalityEdge : public Edge<VectorVertex, VectorVertex, ScalarVertex>
413 {
414  public:
415  using Ptr = std::shared_ptr<LeftSumInequalityEdge>;
416  using UPtr = std::unique_ptr<LeftSumInequalityEdge>;
417 
419  : Edge<VectorVertex, VectorVertex, ScalarVertex>(x1, u1, dt), _stage_ineq(stage_ineq), _k(k)
420  {
421  _dim_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
422  }
423 
424  virtual ~LeftSumInequalityEdge() = default;
425 
426  // implements interface method
427  int getDimension() const override { return _dim_ineq; }
428 
429  // implement in child class:
430  bool isLinear() const override { return false; }
431 
432  // bool providesJacobian() const override { return false; }
433 
434  bool isLeastSquaresForm() const override { return false; }
435 
437  {
438  const VectorVertex* x1 = static_cast<const VectorVertex*>(_vertices[0]);
439  const VectorVertex* u1 = static_cast<const VectorVertex*>(_vertices[1]);
440  const ScalarVertex* dt = static_cast<const ScalarVertex*>(_vertices[2]);
441 
442  _stage_ineq->computeIntegralStateControlTerm(_k, x1->values(), u1->values(), values);
443  values *= dt->value();
444  }
445 
446  private:
448 
449  int _dim_ineq = 0;
450 
451  int _k = 0;
452 
453  public:
455 };
456 
457 } // namespace corbo
458 
459 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_FINITE_DIFFERENCES_COLLOCATION_EDGES_H_
std::shared_ptr< TrapezoidalIntegralInequalityEdge > Ptr
TrapezoidalIntegralInequalityEdge(VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageInequalityConstraint::Ptr stage_ineq, int k)
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
std::shared_ptr< LeftSumEqualityEdge > Ptr
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
TrapezoidalIntegralCostEdge(VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageCost::Ptr stage_cost, int k)
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
LeftSumCostEdge(VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, StageCost::Ptr stage_cost, int k)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:690
LeftSumEqualityEdge(VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, StageEqualityConstraint::Ptr stage_eq, int k)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::shared_ptr< LeftSumCostEdge > Ptr
Vertex implementation for scalar values.
Definition: scalar_vertex.h:50
std::unique_ptr< LeftSumCostEdge > UPtr
std::unique_ptr< LeftSumEqualityEdge > UPtr
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
std::unique_ptr< TrapezoidalIntegralEqualityDynamicsEdge > UPtr
std::shared_ptr< TrapezoidalIntegralEqualityEdge > Ptr
std::shared_ptr< StageEqualityConstraint > Ptr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
std::shared_ptr< StageInequalityConstraint > Ptr
std::shared_ptr< FDCollocationEdge > Ptr
std::shared_ptr< TrapezoidalIntegralCostEdge > Ptr
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
TrapezoidalIntegralEqualityDynamicsEdge(SystemDynamicsInterface::Ptr dynamics, VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageEqualityConstraint::Ptr stage_eq, int k)
std::unique_ptr< TrapezoidalIntegralEqualityEdge > UPtr
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
std::unique_ptr< LeftSumInequalityEdge > UPtr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
void setFiniteDifferencesCollocationMethod(FiniteDifferencesCollocationInterface::Ptr fd_eval)
std::shared_ptr< LeftSumInequalityEdge > Ptr
std::unique_ptr< TrapezoidalIntegralInequalityEdge > UPtr
int getDimension() const override
Return number of elements/values/components stored in this vertex.
Definition: vector_vertex.h:96
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
FDCollocationEdge(SystemDynamicsInterface::Ptr dynamics, VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt)
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
void setFiniteDifferencesCollocationMethod(FiniteDifferencesCollocationInterface::Ptr fd_eval)
const double & value() const
Get underlying value.
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
FiniteDifferencesCollocationInterface::Ptr _fd_eval
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
LeftSumInequalityEdge(VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, StageInequalityConstraint::Ptr stage_ineq, int k)
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::unique_ptr< FDCollocationEdge > UPtr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
Templated base edge class that stores an arbitary number of value.
Definition: edge.h:148
Vertex implementation that stores an Eigen::VectorXd (dynamic dimension)
Definition: vector_vertex.h:51
std::shared_ptr< StageCost > Ptr
const Eigen::VectorXd & values() const
Read-access to the underlying value vector.
TrapezoidalIntegralEqualityEdge(VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageEqualityConstraint::Ptr stage_eq, int k)
const VertexContainer _vertices
Vertex container.
Definition: edge.h:214
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
std::shared_ptr< TrapezoidalIntegralEqualityDynamicsEdge > Ptr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::shared_ptr< FiniteDifferencesCollocationInterface > Ptr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::shared_ptr< SystemDynamicsInterface > Ptr
std::unique_ptr< TrapezoidalIntegralCostEdge > UPtr
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)


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