crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
residual-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_RESIDUAL_BASE_HPP_
10 #define CROCODDYL_CORE_RESIDUAL_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 
19 namespace crocoddyl {
20 
37 template <typename _Scalar>
38 class ResidualModelAbstractTpl {
39  public:
40  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 
42  typedef _Scalar Scalar;
43  typedef MathBaseTpl<Scalar> MathBase;
44  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
45  typedef StateAbstractTpl<Scalar> StateAbstract;
46  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
47  typedef typename MathBase::VectorXs VectorXs;
48  typedef typename MathBase::MatrixXs MatrixXs;
49 
60  ResidualModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu,
61  const bool q_dependent = true, const bool v_dependent = true,
62  const bool u_dependent = true);
63 
75  ResidualModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const bool q_dependent = true,
76  const bool v_dependent = true, const bool u_dependent = true);
77  virtual ~ResidualModelAbstractTpl();
78 
86  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
87  const Eigen::Ref<const VectorXs>& u);
88 
98  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
99 
109  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
110  const Eigen::Ref<const VectorXs>& u);
111 
121  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
122 
133  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
134 
138  const boost::shared_ptr<StateAbstract>& get_state() const;
139 
143  std::size_t get_nr() const;
144 
148  std::size_t get_nu() const;
149 
153  bool get_q_dependent() const;
154 
158  bool get_v_dependent() const;
159 
163  bool get_u_dependent() const;
164 
168  template <class Scalar>
169  friend std::ostream& operator<<(std::ostream& os, const ResidualModelAbstractTpl<Scalar>& model);
170 
176  virtual void print(std::ostream& os) const;
177 
178  protected:
179  boost::shared_ptr<StateAbstract> state_;
180  std::size_t nr_;
181  std::size_t nu_;
182  VectorXs unone_;
186 };
187 
188 template <typename _Scalar>
190  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
191 
192  typedef _Scalar Scalar;
195  typedef typename MathBase::VectorXs VectorXs;
196  typedef typename MathBase::MatrixXs MatrixXs;
197 
198  template <template <typename Scalar> class Model>
199  ResidualDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
200  : shared(data),
201  r(model->get_nr()),
202  Rx(model->get_nr(), model->get_state()->get_ndx()),
203  Ru(model->get_nr(), model->get_nu()) {
204  r.setZero();
205  Rx.setZero();
206  Ru.setZero();
207  }
208  virtual ~ResidualDataAbstractTpl() {}
209 
210  DataCollectorAbstract* shared;
211  VectorXs r;
212  MatrixXs Rx;
213  MatrixXs Ru;
214 };
215 
216 } // namespace crocoddyl
217 
218 /* --- Details -------------------------------------------------------------- */
219 /* --- Details -------------------------------------------------------------- */
220 /* --- Details -------------------------------------------------------------- */
221 #include "crocoddyl/core/residual-base.hxx"
222 
223 #endif // CROCODDYL_CORE_RESIDUAL_BASE_HPP_
bool get_v_dependent() const
Return true if the residual function depends on v.
std::size_t nr_
Residual vector dimension.
bool get_u_dependent() const
Return true if the residual function depends on u.
bool q_dependent_
Label that indicates if the residual function depends on q.
bool u_dependent_
Label that indicates if the residual function depends on u.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
std::size_t get_nr() const
Return the dimension of the residual vector.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
std::size_t nu_
Control dimension.
ResidualModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nr, const std::size_t nu, const bool q_dependent=true, const bool v_dependent=true, const bool u_dependent=true)
Initialize the residual model.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual vector.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
bool get_q_dependent() const
Return true if the residual function depends on q.
virtual void print(std::ostream &os) const
Print relevant information of the residual model.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t get_nu() const
Return the dimension of the control input.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
VectorXs r
Residual vector.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian of the residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.