crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, 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/activations/quadratic.hpp"
20 
21 namespace crocoddyl {
22 
46 template <typename _Scalar>
48  public:
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
51  typedef _Scalar Scalar;
58  typedef typename MathBase::VectorXs VectorXs;
59  typedef typename MathBase::MatrixXs MatrixXs;
60 
68  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
69  const std::size_t nu);
70 
79  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation);
80 
90  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu);
91 
102  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr);
103  virtual ~CostModelAbstractTpl();
104 
112  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
113  const Eigen::Ref<const VectorXs>& u) = 0;
114 
124  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
125  const Eigen::Ref<const VectorXs>& u) = 0;
126 
137  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
138 
145  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
146 
153  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
154 
158  const boost::shared_ptr<StateAbstract>& get_state() const;
159 
163  const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;
164 
168  std::size_t get_nu() const;
169 
173  template <class ReferenceType>
174  void set_reference(ReferenceType ref);
175 
179  template <class ReferenceType>
180  ReferenceType get_reference() const;
181 
182  protected:
186  virtual void set_referenceImpl(const std::type_info&, const void*);
187 
191  virtual void get_referenceImpl(const std::type_info&, void*) const;
192 
193  boost::shared_ptr<StateAbstract> state_;
194  boost::shared_ptr<ActivationModelAbstract> activation_;
195  std::size_t nu_;
196  VectorXs unone_;
197 };
198 
199 template <typename _Scalar>
201  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
202 
203  typedef _Scalar Scalar;
207  typedef typename MathBase::VectorXs VectorXs;
208  typedef typename MathBase::MatrixXs MatrixXs;
209 
210  template <template <typename Scalar> class Model>
211  CostDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
212  : shared(data),
213  activation(model->get_activation()->createData()),
214  cost(Scalar(0.)),
215  Lx(model->get_state()->get_ndx()),
216  Lu(model->get_nu()),
217  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
218  Lxu(model->get_state()->get_ndx(), model->get_nu()),
219  Luu(model->get_nu(), model->get_nu()),
220  r(model->get_activation()->get_nr()),
221  Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()),
222  Ru(model->get_activation()->get_nr(), model->get_nu()) {
223  Lx.setZero();
224  Lu.setZero();
225  Lxx.setZero();
226  Lxu.setZero();
227  Luu.setZero();
228  r.setZero();
229  Rx.setZero();
230  Ru.setZero();
231  }
232  virtual ~CostDataAbstractTpl() {}
233 
234  DataCollectorAbstract* shared;
235  boost::shared_ptr<ActivationDataAbstract> activation;
236  Scalar cost;
237  VectorXs Lx;
238  VectorXs Lu;
239  MatrixXs Lxx;
240  MatrixXs Lxu;
241  MatrixXs Luu;
242  VectorXs r;
243  MatrixXs Rx;
244  MatrixXs Ru;
245 };
246 
247 } // namespace crocoddyl
248 
249 /* --- Details -------------------------------------------------------------- */
250 /* --- Details -------------------------------------------------------------- */
251 /* --- Details -------------------------------------------------------------- */
252 #include "crocoddyl/core/cost-base.hxx"
253 
254 #endif // CROCODDYL_CORE_COST_BASE_HPP_
CostModelAbstractTpl(boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, const std::size_t nu)
Initialize the cost 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.
void set_reference(ReferenceType ref)
Modify the cost reference.
Abstract class for cost models.
Definition: cost-base.hpp:47
const boost::shared_ptr< ActivationModelAbstract > & get_activation() const
Return the activation model.
Abstract class for the state representation.
Definition: fwd.hpp:101
ReferenceType get_reference() const
Return the cost reference.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual void set_referenceImpl(const std::type_info &, const void *)
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
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 boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
virtual void get_referenceImpl(const std::type_info &, void *) const
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
std::size_t get_nu() const
Return the dimension of the control input.