crocoddyl  1.8.1
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 calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
99  const Eigen::Ref<const VectorXs>& u);
100 
111  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
112 
119  void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
120 
127  void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
128 
132  const boost::shared_ptr<StateAbstract>& get_state() const;
133 
137  std::size_t get_nr() const;
138 
142  std::size_t get_nu() const;
143 
147  bool get_q_dependent() const;
148 
152  bool get_v_dependent() const;
153 
157  bool get_u_dependent() const;
158 
162  template <class Scalar>
163  friend std::ostream& operator<<(std::ostream& os, const ResidualModelAbstractTpl<Scalar>& model);
164 
170  virtual void print(std::ostream& os) const;
171 
172  protected:
173  boost::shared_ptr<StateAbstract> state_;
174  std::size_t nr_;
175  std::size_t nu_;
176  VectorXs unone_;
180 };
181 
182 template <typename _Scalar>
184  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
185 
186  typedef _Scalar Scalar;
189  typedef typename MathBase::VectorXs VectorXs;
190  typedef typename MathBase::MatrixXs MatrixXs;
191 
192  template <template <typename Scalar> class Model>
193  ResidualDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
194  : shared(data),
195  r(model->get_nr()),
196  Rx(model->get_nr(), model->get_state()->get_ndx()),
197  Ru(model->get_nr(), model->get_nu()) {
198  r.setZero();
199  Rx.setZero();
200  Ru.setZero();
201  }
202  virtual ~ResidualDataAbstractTpl() {}
203 
204  DataCollectorAbstract* shared;
205  VectorXs r;
206  MatrixXs Rx;
207  MatrixXs Ru;
208 };
209 
210 } // namespace crocoddyl
211 
212 /* --- Details -------------------------------------------------------------- */
213 /* --- Details -------------------------------------------------------------- */
214 /* --- Details -------------------------------------------------------------- */
215 #include "crocoddyl/core/residual-base.hxx"
216 
217 #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.