crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
cost-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_COST_BASE_HPP_
10#define CROCODDYL_CORE_COST_BASE_HPP_
11
12#include <boost/shared_ptr.hpp>
13#include <boost/make_shared.hpp>
14
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/state-base.hpp"
17#include "crocoddyl/core/data-collector-base.hpp"
18#include "crocoddyl/core/activation-base.hpp"
19#include "crocoddyl/core/residual-base.hpp"
20#include "crocoddyl/core/activations/quadratic.hpp"
21#include "crocoddyl/core/utils/deprecate.hpp"
22
23namespace crocoddyl {
24
48template <typename _Scalar>
50 public:
51 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52
53 typedef _Scalar Scalar;
61 typedef typename MathBase::VectorXs VectorXs;
62 typedef typename MathBase::MatrixXs MatrixXs;
63
71 CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
72 boost::shared_ptr<ResidualModelAbstract> residual);
73
81 CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
82 const std::size_t nu);
83
92 CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation);
93
102 CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ResidualModelAbstract> residual);
103
113 CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu);
114
125 CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr);
126 virtual ~CostModelAbstractTpl();
127
135 virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
136 const Eigen::Ref<const VectorXs>& u) = 0;
137
147 virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
148
158 virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
159 const Eigen::Ref<const VectorXs>& u) = 0;
160
170 virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
171
182 virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
183
187 const boost::shared_ptr<StateAbstract>& get_state() const;
188
192 const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;
193
197 const boost::shared_ptr<ResidualModelAbstract>& get_residual() const;
198
202 std::size_t get_nu() const;
203
207 template <class Scalar>
208 friend std::ostream& operator<<(std::ostream& os, const CostModelAbstractTpl<Scalar>& model);
209
213 template <class ReferenceType>
214 void set_reference(ReferenceType ref);
215
219 template <class ReferenceType>
220 ReferenceType get_reference();
221
227 virtual void print(std::ostream& os) const;
228
229 protected:
233 virtual void set_referenceImpl(const std::type_info&, const void*);
234
238 virtual void get_referenceImpl(const std::type_info&, void*);
239
240 boost::shared_ptr<StateAbstract> state_;
241 boost::shared_ptr<ActivationModelAbstract> activation_;
242 boost::shared_ptr<ResidualModelAbstract> residual_;
243 std::size_t nu_;
244 VectorXs unone_;
245};
246
247template <typename _Scalar>
249 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
250
251 typedef _Scalar Scalar;
256 typedef typename MathBase::VectorXs VectorXs;
257 typedef typename MathBase::MatrixXs MatrixXs;
258
259 template <template <typename Scalar> class Model>
260 CostDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
261 : shared(data),
262 activation(model->get_activation()->createData()),
263 residual(model->get_residual()->createData(data)),
264 cost(Scalar(0.)),
265 Lx(model->get_state()->get_ndx()),
266 Lu(model->get_nu()),
267 Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
268 Lxu(model->get_state()->get_ndx(), model->get_nu()),
269 Luu(model->get_nu(), model->get_nu()) {
270 Lx.setZero();
271 Lu.setZero();
272 Lxx.setZero();
273 Lxu.setZero();
274 Luu.setZero();
275 }
276 virtual ~CostDataAbstractTpl() {}
277
278 DEPRECATED("Use residual.r", const VectorXs& get_r() const { return residual->r; };)
279 DEPRECATED("Use residual.Rx", const MatrixXs& get_Rx() const { return residual->Rx; };)
280 DEPRECATED("Use residual.Ru", const MatrixXs& get_Ru() const { return residual->Ru; };)
281 DEPRECATED("Use residual.r", void set_r(const VectorXs& r) { residual->r = r; };)
282 DEPRECATED("Use residual.Rx", void set_Rx(const MatrixXs& Rx) { residual->Rx = Rx; };)
283 DEPRECATED("Use residual.Ru", void set_Ru(const MatrixXs& Ru) { residual->Ru = Ru; };)
284
285 DataCollectorAbstract* shared;
286 boost::shared_ptr<ActivationDataAbstract> activation;
287 boost::shared_ptr<ResidualDataAbstract> residual;
288 Scalar cost;
289 VectorXs Lx;
290 VectorXs Lu;
291 MatrixXs Lxx;
292 MatrixXs Lxu;
293 MatrixXs Luu;
294};
295
296} // namespace crocoddyl
297
298/* --- Details -------------------------------------------------------------- */
299/* --- Details -------------------------------------------------------------- */
300/* --- Details -------------------------------------------------------------- */
301#include "crocoddyl/core/cost-base.hxx"
302
303#endif // CROCODDYL_CORE_COST_BASE_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:49
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nr, const std::size_t nu)
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, boost::shared_ptr< ResidualModelAbstract > residual)
Initialize the cost model.
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation)
virtual void set_referenceImpl(const std::type_info &, const void *)
Modify the cost reference.
void set_reference(ReferenceType ref)
Modify the cost reference.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian and Hessian of the cost functions with respect to the state only.
virtual void print(std::ostream &os) const
Print relevant information of the cost model.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:241
const boost::shared_ptr< ResidualModelAbstract > & get_residual() const
Return the residual model.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
friend std::ostream & operator<<(std::ostream &os, const CostModelAbstractTpl< Scalar > &model)
Print information on the cost model.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the cost value and its residual vector.
virtual void get_referenceImpl(const std::type_info &, void *)
Return the cost reference.
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:240
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:243
VectorXs unone_
No control vector.
Definition: cost-base.hpp:244
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, const std::size_t nu)
Initialize the cost model.
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nr)
const boost::shared_ptr< ActivationModelAbstract > & get_activation() const
Return the activation model.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the Jacobian and Hessian of cost and its residual vector.
ReferenceType get_reference()
Return the cost reference.
boost::shared_ptr< ResidualModelAbstract > residual_
Residual model.
Definition: cost-base.hpp:242
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
std::size_t get_nu() const
Return the dimension of the control input.
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ResidualModelAbstract > residual)
Abstract class for residual models.
Abstract class for the state representation.
Definition: state-base.hpp:42