25 #ifndef SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_FINITE_DIFFERENCES_COLLOCATION_EDGES_H_ 26 #define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_FINITE_DIFFERENCES_COLLOCATION_EDGES_H_ 46 using Ptr = std::shared_ptr<FDCollocationEdge>;
47 using UPtr = std::unique_ptr<FDCollocationEdge>;
65 bool isLinear()
const override {
return false; }
101 using Ptr = std::shared_ptr<TrapezoidalIntegralCostEdge>;
102 using UPtr = std::unique_ptr<TrapezoidalIntegralCostEdge>;
108 assert(stage_cost->getIntegralStateControlTermDimension(_k) == 1);
132 _stage_cost->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), _cost1);
133 _stage_cost->computeIntegralStateControlTerm(_k, x2->
values(), u1->
values(), _cost2);
134 values[0] = 0.5 * dt->
value() * (_cost1[0] + _cost2[0]);
152 using Ptr = std::shared_ptr<TrapezoidalIntegralEqualityDynamicsEdge>;
153 using UPtr = std::unique_ptr<TrapezoidalIntegralEqualityDynamicsEdge>;
159 _dim_dyn =
_dynamics->getStateDimension();
160 _dim_int_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
161 _dim_eq = _dim_dyn + _dim_int_eq;
163 _eq1.resize(_dim_int_eq);
164 _eq2.resize(_dim_int_eq);
191 _stage_eq->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), _eq1);
192 _stage_eq->computeIntegralStateControlTerm(_k, x2->
values(), u1->
values(), _eq2);
193 values.tail(_dim_int_eq).noalias() = 0.5 * dt->
value() * (_eq1 + _eq2);
221 using Ptr = std::shared_ptr<TrapezoidalIntegralEqualityEdge>;
222 using UPtr = std::unique_ptr<TrapezoidalIntegralEqualityEdge>;
228 _dim_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
230 _eq1.resize(_dim_eq);
231 _eq2.resize(_dim_eq);
252 _stage_eq->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), _eq1);
253 _stage_eq->computeIntegralStateControlTerm(_k, x2->
values(), u1->
values(), _eq2);
254 values.noalias() = 0.5 * dt->
value() * (_eq1 + _eq2);
274 using Ptr = std::shared_ptr<TrapezoidalIntegralInequalityEdge>;
275 using UPtr = std::unique_ptr<TrapezoidalIntegralInequalityEdge>;
281 _dim_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
283 _ineq1.resize(_dim_ineq);
284 _ineq2.resize(_dim_ineq);
306 _stage_ineq->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), _ineq1);
307 _stage_ineq->computeIntegralStateControlTerm(_k, x2->
values(), u1->
values(), _ineq2);
308 values.noalias() = 0.5 * dt->
value() * (_ineq1 + _ineq2);
328 using Ptr = std::shared_ptr<LeftSumCostEdge>;
329 using UPtr = std::unique_ptr<LeftSumCostEdge>;
335 assert(stage_cost->getIntegralStateControlTermDimension(_k) == 1);
355 _stage_cost->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), values);
356 values *= dt->
value();
371 using Ptr = std::shared_ptr<LeftSumEqualityEdge>;
372 using UPtr = std::unique_ptr<LeftSumEqualityEdge>;
377 _dim_eq = _stage_eq ? _stage_eq->getIntegralStateControlTermDimension(_k) : 0;
397 _stage_eq->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), values);
398 values *= dt->
value();
415 using Ptr = std::shared_ptr<LeftSumInequalityEdge>;
416 using UPtr = std::unique_ptr<LeftSumInequalityEdge>;
421 _dim_ineq = _stage_ineq ? _stage_ineq->getIntegralStateControlTermDimension(_k) : 0;
442 _stage_ineq->computeIntegralStateControlTerm(_k, x1->
values(), u1->
values(), values);
443 values *= dt->
value();
459 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_STRUCTURED_OCP_EDGES_FINITE_DIFFERENCES_COLLOCATION_EDGES_H_ std::shared_ptr< TrapezoidalIntegralInequalityEdge > Ptr
TrapezoidalIntegralInequalityEdge(VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageInequalityConstraint::Ptr stage_ineq, int k)
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
std::shared_ptr< LeftSumEqualityEdge > Ptr
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
StageEqualityConstraint::Ptr _stage_eq
TrapezoidalIntegralCostEdge(VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageCost::Ptr stage_cost, int k)
FiniteDifferencesCollocationInterface::Ptr _fd_eval
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
LeftSumCostEdge(VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, StageCost::Ptr stage_cost, int k)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
LeftSumEqualityEdge(VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, StageEqualityConstraint::Ptr stage_eq, int k)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
StageCost::Ptr _stage_cost
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::shared_ptr< LeftSumCostEdge > Ptr
Vertex implementation for scalar values.
StageEqualityConstraint::Ptr _stage_eq
std::unique_ptr< LeftSumCostEdge > UPtr
std::unique_ptr< LeftSumEqualityEdge > UPtr
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
std::unique_ptr< TrapezoidalIntegralEqualityDynamicsEdge > UPtr
std::shared_ptr< TrapezoidalIntegralEqualityEdge > Ptr
std::shared_ptr< StageEqualityConstraint > Ptr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
std::shared_ptr< StageInequalityConstraint > Ptr
std::shared_ptr< FDCollocationEdge > Ptr
std::shared_ptr< TrapezoidalIntegralCostEdge > Ptr
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
TrapezoidalIntegralEqualityDynamicsEdge(SystemDynamicsInterface::Ptr dynamics, VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageEqualityConstraint::Ptr stage_eq, int k)
std::unique_ptr< TrapezoidalIntegralEqualityEdge > UPtr
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
StageEqualityConstraint::Ptr _stage_eq
std::unique_ptr< LeftSumInequalityEdge > UPtr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
void setFiniteDifferencesCollocationMethod(FiniteDifferencesCollocationInterface::Ptr fd_eval)
std::shared_ptr< LeftSumInequalityEdge > Ptr
std::unique_ptr< TrapezoidalIntegralInequalityEdge > UPtr
int getDimension() const override
Return number of elements/values/components stored in this vertex.
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
FDCollocationEdge(SystemDynamicsInterface::Ptr dynamics, VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt)
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
void setFiniteDifferencesCollocationMethod(FiniteDifferencesCollocationInterface::Ptr fd_eval)
const double & value() const
Get underlying value.
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
A matrix or vector expression mapping an existing expression.
FiniteDifferencesCollocationInterface::Ptr _fd_eval
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
LeftSumInequalityEdge(VectorVertex &x1, VectorVertex &u1, ScalarVertex &dt, StageInequalityConstraint::Ptr stage_ineq, int k)
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
StageInequalityConstraint::Ptr _stage_ineq
std::unique_ptr< FDCollocationEdge > UPtr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
void computeValues(Eigen::Ref< Eigen::VectorXd > values) override
Compute function values.
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
Templated base edge class that stores an arbitary number of value.
Vertex implementation that stores an Eigen::VectorXd (dynamic dimension)
std::shared_ptr< StageCost > Ptr
const Eigen::VectorXd & values() const
Read-access to the underlying value vector.
TrapezoidalIntegralEqualityEdge(VectorVertex &x1, VectorVertex &u1, VectorVertex &x2, ScalarVertex &dt, StageEqualityConstraint::Ptr stage_eq, int k)
const VertexContainer _vertices
Vertex container.
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
StageInequalityConstraint::Ptr _stage_ineq
SystemDynamicsInterface::Ptr _dynamics
std::shared_ptr< TrapezoidalIntegralEqualityDynamicsEdge > Ptr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::shared_ptr< FiniteDifferencesCollocationInterface > Ptr
int getDimension() const override
Get dimension of the edge (dimension of the cost-function/constraint value vector) ...
std::shared_ptr< SystemDynamicsInterface > Ptr
std::unique_ptr< TrapezoidalIntegralCostEdge > UPtr
bool isLeastSquaresForm() const override
Defines if the edge is formulated as Least-Squares form.
SystemDynamicsInterface::Ptr _dynamics
bool isLinear() const override
Return true if the edge is linear (and hence its Hessian is always zero)
StageCost::Ptr _stage_cost