Crocoddyl
 
Loading...
Searching...
No Matches
cost.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_NUMDIFF_COST_HPP_
11#define CROCODDYL_CORE_NUMDIFF_COST_HPP_
12
13#include <boost/function.hpp>
14
15#include "crocoddyl/core/cost-base.hpp"
16#include "crocoddyl/multibody/fwd.hpp"
17
18namespace crocoddyl {
19
31template <typename _Scalar>
33 public:
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 CROCODDYL_DERIVED_CAST(CostModelBase, CostModelNumDiffTpl)
36
37 typedef _Scalar Scalar;
43 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
44 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
45 typedef boost::function<void(const VectorXs&, const VectorXs&)>
46 ReevaluationFunction;
47
54 explicit CostModelNumDiffTpl(const std::shared_ptr<Base>& model);
55
59 virtual ~CostModelNumDiffTpl() = default;
60
64 virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>& x,
66 const Eigen::Ref<const VectorXs>& u) override;
67
72 virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>& x) override;
74
78 virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
79 const Eigen::Ref<const VectorXs>& x,
80 const Eigen::Ref<const VectorXs>& u) override;
81
86 virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x) override;
88
95 virtual std::shared_ptr<CostDataAbstract> createData(
96 DataCollectorAbstract* const data) override;
97
107 template <typename NewScalar>
109
113 const std::shared_ptr<Base>& get_model() const;
114
119 const Scalar get_disturbance() const;
120
125 void set_disturbance(const Scalar disturbance);
126
134
142 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
143
144 protected:
145 using Base::activation_;
146 using Base::nu_;
147 using Base::state_;
148 using Base::unone_;
149
150 private:
162 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
163
164 std::shared_ptr<Base> model_;
166 Scalar e_jac_;
168 std::vector<ReevaluationFunction>
169 reevals_;
170};
171
172template <typename _Scalar>
173struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> {
174 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
175
176 typedef _Scalar Scalar;
181 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
182
189 template <template <typename Scalar> class Model>
190 explicit CostDataNumDiffTpl(Model<Scalar>* const model,
191 DataCollectorAbstract* const shared_data)
192 : Base(model, shared_data),
193 dx(model->get_state()->get_ndx()),
194 xp(model->get_state()->get_nx()),
195 du(model->get_nu()),
196 up(model->get_nu()) {
197 dx.setZero();
198 xp.setZero();
199 du.setZero();
200 up.setZero();
201
202 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
203 const std::size_t nu = model->get_model()->get_nu();
204 data_0 = model->get_model()->createData(shared_data);
205 for (std::size_t i = 0; i < ndx; ++i) {
206 data_x.push_back(model->get_model()->createData(shared_data));
207 }
208 for (std::size_t i = 0; i < nu; ++i) {
209 data_u.push_back(model->get_model()->createData(shared_data));
210 }
211 }
212
213 virtual ~CostDataNumDiffTpl() = default;
214
215 using Base::activation;
216 using Base::cost;
217 using Base::Lu;
218 using Base::Luu;
219 using Base::Lx;
220 using Base::Lxu;
221 using Base::Lxx;
222 using Base::residual;
223 using Base::shared;
224
225 Scalar x_norm;
226 Scalar
228 Scalar
230 VectorXs dx;
231 VectorXs xp;
233 VectorXs du;
234 VectorXs up;
236 std::shared_ptr<Base> data_0;
237 std::vector<std::shared_ptr<Base> >
239 std::vector<std::shared_ptr<Base> >
241};
242
243} // namespace crocoddyl
244
245/* --- Details -------------------------------------------------------------- */
246/* --- Details -------------------------------------------------------------- */
247/* --- Details -------------------------------------------------------------- */
248#include "crocoddyl/core/numdiff/cost.hxx"
249
250CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CostModelNumDiffTpl)
251CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostDataNumDiffTpl)
252
253#endif // CROCODDYL_CORE_NUMDIFF_COST_HPP_
Abstract class for cost models.
Definition cost-base.hpp:64
std::shared_ptr< ActivationModelAbstract > activation_
Activation model.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
This class computes the numerical differentiation of a cost model.
Definition cost.hpp:32
virtual std::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data) override
Create a numdiff cost data.
CostModelNumDiffTpl< NewScalar > cast() const
Cast the cost numdiff model to a different scalar type.
const std::shared_ptr< Base > & get_model() const
Return the original cost model.
const Scalar get_disturbance() const
Return the disturbance constant used by the numerical differentiation routine.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that updates the shared data computed for a system rollout The updated data is use...
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used by the numerical differentiation routine.
virtual void calc(const std::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
virtual void calcDiff(const std::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
virtual void calcDiff(const std::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobian and Hessian of cost and its residual vector.
virtual ~CostModelNumDiffTpl()=default
Initialize the numdiff cost model.
virtual void calc(const std::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the cost value and its residual vector.
CostModelNumDiffTpl(const std::shared_ptr< Base > &model)
Initialize the numdiff cost model.
Scalar x_norm
Norm of the state vector.
Definition cost.hpp:225
std::vector< std::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition cost.hpp:238
Scalar uh_jac
Disturbance value used for computing .
Definition cost.hpp:229
std::vector< std::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition cost.hpp:240
Scalar xh_jac
Disturbance value used for computing .
Definition cost.hpp:227
VectorXs du
Control disturbance.
Definition cost.hpp:233
std::shared_ptr< Base > data_0
The data at the approximation point.
Definition cost.hpp:236
VectorXs dx
State disturbance.
Definition cost.hpp:230
CostDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff cost data.
Definition cost.hpp:190