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_COLLOCATION_EDGES_H_
26 #define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_COLLOCATION_EDGES_H_
27 
29 
36 
37 #include <functional>
38 #include <memory>
39 
40 namespace corbo {
41 
43 {
44  public:
45  using Ptr = std::shared_ptr<QuadratureCollocationDynamicsOnly>;
46  using UPtr = std::unique_ptr<QuadratureCollocationDynamicsOnly>;
47 
49  : Edge<>(5 + quadrature->getNumIntermediateStates() + quadrature->getNumIntermediateControls()), // x1, u1, x2, u2, dt + intermediate
50  _dynamics(dynamics),
51  _collocation(quadrature),
52  _num_intermediate_x(quadrature->getNumIntermediateStates()),
53  _num_intermediate_u(quadrature->getNumIntermediateControls())
54  {
55  }
56 
57  virtual ~QuadratureCollocationDynamicsOnly() = default;
58 
59  void finalize()
60  {
61  _dim_x = _dynamics->getStateDimension();
62 
63  assert(_vertices.size() == 5 + _num_intermediate_x + _num_intermediate_u);
64 
65  _x1_points.clear();
66  _u1_points.clear();
67 
68  _x2 = &static_cast<const VectorVertex*>(_vertices[0])->values();
69  _u2 = &static_cast<const VectorVertex*>(_vertices[1])->values();
70  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
71 
72  int idx = 3;
73  for (int i = 0; i < _num_intermediate_x + 1; ++i)
74  {
75  _x1_points.push_back(&static_cast<const VectorVertex*>(_vertices[idx++])->values());
76  }
77  for (int i = 0; i < _num_intermediate_u + 1; ++i)
78  {
79  _u1_points.push_back(&static_cast<const VectorVertex*>(_vertices[idx++])->values());
80  }
81  }
82 
83  // implements interface method
84  int getDimension() const override { return _dim_x; }
85  // implement in child class:
86  bool isLinear() const override { return false; }
87 
88  // bool providesJacobian() const override { return false; }
89 
90  bool isLeastSquaresForm() const override { return false; }
91 
93  {
94  assert(_collocation);
95  assert(_dynamics);
96  assert(!_x1_points.empty());
97  assert(!_u1_points.empty());
98  assert(_x2 && _u2 && _dt);
99  assert(_x1_points.front()->size() == _dynamics->getStateDimension());
100  assert(_u1_points.front()->size() == _dynamics->getInputDimension());
101  assert(_x2->size() == _dynamics->getStateDimension());
102  assert(_u2->size() == _dynamics->getInputDimension());
103 
104  // perform quadrature with given control mid points:
105  // _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, _intermediate_u,
106  // values,
107  // _intermediate_x);
108  _collocation->quadrature(_x1_points, _u1_points, *_u2, *_x2, _dt->value(), *_dynamics, values);
109 
110  values.noalias() -= (*_x2 - *_x1_points.front());
111  }
112 
113  protected:
114  private:
117 
120  int _dim_x = 0;
121 
122  std::vector<const Eigen::VectorXd*> _x1_points;
123  std::vector<const Eigen::VectorXd*> _u1_points;
124  const Eigen::VectorXd* _x2 = nullptr;
125  const Eigen::VectorXd* _u2 = nullptr;
126 
127  const ScalarVertex* _dt = nullptr;
128 
129  public:
131 };
132 
134 {
135  public:
136  using Ptr = std::shared_ptr<QuadratureCollocationEdge>;
137  using UPtr = std::unique_ptr<QuadratureCollocationEdge>;
138 
141  bool eval_intermediate_constr, int k)
142  : MixedEdge<>(5 + quadrature->getNumIntermediateStates() + quadrature->getNumIntermediateControls()),
143  _dynamics(dynamics),
144  _collocation(quadrature),
145  _stage_cost(stage_cost),
146  _stage_eq(stage_eq),
147  _stage_ineq(stage_ineq),
148  _k(k),
149  _eval_intermediate_constr(eval_intermediate_constr),
150  _num_intermediate_x(quadrature->getNumIntermediateStates()),
151  _num_intermediate_u(quadrature->getNumIntermediateControls())
152  {
153  assert(quadrature->isIntermediateControlSubjectToOptim());
154  }
155 
156  void finalize()
157  {
158  int x_dim = _dynamics->getStateDimension();
159 
160  _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0; // we have only a single obejtive value
161  _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
162  _dim_eq = x_dim + _dim_int_eq;
163  _dim_int_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
164  _dim_ineq = _dim_int_ineq;
165  _dim_dyn = _dynamics->getStateDimension();
166 
167  assert(_vertices.size() == 5 + _num_intermediate_x + _num_intermediate_u);
168 
169  _x1_points.clear();
170  _u1_points.clear();
171 
172  _x2 = &static_cast<const VectorVertex*>(_vertices[0])->values();
173  _u2 = &static_cast<const VectorVertex*>(_vertices[1])->values();
174  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
175 
176  int idx = 3;
177  _x1 = static_cast<const VectorVertex*>(_vertices[idx]);
178  for (int i = 0; i < _num_intermediate_x + 1; ++i)
179  {
180  _x1_points.push_back(&static_cast<const VectorVertex*>(_vertices[idx++])->values());
181  }
182  for (int i = 0; i < _num_intermediate_u + 1; ++i)
183  {
184  _u1_points.push_back(&static_cast<const VectorVertex*>(_vertices[idx++])->values());
185  }
186 
187  _quadrature_values.resize(_dim_obj + _dim_dyn + _dim_int_eq + _dim_int_ineq);
188 
189  configureIntegrand();
190  PRINT_WARNING_COND_NAMED(_eval_intermediate_constr, "_eval_intermediate_constr not yet implemented");
191  // activateIntermediateConstraints();
192  }
193 
194  virtual ~QuadratureCollocationEdge() = default;
195 
196  // implements interface method
197  int getObjectiveDimension() const override { return _dim_obj; }
198  // implements interface method
199  int getEqualityDimension() const override { return _dim_eq; }
200  int getInequalityDimension() const override { return _dim_ineq; }
201 
202  // implement in child class:
203  bool isObjectiveLinear() const override { return false; }
204  bool isEqualityLinear() const override { return false; }
205  bool isInequalityLinear() const override { return false; }
206 
207  // bool providesJacobian() const override { return false; }
208 
209  bool isObjectiveLeastSquaresForm() const override { return false; }
210 
211  void precompute() override
212  {
213  assert(_collocation);
214  assert(_dynamics);
215  assert(!_x1_points.empty());
216  assert(!_u1_points.empty());
217  assert(_x2 && _u2 && _dt);
218  assert(_x1_points.front()->size() == _dynamics->getStateDimension());
219  assert(_u1_points.front()->size() == _dynamics->getInputDimension());
220  assert(_x2->size() == _dynamics->getStateDimension());
221  assert(_u2->size() == _dynamics->getInputDimension());
222 
223  _collocation->quadrature(_x1_points, _u1_points, *_u2, *_x2, _dt->value(), _integrand, _quadrature_values);
224  }
225 
226  void computeObjectiveValues(Eigen::Ref<Eigen::VectorXd> obj_values) override { obj_values = _quadrature_values.head(1); }
227 
229  {
230  eq_values.head(_dim_dyn).noalias() = *_x2 - _x1->values() - _quadrature_values.segment(_dim_obj, _dim_dyn);
231  if (_dim_int_eq > 0)
232  {
233  eq_values.segment(_dim_dyn, _dim_int_eq).noalias() = _quadrature_values.segment(_dim_obj + _dim_dyn, _dim_int_eq);
234  }
235 
236  // in case we consider intermediate constraints
237  // if (_dim_nonint_eq > 0)
238  // {
239  // int cur_idx = _dim_dyn + _dim_int_eq;
240  // for (int i = 0; i < _intermediate_x; ++i)
241  // {
242  // _stage_eq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], *_intermediate_u[i], _dt->value(),
243  // eq_values.segment(cur_idx, _dim_nonint_eq));
244  // cur_idx += _dim_nonint_eq;
245  // }
246  // assert(cur_idx == getEqualityDimension());
247  // }
248  }
250  {
251  if (_dim_int_ineq > 0)
252  {
253  ineq_values = _quadrature_values.tail(_dim_int_ineq);
254  }
255 
256  // in case we consider intermediate constraints, the following dimensions are > 0
257  // int cur_idx = _dim_int_eq;
258  // if (_dim_nonint_ineq > 0)
259  // {
260  // for (int i = 0; i < (int)_intermediate_x.size(); ++i)
261  // {
262  // _stage_ineq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], *_intermediate_u[i], _dt->value(),
263  // ineq_values.segment(cur_idx, _dim_nonint_ineq));
264  // cur_idx += _dim_nonint_eq;
265  // }
266  // }
267  //
268  // // lower bounds
269  // if (_dim_lb_x > 0)
270  // {
271  // for (int i = 0; i < (int)_intermediate_x.size(); ++i)
272  // {
273  // for (int j = 0; j < _intermediate_x[i]->size(); ++j)
274  // {
275  // if (_x1->hasFiniteLowerBound(j))
276  // {
277  // ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j]; // x_min - x <= 0
278  // }
279  // }
280  // }
281  // }
282  //
283  // // upper bounds
284  // if (_dim_ub_x > 0)
285  // {
286  // for (int i = 0; i < (int)_intermediate_x.size(); ++i)
287  // {
288  // for (int j = 0; j < _intermediate_x[i]->size(); ++j)
289  // {
290  // if (_x1->hasFiniteUpperBound(j))
291  // {
292  // ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j]; // x - x_max <= 0
293  // }
294  // }
295  // }
296  // }
297  // assert(cur_idx == getInequalityDimension());
298  }
299 
300  protected:
301  // void activateIntermediateConstraints()
302  // {
303  // // get number inequalities in case _eval_intermediate_constr is turned on
304  // // TODO(roesmann) currently we only evaluate intermediate states, since u is usually just a linear function
305  // if (_eval_intermediate_constr)
306  // {
307  // _dim_nonint_eq = _stage_eq ? _stage_eq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
308  // _dim_nonint_ineq = _stage_ineq ? _stage_ineq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
309  //
310  // // consider number of finite bounds for all vertices
311  // // we apply the bounds of vertex x1
312  // _dim_lb_x = _x1_points[0]->getNumberFiniteLowerBounds(false);
313  // _dim_ub_x = _x1_points[0]->getNumberFiniteUpperBounds(false);
314  //
315  // // update overall dimensions
316  // _dim_eq = _dim_int_eq + _dim_dyn + _num_intermediate_x * _dim_nonint_eq;
317  // _dim_ineq = _dim_int_ineq + _num_intermediate_x * (_dim_nonint_ineq + _dim_lb_x + _dim_ub_x);
318  // }
319  // }
320 
322  {
323  _integrand = [&](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
324  int idx = 0;
325  _dynamics->dynamics(x, u, result.segment(_dim_obj, _dim_dyn));
326 
327  if (_dim_obj > 0)
328  {
329  assert(_dim_obj == 1);
330  _stage_cost->computeIntegralStateControlTerm(_k, x, u, result.head(1));
331  idx += 1;
332  }
333  idx += _dim_dyn;
334 
335  if (_dim_int_eq > 0)
336  {
337  _stage_eq->computeIntegralStateControlTerm(_k, x, u, result.segment(idx, _dim_int_eq));
338  idx += _dim_int_eq;
339  }
340 
341  if (_dim_int_ineq > 0)
342  {
343  _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result.segment(idx, _dim_int_eq));
344  idx += _dim_int_eq;
345  }
346  assert(idx == result.size());
347  };
348  }
349 
350  private:
353 
357 
358  int _dim_obj = 1;
359  int _dim_eq = 0;
360  int _dim_int_eq = 0;
361  int _dim_ineq = 0;
362  int _dim_int_ineq = 0;
363  int _dim_dyn = 0;
364 
365  int _dim_nonint_eq = 0;
366  int _dim_nonint_ineq = 0;
367 
368  int _dim_lb_x = 0;
369  int _dim_ub_x = 0;
370 
371  int _k = 0;
372 
373  bool _eval_intermediate_constr = false;
374 
377 
378  Eigen::VectorXd _quadrature_values;
379 
380  std::function<void(const Eigen::VectorXd&, const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> _integrand;
381 
382  const VectorVertex* _x1 = nullptr;
383  std::vector<const Eigen::VectorXd*> _x1_points;
384  std::vector<const Eigen::VectorXd*> _u1_points;
385  const Eigen::VectorXd* _x2 = nullptr;
386  const Eigen::VectorXd* _u2 = nullptr;
387 
388  const ScalarVertex* _dt = nullptr;
389 
390  public:
392 };
393 
394 class CompressedCollocationEdge : public Edge<VectorVertex, VectorVertex, ScalarVertex, VectorVertex, VectorVertex>
395 {
396  public:
397  using Ptr = std::shared_ptr<CompressedCollocationEdge>;
398  using UPtr = std::unique_ptr<CompressedCollocationEdge>;
399 
401  VectorVertex& x2)
402  : Edge<VectorVertex, VectorVertex, ScalarVertex, VectorVertex, VectorVertex>(x1, u1, dt, u2, x2), _dynamics(dynamics)
403  {
404  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
405  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
406  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
407  _u2 = static_cast<const VectorVertex*>(_vertices[3]);
408  _x2 = static_cast<const VectorVertex*>(_vertices[4]);
409  }
410 
411  // implements interface method
412  int getDimension() const override
413  {
414  assert(_dynamics);
415  return _dynamics->getStateDimension();
416  }
417  // implement in child class:
418  bool isLinear() const override { return false; }
419 
420  // bool providesJacobian() const override { return false; }
421 
422  bool isLeastSquaresForm() const override { return false; }
423 
425  {
426  assert(_collocation);
427  assert(_dynamics);
428  assert(_x1->getDimension() == _dynamics->getStateDimension());
429  assert(_u1->getDimension() == _dynamics->getInputDimension());
430  assert(_x2->getDimension() == _dynamics->getStateDimension());
431  assert(_u2->getDimension() == _dynamics->getInputDimension());
432 
433  _collocation->computeEqualityConstraint(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, values);
434  }
435 
437  {
438  assert(!collocation->isIntermediateControlSubjectToOptim() && collocation->isSupportingCompressedStatesMode());
439  _collocation = collocation;
440  }
441 
442  private:
445 
446  const VectorVertex* _x1 = nullptr;
447  const VectorVertex* _u1 = nullptr;
448  const VectorVertex* _x2 = nullptr;
449  const VectorVertex* _u2 = nullptr;
450  const ScalarVertex* _dt = nullptr;
451 
452  public:
454 };
455 
457 {
458  public:
459  using Ptr = std::shared_ptr<CompressedCollocationMultipleControlsEdge>;
460  using UPtr = std::unique_ptr<CompressedCollocationMultipleControlsEdge>;
461 
463  : Edge<>(5 + quadrature->getNumIntermediatePoints()),
464  _dynamics(dynamics),
465  _collocation(quadrature),
466  _num_intermediate_points(quadrature->getNumIntermediatePoints())
467  {
468  assert(quadrature->isIntermediateControlSubjectToOptim());
469  }
470 
471  virtual ~CompressedCollocationMultipleControlsEdge() { clearInternalBuffer(); }
472 
473  void finalize()
474  {
475  _dim_x = _dynamics->getStateDimension();
476 
477  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
478  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
479  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
480  _u2 = static_cast<const VectorVertex*>(_vertices[3]);
481  _x2 = static_cast<const VectorVertex*>(_vertices[4]);
482 
483  int idx = 5;
484  _intermediate_u.clear();
485  for (int i = 0; i < _num_intermediate_points; ++i)
486  {
487  VectorVertex* interm_u = static_cast<VectorVertex*>(_vertices[idx + i]);
488  _intermediate_u.push_back(&interm_u->values());
489  }
490 
491  clearInternalBuffer();
492 
493  for (int i = 0; i < _num_intermediate_points; ++i)
494  {
495  _intermediate_x.push_back(new Eigen::VectorXd(_dim_x));
496  }
497  }
498 
499  // implements interface method
500  int getDimension() const override { return _dim_x; }
501  // implement in child class:
502  bool isLinear() const override { return false; }
503 
504  // bool providesJacobian() const override { return false; }
505 
506  bool isLeastSquaresForm() const override { return false; }
507 
509  {
510  assert(_collocation);
511  assert(_dynamics);
512  assert(_x1->getDimension() == _dynamics->getStateDimension());
513  assert(_u1->getDimension() == _dynamics->getInputDimension());
514  assert(_x2->getDimension() == _dynamics->getStateDimension());
515  assert(_u2->getDimension() == _dynamics->getInputDimension());
516 
517  // perform quadrature with given control mid points:
518  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, _intermediate_u, values,
519  _intermediate_x);
520 
521  values.noalias() -= (_x2->values() - _x1->values());
522  }
523 
524  protected:
526  {
527  for (int i = 0; i < _intermediate_x.size(); ++i) delete _intermediate_x[i];
528  _intermediate_x.clear();
529  }
530 
531  private:
534 
536  int _dim_x = 0;
537 
538  std::vector<Eigen::VectorXd*> _intermediate_x;
539  std::vector<Eigen::VectorXd*> _intermediate_u;
540 
541  const VectorVertex* _x1 = nullptr;
542  const VectorVertex* _u1 = nullptr;
543  const VectorVertex* _x2 = nullptr;
544  const VectorVertex* _u2 = nullptr;
545  const ScalarVertex* _dt = nullptr;
546 
547  public:
549 };
550 
551 class ConstControlCombinedCompressedCollocationEdge : public MixedEdge<VectorVertex, VectorVertex, ScalarVertex, VectorVertex>
552 {
553  public:
554  using Ptr = std::shared_ptr<ConstControlCombinedCompressedCollocationEdge>;
555  using UPtr = std::unique_ptr<ConstControlCombinedCompressedCollocationEdge>;
556 
559  bool eval_intermediate_constr, int k, VectorVertex& x1, VectorVertex& u1, ScalarVertex& dt,
560  VectorVertex& x2)
562  _dynamics(dynamics),
563  _stage_cost(stage_cost),
564  _stage_eq(stage_eq),
565  _stage_ineq(stage_ineq),
566  _k(k),
567  _eval_intermediate_constr(eval_intermediate_constr)
568  {
569  int x_dim = _dynamics->getStateDimension();
570  _dynamics_quadrature.resize(x_dim);
571 
572  _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0; // we have only a single obejtive value
573  _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(k) : 0;
574  _dim_eq = x_dim + _dim_int_eq;
575  _dim_int_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(k) : 0;
576  _dim_ineq = _dim_int_ineq;
577  _dim_dyn = _dynamics->getStateDimension();
578 
579  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
580  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
581  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
582  _x2 = static_cast<const VectorVertex*>(_vertices[3]);
583  }
584 
586  {
587  for (int i = 0; i < (int)_intermediate_x.size(); ++i) delete _intermediate_x[i];
588  }
589 
590  // implements interface method
591  int getObjectiveDimension() const override { return _dim_obj; }
592  // implements interface method
593  int getEqualityDimension() const override { return _dim_eq; }
594  int getInequalityDimension() const override { return _dim_ineq; }
595 
596  // implement in child class:
597  bool isObjectiveLinear() const override { return false; }
598  bool isEqualityLinear() const override { return false; }
599  bool isInequalityLinear() const override { return false; }
600 
601  // bool providesJacobian() const override { return false; }
602 
603  bool isObjectiveLeastSquaresForm() const override { return false; }
604 
605  void precompute() override
606  {
607  assert(_collocation);
608  assert(_dynamics);
609 
610  assert(_x1->getDimension() == _dynamics->getStateDimension());
611  assert(_u1->getDimension() == _dynamics->getInputDimension());
612  assert(_x2->getDimension() == _dynamics->getStateDimension());
613 
614  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _dt->value(), *_dynamics, _dynamics_quadrature, _intermediate_x);
615  }
616 
618  {
619  // evaluate quadrature formula with previously computed intermediate states
620  auto integrand = [this](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> result) {
621  _stage_cost->computeIntegralStateControlTerm(_k, x, _u1->values(), result);
622  };
623  _collocation->quadrature(_x1->values(), _x2->values(), _dt->value(), _intermediate_x, integrand, obj_values);
624  }
625 
627  {
628  eq_values.head(_dynamics_quadrature.size()).noalias() = _x2->values() - _x1->values() - _dynamics_quadrature;
629  if (_dim_int_eq > 0)
630  {
631  // evaluate quadrature formula with previously computed intermediate states
632  auto integrand = [this](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> result) {
633  _stage_eq->computeIntegralStateControlTerm(_k, x, _u1->values(), result);
634  };
635  _collocation->quadrature(_x1->values(), _x2->values(), _dt->value(), _intermediate_x, integrand,
636  eq_values.segment(_dim_dyn, _dim_int_eq));
637  }
638 
639  // in case we consider intermediate constraints
640  if (_dim_nonint_eq > 0)
641  {
642  int cur_idx = _dim_dyn + _dim_int_eq;
643 
644  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
645  {
646  _stage_eq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], _u1->values(), _dt->value(),
647  eq_values.segment(cur_idx, _dim_nonint_eq));
648  cur_idx += _dim_nonint_eq;
649  }
650  assert(cur_idx == getEqualityDimension());
651  }
652  }
654  {
655  if (_dim_int_ineq > 0)
656  {
657  // evaluate quadrature formula with previously computed intermediate states
658  auto integrand = [this](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> result) {
659  _stage_ineq->computeIntegralStateControlTerm(_k, x, _u1->values(), result);
660  };
661  _collocation->quadrature(_x1->values(), _x2->values(), _dt->value(), _intermediate_x, integrand, ineq_values);
662  }
663 
664  // in case we consider intermediate constraints, the following dimensions are > 0
665  int cur_idx = _dim_int_eq;
666  if (_dim_nonint_ineq > 0)
667  {
668  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
669  {
670  _stage_ineq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], _u1->values(), _dt->value(),
671  ineq_values.segment(cur_idx, _dim_nonint_ineq));
672  cur_idx += _dim_nonint_eq;
673  }
674  }
675 
676  // lower bounds
677  if (_dim_lb_x > 0)
678  {
679  for (int i = 0; i < _intermediate_x.size(); ++i)
680  {
681  for (int j = 0; j < _intermediate_x[i]->size(); ++j)
682  {
683  if (_x1->hasFiniteLowerBound(j))
684  {
685  ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j]; // x_min - x <= 0
686  }
687  }
688  }
689  }
690 
691  // upper bounds
692  if (_dim_ub_x > 0)
693  {
694  for (int i = 0; i < _intermediate_x.size(); ++i)
695  {
696  for (int j = 0; j < _intermediate_x[i]->size(); ++j)
697  {
698  if (_x1->hasFiniteUpperBound(j))
699  {
700  ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j]; // x - x_max <= 0
701  }
702  }
703  }
704  }
705  assert(cur_idx == getInequalityDimension());
706  }
707 
709  {
710  assert(!quadrature->isIntermediateControlSubjectToOptim() && quadrature->isSupportingCompressedStatesMode());
711  _collocation = quadrature;
712  _intermediate_x.resize(_collocation->getNumIntermediatePoints(), new Eigen::VectorXd(_dim_dyn));
713 
714  activateIntermediateConstraints();
715  }
716 
717  protected:
719  {
720  // get number inequalities in case _eval_intermediate_constr is turned on
721  if (_eval_intermediate_constr)
722  {
723  int num_interm_states = _collocation->getNumIntermediatePoints();
724  _dim_nonint_eq = _stage_eq ? _stage_eq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
725  _dim_nonint_ineq = _stage_ineq ? _stage_ineq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
726 
727  // consider number of finite bounds for all vertices
728  // we apply the bounds of vertex x1
729  _dim_lb_x = _x1->getNumberFiniteLowerBounds(false);
730  _dim_ub_x = _x1->getNumberFiniteUpperBounds(false);
731 
732  // update overall dimensions
733  _dim_eq = _dim_int_eq + _dim_dyn + num_interm_states * _dim_nonint_eq;
734  _dim_ineq = _dim_int_ineq + num_interm_states * (_dim_nonint_ineq + _dim_lb_x + _dim_ub_x);
735  }
736  }
737 
738  private:
741 
745 
746  int _dim_obj = 1;
747  int _dim_eq = 0;
748  int _dim_int_eq = 0;
749  int _dim_ineq = 0;
750  int _dim_int_ineq = 0;
751  int _dim_dyn = 0;
752 
753  int _dim_nonint_eq = 0;
754  int _dim_nonint_ineq = 0;
755 
756  int _dim_lb_x = 0;
757  int _dim_ub_x = 0;
758 
759  int _k = 0;
760 
761  bool _eval_intermediate_constr = false;
762 
763  Eigen::VectorXd _dynamics_quadrature;
764  std::vector<Eigen::VectorXd*> _intermediate_x;
765 
766  const VectorVertex* _x1 = nullptr;
767  const VectorVertex* _u1 = nullptr;
768  const VectorVertex* _x2 = nullptr;
769  const ScalarVertex* _dt = nullptr;
770 
771  public:
773 };
774 
775 class CombinedCompressedCollocationEdge : public MixedEdge<VectorVertex, VectorVertex, ScalarVertex, VectorVertex, VectorVertex>
776 {
777  public:
778  using Ptr = std::shared_ptr<CombinedCompressedCollocationEdge>;
779  using UPtr = std::unique_ptr<CombinedCompressedCollocationEdge>;
780 
783  bool eval_intermediate_constr, int k, VectorVertex& x1, VectorVertex& u1, ScalarVertex& dt,
784  VectorVertex& u2, VectorVertex& x2)
786  _dynamics(dynamics),
787  _stage_cost(stage_cost),
788  _stage_eq(stage_eq),
789  _stage_ineq(stage_ineq),
790  _k(k),
791  _eval_intermediate_constr(eval_intermediate_constr)
792  {
793  int x_dim = _dynamics->getStateDimension();
794  _dynamics_quadrature.resize(x_dim);
795 
796  _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0; // we have only a single obejtive value
797  _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(k) : 0;
798  _dim_eq = x_dim + _dim_int_eq;
799  _dim_int_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(k) : 0;
800  _dim_ineq = _dim_int_ineq;
801  _dim_dyn = _dynamics->getStateDimension();
802 
803  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
804  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
805  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
806  _u2 = static_cast<const VectorVertex*>(_vertices[3]);
807  _x2 = static_cast<const VectorVertex*>(_vertices[4]);
808  }
809 
811  {
812  for (int i = 0; i < (int)_intermediate_x.size(); ++i) delete _intermediate_x[i];
813  for (int i = 0; i < (int)_intermediate_u.size(); ++i) delete _intermediate_u[i];
814  }
815 
816  // implements interface method
817  int getObjectiveDimension() const override { return _dim_obj; }
818  // implements interface method
819  int getEqualityDimension() const override { return _dim_eq; }
820  int getInequalityDimension() const override { return _dim_ineq; }
821 
822  // implement in child class:
823  bool isObjectiveLinear() const override { return false; }
824  bool isEqualityLinear() const override { return false; }
825  bool isInequalityLinear() const override { return false; }
826 
827  // bool providesJacobian() const override { return false; }
828 
829  bool isObjectiveLeastSquaresForm() const override { return false; }
830 
831  void precompute() override
832  {
833  assert(_collocation);
834  assert(_dynamics);
835 
836  assert(_x1->getDimension() == _dynamics->getStateDimension());
837  assert(_u1->getDimension() == _dynamics->getInputDimension());
838  assert(_u2->getDimension() == _dynamics->getInputDimension());
839  assert(_x2->getDimension() == _dynamics->getStateDimension());
840 
841  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, _dynamics_quadrature,
842  _intermediate_x, _intermediate_u);
843  }
844 
846  {
847  // evaluate quadrature formula with previously computed intermediate states
848  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
849  _stage_cost->computeIntegralStateControlTerm(_k, x, u, result);
850  };
851  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
852  integrand, obj_values);
853  }
854 
856  {
857  eq_values.head(_dynamics_quadrature.size()).noalias() = _x2->values() - _x1->values() - _dynamics_quadrature;
858  if (_dim_int_eq > 0)
859  {
860  // evaluate quadrature formula with previously computed intermediate states
861  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
862  _stage_eq->computeIntegralStateControlTerm(_k, x, u, result);
863  };
864  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
865  integrand, eq_values.segment(_dim_dyn, _dim_int_eq));
866  }
867 
868  // in case we consider intermediate constraints
869  if (_dim_nonint_eq > 0)
870  {
871  int cur_idx = _dim_dyn + _dim_int_eq;
872  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
873  {
874  _stage_eq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], *_intermediate_u[i], _dt->value(),
875  eq_values.segment(cur_idx, _dim_nonint_eq));
876  cur_idx += _dim_nonint_eq;
877  }
878  assert(cur_idx == getEqualityDimension());
879  }
880  }
882  {
883  if (_dim_int_ineq > 0)
884  {
885  // evaluate quadrature formula with previously computed intermediate states
886  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
887  _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result);
888  };
889  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
890  integrand, ineq_values);
891  }
892 
893  // in case we consider intermediate constraints, the following dimensions are > 0
894  int cur_idx = _dim_int_eq;
895  if (_dim_nonint_ineq > 0)
896  {
897  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
898  {
899  _stage_ineq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], *_intermediate_u[i], _dt->value(),
900  ineq_values.segment(cur_idx, _dim_nonint_ineq));
901  cur_idx += _dim_nonint_eq;
902  }
903  }
904 
905  // lower bounds
906  if (_dim_lb_x > 0)
907  {
908  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
909  {
910  for (int j = 0; j < _intermediate_x[i]->size(); ++j)
911  {
912  if (_x1->hasFiniteLowerBound(j))
913  {
914  ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j]; // x_min - x <= 0
915  }
916  }
917  }
918  }
919 
920  // upper bounds
921  if (_dim_ub_x > 0)
922  {
923  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
924  {
925  for (int j = 0; j < _intermediate_x[i]->size(); ++j)
926  {
927  if (_x1->hasFiniteUpperBound(j))
928  {
929  ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j]; // x - x_max <= 0
930  }
931  }
932  }
933  }
934  assert(cur_idx == getInequalityDimension());
935  }
936 
938  {
939  assert(!quadrature->isIntermediateControlSubjectToOptim() && quadrature->isSupportingCompressedStatesMode());
940 
941  _collocation = quadrature;
942  _intermediate_x.resize(_collocation->getNumIntermediatePoints(), new Eigen::VectorXd(_dim_dyn));
943  _intermediate_u.resize(_collocation->getNumIntermediatePoints(), new Eigen::VectorXd(_dynamics->getInputDimension()));
944 
945  activateIntermediateConstraints();
946  }
947 
948  protected:
950  {
951  // get number inequalities in case _eval_intermediate_constr is turned on
952  // TODO(roesmann) currently we only evaluate intermediate states, since u is usually just a linear function
953  if (_eval_intermediate_constr)
954  {
955  int num_interm_states = _collocation->getNumIntermediatePoints();
956  _dim_nonint_eq = _stage_eq ? _stage_eq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
957  _dim_nonint_ineq = _stage_ineq ? _stage_ineq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
958 
959  // consider number of finite bounds for all vertices
960  // we apply the bounds of vertex x1
961  _dim_lb_x = _x1->getNumberFiniteLowerBounds(false);
962  _dim_ub_x = _x1->getNumberFiniteUpperBounds(false);
963 
964  // update overall dimensions
965  _dim_eq = _dim_int_eq + _dim_dyn + num_interm_states * _dim_nonint_eq;
966  _dim_ineq = _dim_int_ineq + num_interm_states * (_dim_nonint_ineq + _dim_lb_x + _dim_ub_x);
967  }
968  }
969 
970  private:
973 
977 
978  int _dim_obj = 1;
979  int _dim_eq = 0;
980  int _dim_int_eq = 0;
981  int _dim_ineq = 0;
982  int _dim_int_ineq = 0;
983  int _dim_dyn = 0;
984 
985  int _dim_nonint_eq = 0;
986  int _dim_nonint_ineq = 0;
987 
988  int _dim_lb_x = 0;
989  int _dim_ub_x = 0;
990 
991  int _k = 0;
992 
993  bool _eval_intermediate_constr = false;
994 
995  Eigen::VectorXd _dynamics_quadrature;
996  std::vector<Eigen::VectorXd*> _intermediate_x;
997  std::vector<Eigen::VectorXd*> _intermediate_u;
998 
999  const VectorVertex* _x1 = nullptr;
1000  const VectorVertex* _u1 = nullptr;
1001  const VectorVertex* _u2 = nullptr;
1002  const VectorVertex* _x2 = nullptr;
1003  const ScalarVertex* _dt = nullptr;
1004 
1005  public:
1007 };
1008 
1010 {
1011  public:
1012  using Ptr = std::shared_ptr<CombinedCompressedCollocationMultipleControlsEdge>;
1013  using UPtr = std::unique_ptr<CombinedCompressedCollocationMultipleControlsEdge>;
1014 
1016  StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq,
1017  StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k)
1018  : MixedEdge<>(5 + quadrature->getNumIntermediatePoints()),
1019  _dynamics(dynamics),
1020  _collocation(quadrature),
1021  _stage_cost(stage_cost),
1022  _stage_eq(stage_eq),
1023  _stage_ineq(stage_ineq),
1024  _k(k),
1025  _eval_intermediate_constr(eval_intermediate_constr),
1026  _num_intermediate_points(quadrature->getNumIntermediatePoints())
1027  {
1028  assert(quadrature->isIntermediateControlSubjectToOptim());
1029  }
1030 
1031  void finalize()
1032  {
1033  int x_dim = _dynamics->getStateDimension();
1034  _dynamics_quadrature.resize(x_dim);
1035 
1036  _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0; // we have only a single obejtive value
1037  _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
1038  _dim_eq = x_dim + _dim_int_eq;
1039  _dim_int_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
1040  _dim_ineq = _dim_int_ineq;
1041  _dim_dyn = _dynamics->getStateDimension();
1042 
1043  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
1044  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
1045  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
1046  _u2 = static_cast<const VectorVertex*>(_vertices[3]);
1047  _x2 = static_cast<const VectorVertex*>(_vertices[4]);
1048 
1049  int idx = 5;
1050  _intermediate_u.clear();
1051  for (int i = 0; i < _num_intermediate_points; ++i)
1052  {
1053  VectorVertex* interm_u = static_cast<VectorVertex*>(_vertices[idx + i]);
1054  _intermediate_u.push_back(&interm_u->values());
1055  }
1056 
1057  clearInternalBuffer();
1058 
1059  for (int i = 0; i < _num_intermediate_points; ++i)
1060  {
1061  _intermediate_x.push_back(new Eigen::VectorXd(_dim_dyn));
1062  }
1063 
1064  activateIntermediateConstraints();
1065  }
1066 
1067  virtual ~CombinedCompressedCollocationMultipleControlsEdge() { clearInternalBuffer(); }
1068 
1069  // implements interface method
1070  int getObjectiveDimension() const override { return _dim_obj; }
1071  // implements interface method
1072  int getEqualityDimension() const override { return _dim_eq; }
1073  int getInequalityDimension() const override { return _dim_ineq; }
1074 
1075  // implement in child class:
1076  bool isObjectiveLinear() const override { return false; }
1077  bool isEqualityLinear() const override { return false; }
1078  bool isInequalityLinear() const override { return false; }
1079 
1080  // bool providesJacobian() const override { return false; }
1081 
1082  bool isObjectiveLeastSquaresForm() const override { return false; }
1083 
1084  void precompute() override
1085  {
1086  assert(_collocation);
1087  assert(_dynamics);
1088 
1089  assert(_x1->getDimension() == _dynamics->getStateDimension());
1090  assert(_u1->getDimension() == _dynamics->getInputDimension());
1091  assert(_u2->getDimension() == _dynamics->getInputDimension());
1092  assert(_x2->getDimension() == _dynamics->getStateDimension());
1093 
1094  // perform quadrature with given control mid points
1095  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, _intermediate_u,
1096  _dynamics_quadrature, _intermediate_x);
1097  }
1098 
1100  {
1101  // evaluate quadrature formula with previously computed intermediate states
1102  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
1103  _stage_cost->computeIntegralStateControlTerm(_k, x, u, result);
1104  };
1105  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
1106  integrand, obj_values);
1107  }
1108 
1110  {
1111  eq_values.head(_dynamics_quadrature.size()).noalias() = _x2->values() - _x1->values() - _dynamics_quadrature;
1112  if (_dim_int_eq > 0)
1113  {
1114  // evaluate quadrature formula with previously computed intermediate states
1115  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
1116  _stage_eq->computeIntegralStateControlTerm(_k, x, u, result);
1117  };
1118  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
1119  integrand, eq_values.segment(_dim_dyn, _dim_int_eq));
1120  }
1121 
1122  // in case we consider intermediate constraints
1123  if (_dim_nonint_eq > 0)
1124  {
1125  int cur_idx = _dim_dyn + _dim_int_eq;
1126  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
1127  {
1128  _stage_eq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], *_intermediate_u[i], _dt->value(),
1129  eq_values.segment(cur_idx, _dim_nonint_eq));
1130  cur_idx += _dim_nonint_eq;
1131  }
1132  assert(cur_idx == getEqualityDimension());
1133  }
1134  }
1136  {
1137  if (_dim_int_ineq > 0)
1138  {
1139  // evaluate quadrature formula with previously computed intermediate states
1140  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
1141  _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result);
1142  };
1143  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
1144  integrand, ineq_values);
1145  }
1146 
1147  // in case we consider intermediate constraints, the following dimensions are > 0
1148  int cur_idx = _dim_int_eq;
1149  if (_dim_nonint_ineq > 0)
1150  {
1151  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
1152  {
1153  _stage_ineq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], *_intermediate_u[i], _dt->value(),
1154  ineq_values.segment(cur_idx, _dim_nonint_ineq));
1155  cur_idx += _dim_nonint_eq;
1156  }
1157  }
1158 
1159  // lower bounds
1160  if (_dim_lb_x > 0)
1161  {
1162  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
1163  {
1164  for (int j = 0; j < _intermediate_x[i]->size(); ++j)
1165  {
1166  if (_x1->hasFiniteLowerBound(j))
1167  {
1168  ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j]; // x_min - x <= 0
1169  }
1170  }
1171  }
1172  }
1173 
1174  // upper bounds
1175  if (_dim_ub_x > 0)
1176  {
1177  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
1178  {
1179  for (int j = 0; j < _intermediate_x[i]->size(); ++j)
1180  {
1181  if (_x1->hasFiniteUpperBound(j))
1182  {
1183  ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j]; // x - x_max <= 0
1184  }
1185  }
1186  }
1187  }
1188  assert(cur_idx == getInequalityDimension());
1189  }
1190 
1191  protected:
1193  {
1194  // get number inequalities in case _eval_intermediate_constr is turned on
1195  // TODO(roesmann) currently we only evaluate intermediate states, since u is usually just a linear function
1196  if (_eval_intermediate_constr)
1197  {
1198  int num_interm_states = _collocation->getNumIntermediatePoints();
1199  _dim_nonint_eq = _stage_eq ? _stage_eq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
1200  _dim_nonint_ineq = _stage_ineq ? _stage_ineq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
1201 
1202  // consider number of finite bounds for all vertices
1203  // we apply the bounds of vertex x1
1204  _dim_lb_x = _x1->getNumberFiniteLowerBounds(false);
1205  _dim_ub_x = _x1->getNumberFiniteUpperBounds(false);
1206 
1207  // update overall dimensions
1208  _dim_eq = _dim_int_eq + _dim_dyn + num_interm_states * _dim_nonint_eq;
1209  _dim_ineq = _dim_int_ineq + num_interm_states * (_dim_nonint_ineq + _dim_lb_x + _dim_ub_x);
1210  }
1211  }
1212 
1214  {
1215  for (int i = 0; i < _intermediate_x.size(); ++i) delete _intermediate_x[i];
1216  _intermediate_x.clear();
1217  }
1218 
1219  private:
1222 
1226 
1227  int _dim_obj = 1;
1228  int _dim_eq = 0;
1229  int _dim_int_eq = 0;
1230  int _dim_ineq = 0;
1231  int _dim_int_ineq = 0;
1232  int _dim_dyn = 0;
1233 
1234  int _dim_nonint_eq = 0;
1235  int _dim_nonint_ineq = 0;
1236 
1237  int _dim_lb_x = 0;
1238  int _dim_ub_x = 0;
1239 
1240  int _k = 0;
1241 
1242  bool _eval_intermediate_constr = false;
1243 
1245 
1246  Eigen::VectorXd _dynamics_quadrature;
1247  std::vector<Eigen::VectorXd*> _intermediate_x;
1248  std::vector<Eigen::VectorXd*> _intermediate_u;
1249 
1250  const VectorVertex* _x1 = nullptr;
1251  const VectorVertex* _u1 = nullptr;
1252  const VectorVertex* _u2 = nullptr;
1253  const VectorVertex* _x2 = nullptr;
1254  const ScalarVertex* _dt = nullptr;
1255 
1256  public:
1258 };
1259 
1261 {
1262  public:
1263  using Ptr = std::shared_ptr<UncompressedCollocationEdge>;
1264  using UPtr = std::unique_ptr<UncompressedCollocationEdge>;
1265 
1267  : Edge<>(5 + quadrature->getNumIntermediatePoints() * (1 + (int)quadrature->isIntermediateControlSubjectToOptim())),
1268  _dynamics(dynamics),
1269  _collocation(quadrature),
1270  _num_intermediate_points(quadrature->getNumIntermediatePoints())
1271  {
1272  }
1273 
1274  void finalize()
1275  {
1276  _dim_x = _dynamics->getStateDimension();
1277 
1278  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
1279  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
1280  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
1281  _u2 = static_cast<const VectorVertex*>(_vertices[3]);
1282  _x2 = static_cast<const VectorVertex*>(_vertices[4]);
1283 
1284  int idx = 5;
1285  _intermediate_x.clear();
1286  for (int i = 0; i < _num_intermediate_points; ++i)
1287  {
1288  VectorVertex* interm_x = static_cast<VectorVertex*>(_vertices[idx + i]);
1289  _intermediate_x.push_back(&interm_x->values());
1290  }
1291 
1292  idx += _num_intermediate_points;
1293 
1294  _dimension = _dim_x + _num_intermediate_points * _dim_x;
1295 
1296  clearInternalBuffer();
1297 
1298  if (_collocation->isIntermediateControlSubjectToOptim())
1299  {
1300  _intermediate_u_internal_buf = false;
1301  for (int i = 0; i < _num_intermediate_points; ++i)
1302  {
1303  VectorVertex* interm_u = static_cast<VectorVertex*>(_vertices[idx + i]);
1304  _intermediate_u.push_back(&interm_u->values());
1305  }
1306  }
1307  else if (_num_intermediate_points > 0)
1308  {
1309  _intermediate_u_internal_buf = true;
1310  for (int i = 0; i < _num_intermediate_points; ++i)
1311  {
1312  _intermediate_u.push_back(new Eigen::VectorXd(_dynamics->getInputDimension()));
1313  }
1314  }
1315  }
1316 
1317  // implements interface method
1318  int getDimension() const override { return _dimension; }
1319  // implement in child class:
1320  bool isLinear() const override { return false; }
1321 
1322  // bool providesJacobian() const override { return false; }
1323 
1324  bool isLeastSquaresForm() const override { return false; }
1325 
1327  {
1328  assert(_collocation);
1329  assert(_dynamics);
1330  assert(_x1->getDimension() == _dynamics->getStateDimension());
1331  assert(_u1->getDimension() == _dynamics->getInputDimension());
1332  assert(_x2->getDimension() == _dynamics->getStateDimension());
1333  assert(_u2->getDimension() == _dynamics->getInputDimension());
1334 
1335  if (_intermediate_u_internal_buf)
1336  {
1337  _collocation->computeIntermediateControls(_u1->values(), _u2->values(), _intermediate_u);
1338  }
1339 
1340  // preform collocation with given mid points:
1341  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, _intermediate_x,
1342  _intermediate_u, values.head(_dim_x), values.tail(_dim_x * _num_intermediate_points));
1343 
1344  values.head(_dim_x).noalias() -= (_x2->values() - _x1->values());
1345  }
1346 
1347  protected:
1349  {
1350  if (_intermediate_u_internal_buf)
1351  {
1352  for (int i = 0; i < _intermediate_u.size(); ++i) delete _intermediate_u[i];
1353  }
1354  _intermediate_u.clear();
1355  }
1356 
1357  private:
1360 
1361  Eigen::VectorXd _dynamics_quadrature;
1362  Eigen::VectorXd _midpoint_error;
1363 
1364  int _dim_x = 0;
1365  int _dimension = 0;
1366 
1367  int _num_intermediate_points = 0;
1368  std::vector<Eigen::VectorXd*> _intermediate_x;
1369  std::vector<Eigen::VectorXd*> _intermediate_u;
1370  bool _intermediate_u_internal_buf = false;
1371 
1372  const VectorVertex* _x1 = nullptr;
1373  const VectorVertex* _u1 = nullptr;
1374  const VectorVertex* _x2 = nullptr;
1375  const VectorVertex* _u2 = nullptr;
1376  const ScalarVertex* _dt = nullptr;
1377 
1378  public:
1380 };
1381 
1382 // vertices
1383 // x1
1384 // u1
1385 // dt
1386 // x2
1387 // u2
1388 // intermediate_x1
1389 // ..
1390 // intermediate_u1
1391 // ..
1393 {
1394  public:
1395  using Ptr = std::shared_ptr<CombinedUncompressedCollocationEdge>;
1396  using UPtr = std::unique_ptr<CombinedUncompressedCollocationEdge>;
1397 
1399  StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq,
1400  StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k)
1401  : MixedEdge<>(5 + quadrature->getNumIntermediatePoints() * (1 + (int)quadrature->isIntermediateControlSubjectToOptim())),
1402  _dynamics(dynamics),
1403  _collocation(quadrature),
1404  _stage_cost(stage_cost),
1405  _stage_eq(stage_eq),
1406  _stage_ineq(stage_ineq),
1407  _k(k),
1408  _eval_intermediate_constr(eval_intermediate_constr),
1409  _num_intermediate_points(quadrature->getNumIntermediatePoints())
1410  {
1411  }
1412 
1413  virtual ~CombinedUncompressedCollocationEdge() { clearInternalBuffer(); }
1414 
1415  void finalize()
1416  {
1417  int x_dim = _dynamics->getStateDimension();
1418 
1419  _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0; // we have only a single obejtive value
1420  _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
1421  // Note, we have additional equations for intermediate points in uncompressed mode:
1422  _dim_eq = x_dim + _dim_int_eq + x_dim * _num_intermediate_points;
1423  _dim_int_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
1424  _dim_ineq = _dim_int_ineq;
1425  _dim_dyn = _dynamics->getStateDimension();
1426 
1427  _dynamics_quadrature.resize(x_dim);
1428  _midpoint_error.resize(x_dim * _num_intermediate_points);
1429 
1430  _x1 = static_cast<const VectorVertex*>(_vertices[0]);
1431  _u1 = static_cast<const VectorVertex*>(_vertices[1]);
1432  _dt = static_cast<const ScalarVertex*>(_vertices[2]);
1433  _u2 = static_cast<const VectorVertex*>(_vertices[3]);
1434  _x2 = static_cast<const VectorVertex*>(_vertices[4]);
1435 
1436  int idx = 5;
1437  _intermediate_x.clear();
1438  for (int i = 0; i < _num_intermediate_points; ++i)
1439  {
1440  VectorVertex* interm_x = static_cast<VectorVertex*>(_vertices[idx + i]);
1441  _intermediate_x.push_back(&interm_x->values());
1442  }
1443 
1444  idx += _num_intermediate_points;
1445 
1446  clearInternalBuffer();
1447 
1448  if (_collocation->isIntermediateControlSubjectToOptim())
1449  {
1450  _intermediate_u_internal_buf = false;
1451  for (int i = 0; i < _num_intermediate_points; ++i)
1452  {
1453  VectorVertex* interm_u = static_cast<VectorVertex*>(_vertices[idx + i]);
1454  _intermediate_u.push_back(&interm_u->values());
1455  }
1456  }
1457  else if (_num_intermediate_points > 0)
1458  {
1459  _intermediate_u_internal_buf = true;
1460  for (int i = 0; i < _num_intermediate_points; ++i)
1461  {
1462  _intermediate_u.push_back(new Eigen::VectorXd(_dynamics->getInputDimension()));
1463  }
1464  }
1465 
1466  activateIntermediateConstraints();
1467  }
1468 
1469  // implements interface method
1470  int getObjectiveDimension() const override { return _dim_obj; }
1471  // implements interface method
1472  int getEqualityDimension() const override { return _dim_eq; }
1473  int getInequalityDimension() const override { return _dim_ineq; }
1474 
1475  // implement in child class:
1476  bool isObjectiveLinear() const override { return false; }
1477  bool isEqualityLinear() const override { return false; }
1478  bool isInequalityLinear() const override { return false; }
1479 
1480  // bool providesJacobian() const override { return false; }
1481 
1482  bool isObjectiveLeastSquaresForm() const override { return false; }
1483 
1484  void precompute() override
1485  {
1486  assert(_collocation);
1487  assert(_dynamics);
1488 
1489  assert(_x1->getDimension() == _dynamics->getStateDimension());
1490  assert(_u1->getDimension() == _dynamics->getInputDimension());
1491  assert(_u2->getDimension() == _dynamics->getInputDimension());
1492  assert(_x2->getDimension() == _dynamics->getStateDimension());
1493 
1494  if (_intermediate_u_internal_buf)
1495  {
1496  _collocation->computeIntermediateControls(_u1->values(), _u2->values(), _intermediate_u);
1497  }
1498 
1499  // preform collocation with given mid points:
1500  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), *_dynamics, _intermediate_x,
1501  _intermediate_u, _dynamics_quadrature, _midpoint_error);
1502  }
1503 
1505  {
1506  // evaluate quadrature formula with previously computed intermediate states
1507  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
1508  _stage_cost->computeIntegralStateControlTerm(_k, x, u, result);
1509  };
1510  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
1511  integrand, obj_values);
1512  }
1513 
1515  {
1516  eq_values.head(_dim_dyn).noalias() = _x2->values() - _x1->values() - _dynamics_quadrature;
1517  eq_values.segment(_dim_dyn, _midpoint_error.size()).noalias() = _midpoint_error;
1518 
1519  int idx = _midpoint_error.size() + _dim_dyn;
1520 
1521  if (_dim_int_eq > 0)
1522  {
1523  // evaluate quadrature formula with previously computed intermediate states
1524  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
1525  _stage_eq->computeIntegralStateControlTerm(_k, x, u, result);
1526  };
1527  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
1528  integrand, eq_values.segment(idx, _dim_int_eq));
1529  idx += _dim_int_eq;
1530  }
1531 
1532  // in case we consider intermediate constraints
1533  if (_dim_nonint_eq > 0)
1534  {
1535  for (int i = 0; i < (int)_intermediate_x.size(); ++i)
1536  {
1537  if (i < _intermediate_u.size())
1538  {
1539  _stage_eq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], _u1->values(), _dt->value(),
1540  eq_values.segment(idx, _dim_nonint_eq));
1541  }
1542  idx += _dim_nonint_eq;
1543  }
1544  assert(idx == getEqualityDimension());
1545  }
1546  }
1548  {
1549  if (_dim_int_ineq > 0)
1550  {
1551  // evaluate quadrature formula with previously computed intermediate states
1552  auto integrand = [this](const Eigen::VectorXd& x, const Eigen::VectorXd& u, Eigen::Ref<Eigen::VectorXd> result) {
1553  _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result);
1554  };
1555  _collocation->quadrature(_x1->values(), _u1->values(), _x2->values(), _u2->values(), _dt->value(), _intermediate_x, _intermediate_u,
1556  integrand, ineq_values);
1557  }
1558 
1559  // in case we consider intermediate constraints, the following dimensions are > 0
1560  int cur_idx = _dim_int_eq;
1561  if (_dim_nonint_ineq > 0)
1562  {
1563  for (int i = 0; i < _collocation->getNumIntermediatePoints(); ++i)
1564  {
1565  _stage_ineq->computeConcatenatedNonIntegralStateTerms(_k, _x1->values(), _u1->values(), _dt->value(),
1566  ineq_values.segment(cur_idx, _dim_nonint_ineq));
1567  cur_idx += _dim_nonint_eq;
1568  }
1569  }
1570  assert(cur_idx == getInequalityDimension());
1571  }
1572 
1573  protected:
1575  {
1576  // get number inequalities in case _eval_intermediate_constr is turned on
1577  // TODO(roesmann) currently we only evaluate intermediate states, since u is usually just a linear function
1578  if (_eval_intermediate_constr)
1579  {
1580  int num_interm_states = _collocation->getNumIntermediatePoints();
1581  _dim_nonint_eq = _stage_eq ? _stage_eq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
1582  _dim_nonint_ineq = _stage_ineq ? _stage_ineq->getConcatenatedNonIntegralStateTermDimension(_k) : 0;
1583 
1584  // update overall dimensions
1585  _dim_eq = _dim_int_eq + _dim_dyn + _dim_dyn * _num_intermediate_points + num_interm_states * _dim_nonint_eq;
1586  _dim_ineq = _dim_int_ineq + num_interm_states * _dim_nonint_ineq;
1587  }
1588  }
1589 
1591  {
1592  if (_intermediate_u_internal_buf)
1593  {
1594  for (int i = 0; i < _intermediate_u.size(); ++i) delete _intermediate_u[i];
1595  }
1596  _intermediate_u.clear();
1597  }
1598 
1599  private:
1602 
1606 
1607  int _dim_obj = 1;
1608  int _dim_eq = 0;
1609  int _dim_int_eq = 0;
1610  int _dim_ineq = 0;
1611  int _dim_int_ineq = 0;
1612  int _dim_dyn = 0;
1613 
1614  int _dim_nonint_eq = 0;
1615  int _dim_nonint_ineq = 0;
1616 
1617  int _k = 0;
1618 
1619  bool _eval_intermediate_constr = false;
1620 
1621  Eigen::VectorXd _dynamics_quadrature;
1622  Eigen::VectorXd _midpoint_error;
1623 
1624  int _num_intermediate_points = 0;
1625  std::vector<Eigen::VectorXd*> _intermediate_x;
1626  std::vector<Eigen::VectorXd*> _intermediate_u;
1627  bool _intermediate_u_internal_buf = false;
1628 
1629  const VectorVertex* _x1 = nullptr;
1630  const VectorVertex* _u1 = nullptr;
1631  const VectorVertex* _u2 = nullptr;
1632  const VectorVertex* _x2 = nullptr;
1633  const ScalarVertex* _dt = nullptr;
1634 
1635  public:
1637 };
1638 
1639 } // namespace corbo
1640 
1641 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_COLLOCATION_EDGES_H_
UncompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature)
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
void setCollocationMethod(QuadratureCollocationInterface::Ptr collocation)
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::vector< const Eigen::VectorXd * > _x1_points
bool isObjectiveLeastSquaresForm() const override
std::shared_ptr< UncompressedCollocationEdge > Ptr
SystemDynamicsInterface::Ptr _dynamics
std::vector< const Eigen::VectorXd * > _u1_points
Scalar * x
CombinedCompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq, StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k, VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, VectorVertex &u2, VectorVertex &x2)
#define PRINT_WARNING_COND_NAMED(cond, msg)
Definition: console.h:257
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
return int(ret)+1
std::shared_ptr< CombinedCompressedCollocationEdge > Ptr
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
QuadratureCollocationInterface::Ptr _collocation
void setCollocationMethod(QuadratureCollocationInterface::Ptr quadrature)
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
VertexContainer _vertices
Vertex container.
Definition: edge.h:281
std::shared_ptr< QuadratureCollocationDynamicsOnly > Ptr
std::vector< Eigen::VectorXd * > _intermediate_x
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
std::vector< Eigen::VectorXd * > _intermediate_u
QuadratureCollocationInterface::Ptr _collocation
QuadratureCollocationEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature, StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq, StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k)
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:690
QuadratureCollocationInterface::Ptr _collocation
bool isObjectiveLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
QuadratureCollocationInterface::Ptr _collocation
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
CompressedCollocationMultipleControlsEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature)
bool isEqualityLinear() const override
void setCollocationMethod(QuadratureCollocationInterface::Ptr quadrature)
bool isObjectiveLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
Vertex implementation for scalar values.
Definition: scalar_vertex.h:50
StageInequalityConstraint::Ptr _stage_ineq
std::shared_ptr< ConstControlCombinedCompressedCollocationEdge > Ptr
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
std::unique_ptr< UncompressedCollocationEdge > UPtr
StageInequalityConstraint::Ptr _stage_ineq
bool isObjectiveLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
QuadratureCollocationInterface::Ptr _collocation
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
std::shared_ptr< StageEqualityConstraint > Ptr
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
SystemDynamicsInterface::Ptr _dynamics
SystemDynamicsInterface::Ptr _dynamics
QuadratureCollocationInterface::Ptr _collocation
std::shared_ptr< StageInequalityConstraint > Ptr
SystemDynamicsInterface::Ptr _dynamics
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::vector< const Eigen::VectorXd * > _u1_points
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
std::vector< const Eigen::VectorXd * > _x1_points
std::vector< Eigen::VectorXd * > _intermediate_u
QuadratureCollocationInterface::Ptr _collocation
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.
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
std::shared_ptr< QuadratureCollocationEdge > Ptr
std::unique_ptr< CombinedUncompressedCollocationEdge > UPtr
int getInequalityDimension() const override
CompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, VectorVertex &u2, VectorVertex &x2)
void clear() override
Clear complete backup container.
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
std::shared_ptr< QuadratureCollocationInterface > Ptr
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
std::shared_ptr< CompressedCollocationEdge > Ptr
const double & value() const
Get underlying value.
std::unique_ptr< ConstControlCombinedCompressedCollocationEdge > UPtr
std::unique_ptr< QuadratureCollocationEdge > UPtr
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
QuadratureCollocationDynamicsOnly(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature)
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
int getEqualityDimension() const override
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
bool isObjectiveLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
SystemDynamicsInterface::Ptr _dynamics
std::vector< Eigen::VectorXd * > _intermediate_x
StageEqualityConstraint::Ptr _stage_eq
std::shared_ptr< CombinedCompressedCollocationMultipleControlsEdge > Ptr
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
CombinedCompressedCollocationMultipleControlsEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature, StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq, StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k)
std::shared_ptr< CompressedCollocationMultipleControlsEdge > Ptr
std::shared_ptr< StageCost > Ptr
ConstControlCombinedCompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq, StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k, VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, VectorVertex &x2)
std::unique_ptr< CompressedCollocationEdge > UPtr
std::shared_ptr< CombinedUncompressedCollocationEdge > Ptr
std::unique_ptr< QuadratureCollocationDynamicsOnly > UPtr
virtual ~QuadratureCollocationDynamicsOnly()=default
const Eigen::VectorXd & values() const
Read-access to the underlying value vector.
bool isInequalityLinear() const override
std::vector< Eigen::VectorXd * > _intermediate_u
std::vector< Eigen::VectorXd * > _intermediate_x
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
int getObjectiveDimension() const override
StageInequalityConstraint::Ptr _stage_ineq
std::unique_ptr< CompressedCollocationMultipleControlsEdge > UPtr
std::vector< Eigen::VectorXd * > _intermediate_u
StageEqualityConstraint::Ptr _stage_eq
std::shared_ptr< SystemDynamicsInterface > Ptr
std::unique_ptr< CombinedCompressedCollocationMultipleControlsEdge > UPtr
QuadratureCollocationInterface::Ptr _collocation
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
std::vector< Eigen::VectorXd * > _intermediate_x
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::unique_ptr< CombinedCompressedCollocationEdge > UPtr
CombinedUncompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature, StageCost::Ptr stage_cost, StageEqualityConstraint::Ptr stage_eq, StageInequalityConstraint::Ptr stage_ineq, bool eval_intermediate_constr, int k)
bool isObjectiveLinear() 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:46