4#include <proxsuite-nlp/modelling/spaces/vector-space.hpp>
8template <
typename Scalar>
struct QuadraticCostDataTpl;
15 using Base = CostAbstractTpl<Scalar>;
18 using Data = QuadraticCostDataTpl<Scalar>;
19 using VectorSpace = proxsuite::nlp::VectorSpaceTpl<Scalar, Eigen::Dynamic>;
37 return std::make_shared<VectorSpace>((
int)
nx);
51 const ConstMatrixRef &w_cross,
62 VectorXs::Zero(w_u.cols())) {}
65 const ConstMatrixRef &w_cross)
67 VectorXs::Zero(w_u.cols())) {}
69 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
72 d.w_times_x_.noalias() =
Wxx_ * x;
73 d.w_times_u_.noalias() =
Wuu_ * u;
75 d.cross_x_.noalias() =
Wxu_ * u;
76 d.cross_u_.noalias() =
Wxu_.transpose() * x;
78 d.w_times_x_ += d.cross_x_;
79 d.w_times_u_ += d.cross_u_;
96 auto data = std::make_shared<Data>(this->
ndx(), this->
nu);
100 data->Lux_ =
Wxu_.transpose();
119 static void _check_dim_equal(
long n,
long m,
const std::string &msg =
"") {
122 "Dimensions inconsistent: got {:d} and {:d}{}.\n", n, m, msg));
125 void debug_check_dims()
const {
126 _check_dim_equal(
Wxx_.rows(),
Wxx_.cols(),
" for x weights");
127 _check_dim_equal(
Wuu_.rows(),
Wuu_.cols(),
" for u weights");
128 _check_dim_equal(
Wxu_.rows(), this->ndx(),
" for cross-term weight");
129 _check_dim_equal(
Wxu_.cols(), this->nu,
" for cross-term weight");
131 " for x weights and intercept");
133 " for u weights and intercept");
137template <
typename Scalar>
139 using Base = CostDataAbstractTpl<Scalar>;
155#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
156#include "./quad-costs.txx"
#define ALIGATOR_RUNTIME_ERROR(msg)
Stage costs for control problems.
QuadraticCostDataTpl(const int nx, const int nu)
typename Base::VectorXs VectorXs
Euclidean quadratic cost.
VectorXs interp_x
Affine term in .
QuadraticCostDataTpl< Scalar > Data
QuadraticCostTpl(const ConstMatrixRef &w_x, const ConstMatrixRef &w_u)
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
VectorXs interp_u
Affine term in .
void computeGradients(const ConstVectorRef &, const ConstVectorRef &, CostData &data) const
Compute the cost gradients .
QuadraticCostTpl(const ConstMatrixRef &w_x, const ConstMatrixRef &w_u, const ConstMatrixRef &w_cross)
shared_ptr< CostData > createData() const
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, CostData &data) const
Evaluate the cost function.
MatrixXs Wxu_
Weight N for term .
bool has_cross_term_
Whether a cross term exists.
ConstMatrixRef getCrossWeights() const
bool hasCrossTerm() const
Whether a cross term exists.
QuadraticCostTpl(const ConstMatrixRef &w_x, const ConstMatrixRef &w_u, const ConstVectorRef &interp_x, const ConstVectorRef &interp_u)
static auto get_vector_space(Eigen::Index nx)
proxsuite::nlp::VectorSpaceTpl< Scalar, Eigen::Dynamic > VectorSpace
void computeHessians(const ConstVectorRef &, const ConstVectorRef &, CostData &) const
Compute the cost Hessians .
void setCrossWeight(const ConstMatrixRef &w)
QuadraticCostTpl(const ConstMatrixRef &w_x, const ConstMatrixRef &w_u, const ConstMatrixRef &w_cross, const ConstVectorRef &interp_x, const ConstVectorRef &interp_u)