crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
cost.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, New York University,
5// Max Planck Gesellschaft
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#include "crocoddyl/multibody/fwd.hpp"
15#include "crocoddyl/core/cost-base.hpp"
16
17namespace crocoddyl {
18
28template <typename _Scalar>
30 public:
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32
33 typedef _Scalar Scalar;
39 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
40 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
41 typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
42
48 explicit CostModelNumDiffTpl(const boost::shared_ptr<Base>& model);
49
54
58 virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
59 const Eigen::Ref<const VectorXs>& u);
60
64 virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
65
69 virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
70 const Eigen::Ref<const VectorXs>& u);
71
76 virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
77
84 virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
85
89 const boost::shared_ptr<Base>& get_model() const;
90
94 const Scalar get_disturbance() const;
95
99 void set_disturbance(const Scalar disturbance);
100
108
115 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
116
117 protected:
118 using Base::activation_;
119 using Base::nu_;
120 using Base::state_;
121 using Base::unone_;
122
123 private:
135 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
136
137 boost::shared_ptr<Base> model_;
138 Scalar disturbance_;
139 std::vector<ReevaluationFunction> reevals_;
140};
141
142template <typename _Scalar>
143struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> {
144 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145
146 typedef _Scalar Scalar;
151 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
152
159 template <template <typename Scalar> class Model>
160 explicit CostDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
161 : Base(model, shared_data),
162 dx(model->get_state()->get_ndx()),
163 xp(model->get_state()->get_nx()),
164 du(model->get_nu()),
165 up(model->get_nu()) {
166 dx.setZero();
167 xp.setZero();
168 du.setZero();
169 up.setZero();
170
171 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
172 const std::size_t nu = model->get_model()->get_nu();
173 data_0 = model->get_model()->createData(shared_data);
174 for (std::size_t i = 0; i < ndx; ++i) {
175 data_x.push_back(model->get_model()->createData(shared_data));
176 }
177 for (std::size_t i = 0; i < nu; ++i) {
178 data_u.push_back(model->get_model()->createData(shared_data));
179 }
180 }
181
182 virtual ~CostDataNumDiffTpl() {}
183
184 using Base::activation;
185 using Base::cost;
186 using Base::Lu;
187 using Base::Luu;
188 using Base::Lx;
189 using Base::Lxu;
190 using Base::Lxx;
191 using Base::residual;
192 using Base::shared;
193
194 VectorXs dx;
195 VectorXs xp;
196 VectorXs du;
197 VectorXs up;
198 boost::shared_ptr<Base> data_0;
199 std::vector<boost::shared_ptr<Base> > data_x;
200 std::vector<boost::shared_ptr<Base> > data_u;
201};
202
203} // namespace crocoddyl
204
205/* --- Details -------------------------------------------------------------- */
206/* --- Details -------------------------------------------------------------- */
207/* --- Details -------------------------------------------------------------- */
208#include "crocoddyl/core/numdiff/cost.hxx"
209
210#endif // CROCODDYL_CORE_NUMDIFF_COST_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:49
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:241
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:240
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:243
VectorXs unone_
No control vector.
Definition: cost-base.hpp:244
This class computes the numerical differentiation of a cost model.
Definition: cost.hpp:29
const boost::shared_ptr< Base > & get_model() const
Return the original cost model.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff 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...
virtual ~CostModelNumDiffTpl()
Initialize the numdiff cost model.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the cost value and its residual vector.
CostModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff cost model.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian and Hessian of cost and its residual vector.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create a numdiff cost data.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: cost.hpp:200
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: cost.hpp:198
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: cost.hpp:195
VectorXs du
Control disturbance.
Definition: cost.hpp:196
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: cost.hpp:199
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
Definition: cost.hpp:197
VectorXs dx
State disturbance.
Definition: cost.hpp:194
CostDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff cost data.
Definition: cost.hpp:160