crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
state.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_STATE_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_STATE_HPP_
11 
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/core/state-base.hpp"
15 #include "crocoddyl/core/residual-base.hpp"
16 #include "crocoddyl/multibody/states/multibody.hpp"
17 
18 namespace crocoddyl {
19 
34 template <typename _Scalar>
35 class ResidualModelStateTpl : public ResidualModelAbstractTpl<_Scalar> {
36  public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
39  typedef _Scalar Scalar;
40  typedef MathBaseTpl<Scalar> MathBase;
41  typedef ResidualModelAbstractTpl<Scalar> Base;
42  typedef StateMultibodyTpl<Scalar> StateMultibody;
43  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
44  typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
45  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
46  typedef typename MathBase::VectorXs VectorXs;
47  typedef typename MathBase::MatrixXs MatrixXs;
48 
56  ResidualModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref,
57  const std::size_t nu);
58 
67  ResidualModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref);
68 
77  ResidualModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu);
78 
88  ResidualModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state);
89  virtual ~ResidualModelStateTpl();
90 
98  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
99  const Eigen::Ref<const VectorXs>& u);
100 
108  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
109  const Eigen::Ref<const VectorXs>& u);
110 
114  const VectorXs& get_reference() const;
115 
119  void set_reference(const VectorXs& reference);
120 
126  virtual void print(std::ostream& os) const;
127 
128  protected:
129  using Base::nu_;
130  using Base::state_;
131  using Base::u_dependent_;
132  using Base::unone_;
133 
134  private:
135  VectorXs xref_;
136 };
137 
138 } // namespace crocoddyl
139 
140 /* --- Details -------------------------------------------------------------- */
141 /* --- Details -------------------------------------------------------------- */
142 /* --- Details -------------------------------------------------------------- */
143 #include "crocoddyl/multibody/residuals/state.hxx"
144 
145 #endif // CROCODDYL_MULTIBODY_RESIDUALS_STATE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the state residual.
bool u_dependent_
Label that indicates if the residual function depends on u.
ResidualModelStateTpl(boost::shared_ptr< typename Base::StateAbstract > state, const VectorXs &xref, const std::size_t nu)
Initialize the state residual model.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the state residual.
std::size_t nu_
Control dimension.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the state residual.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
const VectorXs & get_reference() const
Return the reference state.
void set_reference(const VectorXs &reference)
Modify the reference state.