6#include <proxsuite-nlp/modelling/spaces/cartesian-product.hpp>
21 assert(c1 !=
nullptr && c2 !=
nullptr);
26 shared_ptr<BaseData>
createData()
const override;
28 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
36 using CartesianProduct = proxsuite::nlp::CartesianProductTpl<Scalar>;
37 auto get_product_space()
const {
38 return static_cast<CartesianProduct
const *
>(this->
space.get());
40 static Data &data_cast(BaseData &data) {
return static_cast<Data &
>(data); }
51template <
typename Scalar>
53 const ConstVectorRef &u,
55 CartesianProduct
const *space = get_product_space();
56 Data &d = data_cast(data);
57 auto xs = space->split(x);
58 ConstVectorRef u1 = u.head(c1_->nu);
59 ConstVectorRef u2 = u.tail(c2_->nu);
61 c1_->evaluate(xs[0], u1, *d.
data1_);
62 c2_->evaluate(xs[1], u2, *d.
data2_);
67template <
typename Scalar>
69 const ConstVectorRef &u,
71 CartesianProduct
const *space = get_product_space();
72 Data &d = data_cast(data);
73 auto xs = space->split(x);
74 ConstVectorRef u1 = u.head(c1_->nu);
75 ConstVectorRef u2 = u.tail(c2_->nu);
80 c1_->computeGradients(xs[0], u1, d1);
81 c2_->computeGradients(xs[1], u2, d2);
83 d.
Lx_.head(c1_->ndx()) = d1.Lx_;
84 d.
Lx_.tail(c2_->ndx()) = d2.Lx_;
85 d.
Lu_.head(c1_->nu) = d1.Lu_;
86 d.
Lu_.tail(c2_->nu) = d2.Lu_;
89template <
typename Scalar>
91 const ConstVectorRef &u,
93 CartesianProduct
const *space = get_product_space();
94 Data &d = data_cast(data);
95 auto xs = space->split(x);
96 ConstVectorRef u1 = u.head(c1_->nu);
97 ConstVectorRef u2 = u.tail(c2_->nu);
102 c1_->computeHessians(xs[0], u1, d1);
103 c2_->computeHessians(xs[1], u2, d2);
105 d.
Lxx_.topLeftCorner(c1_->ndx(), c1_->ndx()) = d1.Lxx_;
106 d.
Lxx_.bottomRightCorner(c2_->ndx(), c2_->ndx()) = d2.Lxx_;
108 d.
Luu_.topLeftCorner(c1_->nu, c1_->nu) = d1.Luu_;
109 d.
Luu_.bottomRightCorner(c2_->nu, c2_->nu) = d2.Luu_;
111 d.
Lxu_.topLeftCorner(c1_->ndx(), c1_->nu) = d1.Lxu_;
112 d.
Lxu_.bottomRightCorner(c2_->ndx(), c2_->nu) = d2.Lxu_;
117template <
typename Scalar>
119 return std::make_shared<Data>(*
this);
122template <
typename Scalar>
123auto directSum(shared_ptr<CostAbstractTpl<Scalar>>
const &c1,
124 shared_ptr<CostAbstractTpl<Scalar>>
const &c2) {
125 return std::make_shared<DirectSumCostTpl<Scalar>>(c1, c2);
130#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
131#include "./cost-direct-sum.txx"
auto directSum(shared_ptr< CostAbstractTpl< Scalar > > const &c1, shared_ptr< CostAbstractTpl< Scalar > > const &c2)
Stage costs for control problems.
shared_ptr< Manifold > space
State dimension.
shared_ptr< BaseData > data1_
shared_ptr< BaseData > data2_
Data(const DirectSumCostTpl &model)
void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const override
Compute the cost gradients .
shared_ptr< BaseCost > c1_
shared_ptr< BaseData > createData() const override
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const override
Evaluate the cost function.
void computeHessians(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const override
Compute the cost Hessians .
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
shared_ptr< BaseCost > c2_
DirectSumCostTpl(shared_ptr< BaseCost > c1, shared_ptr< BaseCost > c2)
ManifoldAbstractTpl< Scalar > Manifold