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_ 45 using Ptr = std::shared_ptr<QuadratureCollocationDynamicsOnly>;
46 using UPtr = std::unique_ptr<QuadratureCollocationDynamicsOnly>;
49 :
Edge<>(5 + quadrature->getNumIntermediateStates() + quadrature->getNumIntermediateControls()),
86 bool isLinear()
const override {
return false; }
124 const Eigen::VectorXd*
_x2 =
nullptr;
125 const Eigen::VectorXd*
_u2 =
nullptr;
136 using Ptr = std::shared_ptr<QuadratureCollocationEdge>;
137 using UPtr = std::unique_ptr<QuadratureCollocationEdge>;
141 bool eval_intermediate_constr,
int k)
142 :
MixedEdge<>(5 + quadrature->getNumIntermediateStates() + quadrature->getNumIntermediateControls()),
145 _stage_cost(stage_cost),
147 _stage_ineq(stage_ineq),
149 _eval_intermediate_constr(eval_intermediate_constr),
153 assert(quadrature->isIntermediateControlSubjectToOptim());
158 int x_dim =
_dynamics->getStateDimension();
160 _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0;
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();
187 _quadrature_values.resize(_dim_obj + _dim_dyn + _dim_int_eq + _dim_int_ineq);
189 configureIntegrand();
230 eq_values.head(_dim_dyn).noalias() = *
_x2 - _x1->values() - _quadrature_values.segment(_dim_obj, _dim_dyn);
233 eq_values.segment(_dim_dyn, _dim_int_eq).noalias() = _quadrature_values.segment(_dim_obj + _dim_dyn, _dim_int_eq);
251 if (_dim_int_ineq > 0)
253 ineq_values = _quadrature_values.tail(_dim_int_ineq);
325 _dynamics->dynamics(x, u, result.segment(_dim_obj, _dim_dyn));
329 assert(_dim_obj == 1);
330 _stage_cost->computeIntegralStateControlTerm(_k, x, u, result.head(1));
337 _stage_eq->computeIntegralStateControlTerm(_k, x, u, result.segment(idx, _dim_int_eq));
341 if (_dim_int_ineq > 0)
343 _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result.segment(idx, _dim_int_eq));
346 assert(idx == result.size());
362 int _dim_int_ineq = 0;
365 int _dim_nonint_eq = 0;
366 int _dim_nonint_ineq = 0;
373 bool _eval_intermediate_constr =
false;
380 std::function<void(const Eigen::VectorXd&, const Eigen::VectorXd&, Eigen::Ref<Eigen::VectorXd>)> _integrand;
385 const Eigen::VectorXd*
_x2 =
nullptr;
386 const Eigen::VectorXd*
_u2 =
nullptr;
397 using Ptr = std::shared_ptr<CompressedCollocationEdge>;
398 using UPtr = std::unique_ptr<CompressedCollocationEdge>;
428 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
429 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
430 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
431 assert(
_u2->getDimension() ==
_dynamics->getInputDimension());
438 assert(!collocation->isIntermediateControlSubjectToOptim() && collocation->isSupportingCompressedStatesMode());
459 using Ptr = std::shared_ptr<CompressedCollocationMultipleControlsEdge>;
460 using UPtr = std::unique_ptr<CompressedCollocationMultipleControlsEdge>;
463 :
Edge<>(5 + quadrature->getNumIntermediatePoints()),
466 _num_intermediate_points(quadrature->getNumIntermediatePoints())
468 assert(quadrature->isIntermediateControlSubjectToOptim());
484 _intermediate_u.
clear();
485 for (
int i = 0; i < _num_intermediate_points; ++i)
488 _intermediate_u.push_back(&interm_u->
values());
491 clearInternalBuffer();
493 for (
int i = 0; i < _num_intermediate_points; ++i)
495 _intermediate_x.push_back(
new Eigen::VectorXd(
_dim_x));
512 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
513 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
514 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
515 assert(
_u2->getDimension() ==
_dynamics->getInputDimension());
521 values.noalias() -= (
_x2->values() - _x1->values());
527 for (
int i = 0; i < _intermediate_x.size(); ++i)
delete _intermediate_x[i];
528 _intermediate_x.clear();
554 using Ptr = std::shared_ptr<ConstControlCombinedCompressedCollocationEdge>;
555 using UPtr = std::unique_ptr<ConstControlCombinedCompressedCollocationEdge>;
563 _stage_cost(stage_cost),
565 _stage_ineq(stage_ineq),
567 _eval_intermediate_constr(eval_intermediate_constr)
569 int x_dim =
_dynamics->getStateDimension();
570 _dynamics_quadrature.resize(x_dim);
572 _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0;
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();
587 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
delete _intermediate_x[i];
610 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
611 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
612 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
621 _stage_cost->computeIntegralStateControlTerm(_k, x, _u1->values(), result);
628 eq_values.head(_dynamics_quadrature.size()).noalias() =
_x2->values() - _x1->values() - _dynamics_quadrature;
633 _stage_eq->computeIntegralStateControlTerm(_k, x, _u1->values(), result);
636 eq_values.segment(_dim_dyn, _dim_int_eq));
640 if (_dim_nonint_eq > 0)
642 int cur_idx = _dim_dyn + _dim_int_eq;
644 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
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;
650 assert(cur_idx == getEqualityDimension());
655 if (_dim_int_ineq > 0)
659 _stage_ineq->computeIntegralStateControlTerm(_k, x, _u1->values(), result);
665 int cur_idx = _dim_int_eq;
666 if (_dim_nonint_ineq > 0)
668 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
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;
679 for (
int i = 0; i < _intermediate_x.size(); ++i)
681 for (
int j = 0; j < _intermediate_x[i]->size(); ++j)
683 if (_x1->hasFiniteLowerBound(j))
685 ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j];
694 for (
int i = 0; i < _intermediate_x.size(); ++i)
696 for (
int j = 0; j < _intermediate_x[i]->size(); ++j)
698 if (_x1->hasFiniteUpperBound(j))
700 ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j];
705 assert(cur_idx == getInequalityDimension());
710 assert(!quadrature->isIntermediateControlSubjectToOptim() && quadrature->isSupportingCompressedStatesMode());
712 _intermediate_x.resize(_collocation->getNumIntermediatePoints(),
new Eigen::VectorXd(_dim_dyn));
714 activateIntermediateConstraints();
721 if (_eval_intermediate_constr)
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;
729 _dim_lb_x = _x1->getNumberFiniteLowerBounds(
false);
730 _dim_ub_x = _x1->getNumberFiniteUpperBounds(
false);
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);
750 int _dim_int_ineq = 0;
753 int _dim_nonint_eq = 0;
754 int _dim_nonint_ineq = 0;
761 bool _eval_intermediate_constr =
false;
778 using Ptr = std::shared_ptr<CombinedCompressedCollocationEdge>;
779 using UPtr = std::unique_ptr<CombinedCompressedCollocationEdge>;
787 _stage_cost(stage_cost),
789 _stage_ineq(stage_ineq),
791 _eval_intermediate_constr(eval_intermediate_constr)
793 int x_dim =
_dynamics->getStateDimension();
794 _dynamics_quadrature.resize(x_dim);
796 _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0;
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();
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];
836 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
837 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
838 assert(
_u2->getDimension() ==
_dynamics->getInputDimension());
839 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
842 _intermediate_x, _intermediate_u);
849 _stage_cost->computeIntegralStateControlTerm(_k, x, u, result);
851 _collocation->quadrature(_x1->values(), _u1->values(),
_x2->values(),
_u2->values(),
_dt->
value(), _intermediate_x, _intermediate_u,
852 integrand, obj_values);
857 eq_values.head(_dynamics_quadrature.size()).noalias() =
_x2->values() - _x1->values() - _dynamics_quadrature;
862 _stage_eq->computeIntegralStateControlTerm(_k, x, u, result);
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));
869 if (_dim_nonint_eq > 0)
871 int cur_idx = _dim_dyn + _dim_int_eq;
872 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
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;
878 assert(cur_idx == getEqualityDimension());
883 if (_dim_int_ineq > 0)
887 _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result);
889 _collocation->quadrature(_x1->values(), _u1->values(),
_x2->values(),
_u2->values(),
_dt->
value(), _intermediate_x, _intermediate_u,
890 integrand, ineq_values);
894 int cur_idx = _dim_int_eq;
895 if (_dim_nonint_ineq > 0)
897 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
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;
908 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
910 for (
int j = 0; j < _intermediate_x[i]->size(); ++j)
912 if (_x1->hasFiniteLowerBound(j))
914 ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j];
923 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
925 for (
int j = 0; j < _intermediate_x[i]->size(); ++j)
927 if (_x1->hasFiniteUpperBound(j))
929 ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j];
934 assert(cur_idx == getInequalityDimension());
939 assert(!quadrature->isIntermediateControlSubjectToOptim() && quadrature->isSupportingCompressedStatesMode());
942 _intermediate_x.resize(_collocation->getNumIntermediatePoints(),
new Eigen::VectorXd(_dim_dyn));
943 _intermediate_u.resize(_collocation->getNumIntermediatePoints(),
new Eigen::VectorXd(
_dynamics->getInputDimension()));
945 activateIntermediateConstraints();
953 if (_eval_intermediate_constr)
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;
961 _dim_lb_x = _x1->getNumberFiniteLowerBounds(
false);
962 _dim_ub_x = _x1->getNumberFiniteUpperBounds(
false);
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);
982 int _dim_int_ineq = 0;
985 int _dim_nonint_eq = 0;
986 int _dim_nonint_ineq = 0;
993 bool _eval_intermediate_constr =
false;
1012 using Ptr = std::shared_ptr<CombinedCompressedCollocationMultipleControlsEdge>;
1013 using UPtr = std::unique_ptr<CombinedCompressedCollocationMultipleControlsEdge>;
1018 :
MixedEdge<>(5 + quadrature->getNumIntermediatePoints()),
1021 _stage_cost(stage_cost),
1022 _stage_eq(stage_eq),
1023 _stage_ineq(stage_ineq),
1025 _eval_intermediate_constr(eval_intermediate_constr),
1026 _num_intermediate_points(quadrature->getNumIntermediatePoints())
1028 assert(quadrature->isIntermediateControlSubjectToOptim());
1033 int x_dim =
_dynamics->getStateDimension();
1034 _dynamics_quadrature.resize(x_dim);
1036 _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0;
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();
1050 _intermediate_u.
clear();
1051 for (
int i = 0; i < _num_intermediate_points; ++i)
1054 _intermediate_u.push_back(&interm_u->
values());
1057 clearInternalBuffer();
1059 for (
int i = 0; i < _num_intermediate_points; ++i)
1061 _intermediate_x.push_back(
new Eigen::VectorXd(_dim_dyn));
1064 activateIntermediateConstraints();
1089 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
1090 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
1091 assert(
_u2->getDimension() ==
_dynamics->getInputDimension());
1092 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
1096 _dynamics_quadrature, _intermediate_x);
1103 _stage_cost->computeIntegralStateControlTerm(_k, x, u, result);
1105 _collocation->quadrature(_x1->values(), _u1->values(),
_x2->values(),
_u2->values(),
_dt->
value(), _intermediate_x, _intermediate_u,
1106 integrand, obj_values);
1111 eq_values.head(_dynamics_quadrature.size()).noalias() =
_x2->values() - _x1->values() - _dynamics_quadrature;
1112 if (_dim_int_eq > 0)
1116 _stage_eq->computeIntegralStateControlTerm(_k, x, u, result);
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));
1123 if (_dim_nonint_eq > 0)
1125 int cur_idx = _dim_dyn + _dim_int_eq;
1126 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
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;
1132 assert(cur_idx == getEqualityDimension());
1137 if (_dim_int_ineq > 0)
1141 _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result);
1143 _collocation->quadrature(_x1->values(), _u1->values(),
_x2->values(),
_u2->values(),
_dt->
value(), _intermediate_x, _intermediate_u,
1144 integrand, ineq_values);
1148 int cur_idx = _dim_int_eq;
1149 if (_dim_nonint_ineq > 0)
1151 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
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;
1162 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
1164 for (
int j = 0; j < _intermediate_x[i]->size(); ++j)
1166 if (_x1->hasFiniteLowerBound(j))
1168 ineq_values(cur_idx++) = _x1->lowerBound()[j] - (*_intermediate_x[i])[j];
1177 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
1179 for (
int j = 0; j < _intermediate_x[i]->size(); ++j)
1181 if (_x1->hasFiniteUpperBound(j))
1183 ineq_values(cur_idx++) = (*_intermediate_x[i])[j] - _x1->upperBound()[j];
1188 assert(cur_idx == getInequalityDimension());
1196 if (_eval_intermediate_constr)
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;
1204 _dim_lb_x = _x1->getNumberFiniteLowerBounds(
false);
1205 _dim_ub_x = _x1->getNumberFiniteUpperBounds(
false);
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);
1215 for (
int i = 0; i < _intermediate_x.size(); ++i)
delete _intermediate_x[i];
1216 _intermediate_x.clear();
1229 int _dim_int_eq = 0;
1231 int _dim_int_ineq = 0;
1234 int _dim_nonint_eq = 0;
1235 int _dim_nonint_ineq = 0;
1242 bool _eval_intermediate_constr =
false;
1263 using Ptr = std::shared_ptr<UncompressedCollocationEdge>;
1264 using UPtr = std::unique_ptr<UncompressedCollocationEdge>;
1267 :
Edge<>(5 + quadrature->getNumIntermediatePoints() * (1 + (
int)quadrature->isIntermediateControlSubjectToOptim())),
1270 _num_intermediate_points(quadrature->getNumIntermediatePoints())
1285 _intermediate_x.
clear();
1286 for (
int i = 0; i < _num_intermediate_points; ++i)
1289 _intermediate_x.push_back(&interm_x->
values());
1292 idx += _num_intermediate_points;
1294 _dimension =
_dim_x + _num_intermediate_points *
_dim_x;
1296 clearInternalBuffer();
1298 if (
_collocation->isIntermediateControlSubjectToOptim())
1300 _intermediate_u_internal_buf =
false;
1301 for (
int i = 0; i < _num_intermediate_points; ++i)
1304 _intermediate_u.push_back(&interm_u->
values());
1307 else if (_num_intermediate_points > 0)
1309 _intermediate_u_internal_buf =
true;
1310 for (
int i = 0; i < _num_intermediate_points; ++i)
1312 _intermediate_u.push_back(
new Eigen::VectorXd(
_dynamics->getInputDimension()));
1330 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
1331 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
1332 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
1333 assert(
_u2->getDimension() ==
_dynamics->getInputDimension());
1335 if (_intermediate_u_internal_buf)
1337 _collocation->computeIntermediateControls(_u1->values(),
_u2->values(), _intermediate_u);
1342 _intermediate_u, values.head(
_dim_x), values.tail(
_dim_x * _num_intermediate_points));
1344 values.head(
_dim_x).noalias() -= (
_x2->values() - _x1->values());
1350 if (_intermediate_u_internal_buf)
1352 for (
int i = 0; i < _intermediate_u.size(); ++i)
delete _intermediate_u[i];
1354 _intermediate_u.clear();
1367 int _num_intermediate_points = 0;
1370 bool _intermediate_u_internal_buf =
false;
1395 using Ptr = std::shared_ptr<CombinedUncompressedCollocationEdge>;
1396 using UPtr = std::unique_ptr<CombinedUncompressedCollocationEdge>;
1401 :
MixedEdge<>(5 + quadrature->getNumIntermediatePoints() * (1 + (
int)quadrature->isIntermediateControlSubjectToOptim())),
1404 _stage_cost(stage_cost),
1405 _stage_eq(stage_eq),
1406 _stage_ineq(stage_ineq),
1408 _eval_intermediate_constr(eval_intermediate_constr),
1409 _num_intermediate_points(quadrature->getNumIntermediatePoints())
1417 int x_dim =
_dynamics->getStateDimension();
1419 _dim_obj = (_stage_cost && _stage_cost->getIntegralStateControlTermDimension(_k) > 0) ? 1 : 0;
1420 _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
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();
1427 _dynamics_quadrature.resize(x_dim);
1428 _midpoint_error.resize(x_dim * _num_intermediate_points);
1437 _intermediate_x.
clear();
1438 for (
int i = 0; i < _num_intermediate_points; ++i)
1441 _intermediate_x.push_back(&interm_x->
values());
1444 idx += _num_intermediate_points;
1446 clearInternalBuffer();
1448 if (
_collocation->isIntermediateControlSubjectToOptim())
1450 _intermediate_u_internal_buf =
false;
1451 for (
int i = 0; i < _num_intermediate_points; ++i)
1454 _intermediate_u.push_back(&interm_u->
values());
1457 else if (_num_intermediate_points > 0)
1459 _intermediate_u_internal_buf =
true;
1460 for (
int i = 0; i < _num_intermediate_points; ++i)
1462 _intermediate_u.push_back(
new Eigen::VectorXd(
_dynamics->getInputDimension()));
1466 activateIntermediateConstraints();
1489 assert(_x1->getDimension() ==
_dynamics->getStateDimension());
1490 assert(_u1->getDimension() ==
_dynamics->getInputDimension());
1491 assert(
_u2->getDimension() ==
_dynamics->getInputDimension());
1492 assert(
_x2->getDimension() ==
_dynamics->getStateDimension());
1494 if (_intermediate_u_internal_buf)
1496 _collocation->computeIntermediateControls(_u1->values(),
_u2->values(), _intermediate_u);
1501 _intermediate_u, _dynamics_quadrature, _midpoint_error);
1508 _stage_cost->computeIntegralStateControlTerm(_k, x, u, result);
1510 _collocation->quadrature(_x1->values(), _u1->values(),
_x2->values(),
_u2->values(),
_dt->
value(), _intermediate_x, _intermediate_u,
1511 integrand, obj_values);
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;
1519 int idx = _midpoint_error.size() + _dim_dyn;
1521 if (_dim_int_eq > 0)
1525 _stage_eq->computeIntegralStateControlTerm(_k, x, u, result);
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));
1533 if (_dim_nonint_eq > 0)
1535 for (
int i = 0; i < (
int)_intermediate_x.size(); ++i)
1537 if (i < _intermediate_u.size())
1539 _stage_eq->computeConcatenatedNonIntegralStateTerms(_k, *_intermediate_x[i], _u1->values(),
_dt->
value(),
1540 eq_values.segment(idx, _dim_nonint_eq));
1542 idx += _dim_nonint_eq;
1544 assert(idx == getEqualityDimension());
1549 if (_dim_int_ineq > 0)
1553 _stage_ineq->computeIntegralStateControlTerm(_k, x, u, result);
1555 _collocation->quadrature(_x1->values(), _u1->values(),
_x2->values(),
_u2->values(),
_dt->
value(), _intermediate_x, _intermediate_u,
1556 integrand, ineq_values);
1560 int cur_idx = _dim_int_eq;
1561 if (_dim_nonint_ineq > 0)
1563 for (
int i = 0; i <
_collocation->getNumIntermediatePoints(); ++i)
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;
1570 assert(cur_idx == getInequalityDimension());
1578 if (_eval_intermediate_constr)
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;
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;
1592 if (_intermediate_u_internal_buf)
1594 for (
int i = 0; i < _intermediate_u.size(); ++i)
delete _intermediate_u[i];
1596 _intermediate_u.clear();
1609 int _dim_int_eq = 0;
1611 int _dim_int_ineq = 0;
1614 int _dim_nonint_eq = 0;
1615 int _dim_nonint_ineq = 0;
1619 bool _eval_intermediate_constr =
false;
1624 int _num_intermediate_points = 0;
1627 bool _intermediate_u_internal_buf =
false;
1641 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_COLLOCATION_EDGES_H_ UncompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature)
SystemDynamicsInterface::Ptr _dynamics
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
bool isInequalityLinear() const override
int getEqualityDimension() const override
void setCollocationMethod(QuadratureCollocationInterface::Ptr collocation)
Eigen::VectorXd _midpoint_error
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
int getInequalityDimension() const override
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
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)
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
SystemDynamicsInterface::Ptr _dynamics
int getEqualityDimension() const override
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
void precompute() override
std::shared_ptr< CombinedCompressedCollocationEdge > Ptr
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
StageEqualityConstraint::Ptr _stage_eq
QuadratureCollocationInterface::Ptr _collocation
void setCollocationMethod(QuadratureCollocationInterface::Ptr quadrature)
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
int getInequalityDimension() const override
VertexContainer _vertices
Vertex container.
std::shared_ptr< QuadratureCollocationDynamicsOnly > Ptr
bool isEqualityLinear() const override
std::vector< Eigen::VectorXd * > _intermediate_x
int _num_intermediate_points
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
void activateIntermediateConstraints()
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)
virtual ~CombinedCompressedCollocationEdge()
void activateIntermediateConstraints()
Eigen::VectorXd _dynamics_quadrature
int _num_intermediate_points
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
QuadratureCollocationInterface::Ptr _collocation
StageCost::Ptr _stage_cost
bool isObjectiveLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
QuadratureCollocationInterface::Ptr _collocation
bool isObjectiveLeastSquaresForm() const override
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
int getInequalityDimension() const override
CompressedCollocationMultipleControlsEdge(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature)
void precompute() override
StageCost::Ptr _stage_cost
bool isEqualityLinear() const override
StageCost::Ptr _stage_cost
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.
const Eigen::VectorXd * _x2
StageInequalityConstraint::Ptr _stage_ineq
std::shared_ptr< ConstControlCombinedCompressedCollocationEdge > Ptr
void clearInternalBuffer()
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
void configureIntegrand()
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
bool isInequalityLinear() const override
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::vector< const Eigen::VectorXd * > _u1_points
StageEqualityConstraint::Ptr _stage_eq
int getObjectiveDimension() const override
bool isInequalityLinear() const override
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
virtual ~CombinedUncompressedCollocationEdge()
std::vector< const Eigen::VectorXd * > _x1_points
std::vector< Eigen::VectorXd * > _intermediate_u
QuadratureCollocationInterface::Ptr _collocation
bool isObjectiveLeastSquaresForm() const override
std::vector< Eigen::VectorXd * > _intermediate_x
bool isEqualityLinear() const override
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
bool isObjectiveLeastSquaresForm() const override
void activateIntermediateConstraints()
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
void precompute() override
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
std::shared_ptr< QuadratureCollocationEdge > Ptr
bool isEqualityLinear() const override
std::unique_ptr< CombinedUncompressedCollocationEdge > UPtr
int getInequalityDimension() const override
bool isEqualityLinear() const override
StageInequalityConstraint::Ptr _stage_ineq
std::vector< Eigen::VectorXd * > _intermediate_x
CompressedCollocationEdge(SystemDynamicsInterface::Ptr dynamics, VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, VectorVertex &u2, VectorVertex &x2)
Eigen::VectorXd _dynamics_quadrature
void clear() override
Clear complete backup container.
StageEqualityConstraint::Ptr _stage_eq
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
void activateIntermediateConstraints()
bool isObjectiveLeastSquaresForm() const override
StageCost::Ptr _stage_cost
int getEqualityDimension() const override
std::shared_ptr< QuadratureCollocationInterface > Ptr
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
std::shared_ptr< CompressedCollocationEdge > Ptr
int getObjectiveDimension() const override
const double & value() const
Get underlying value.
QuadratureCollocationInterface::Ptr _collocation
std::unique_ptr< ConstControlCombinedCompressedCollocationEdge > UPtr
StageCost::Ptr _stage_cost
std::unique_ptr< QuadratureCollocationEdge > UPtr
Eigen::VectorXd _dynamics_quadrature
A matrix or vector expression mapping an existing expression.
QuadratureCollocationDynamicsOnly(SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature)
void precompute() override
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
std::vector< Eigen::VectorXd * > _intermediate_u
int getEqualityDimension() const override
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
Eigen::VectorXd _dynamics_quadrature
bool isObjectiveLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
virtual ~CombinedCompressedCollocationMultipleControlsEdge()
int getEqualityDimension() const override
int getObjectiveDimension() const override
void clearInternalBuffer()
bool isInequalityLinear() const override
void computeEqualityValues(Eigen::Ref< Eigen::VectorXd > eq_values) override
SystemDynamicsInterface::Ptr _dynamics
std::vector< Eigen::VectorXd * > _intermediate_x
StageEqualityConstraint::Ptr _stage_eq
StageInequalityConstraint::Ptr _stage_ineq
std::shared_ptr< CombinedCompressedCollocationMultipleControlsEdge > Ptr
Templated base edge class that stores an arbitary number of value.
Vertex implementation that stores an Eigen::VectorXd (dynamic dimension)
void clearInternalBuffer()
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)
void precompute() override
std::shared_ptr< CompressedCollocationMultipleControlsEdge > Ptr
std::shared_ptr< StageCost > Ptr
Eigen::VectorXd _midpoint_error
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)
SystemDynamicsInterface::Ptr _dynamics
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
virtual ~ConstControlCombinedCompressedCollocationEdge()
std::vector< Eigen::VectorXd * > _intermediate_x
virtual ~CompressedCollocationMultipleControlsEdge()
void clearInternalBuffer()
void computeInequalityValues(Eigen::Ref< Eigen::VectorXd > ineq_values) override
int getObjectiveDimension() const override
StageInequalityConstraint::Ptr _stage_ineq
SystemDynamicsInterface::Ptr _dynamics
std::unique_ptr< CompressedCollocationMultipleControlsEdge > UPtr
std::vector< Eigen::VectorXd * > _intermediate_u
Eigen::VectorXd _quadrature_values
StageEqualityConstraint::Ptr _stage_eq
int getInequalityDimension() const override
Eigen::VectorXd _dynamics_quadrature
std::shared_ptr< SystemDynamicsInterface > Ptr
std::unique_ptr< CombinedCompressedCollocationMultipleControlsEdge > UPtr
const Eigen::VectorXd * _u2
QuadratureCollocationInterface::Ptr _collocation
void computeObjectiveValues(Eigen::Ref< Eigen::VectorXd > obj_values) override
std::vector< Eigen::VectorXd * > _intermediate_x
int getObjectiveDimension() const override
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)