crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
state.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft,
5 // University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_NUMDIFF_STATE_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_STATE_HPP_
12 
13 #include <boost/make_shared.hpp>
14 #include <boost/shared_ptr.hpp>
15 
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/state-base.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
22 class StateNumDiffTpl : public StateAbstractTpl<_Scalar> {
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
27  typedef MathBaseTpl<Scalar> MathBase;
28  typedef StateAbstractTpl<_Scalar> Base;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  explicit StateNumDiffTpl(boost::shared_ptr<Base> state);
33  virtual ~StateNumDiffTpl();
34 
35  virtual VectorXs zero() const;
36  virtual VectorXs rand() const;
37  virtual void diff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
38  Eigen::Ref<VectorXs> dxout) const;
39  virtual void integrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
40  Eigen::Ref<VectorXs> xout) const;
56  virtual void Jdiff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
57  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond, Jcomponent firstsecond = both) const;
73  virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
74  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
75  const Jcomponent firstsecond = both, const AssignmentOp op = setto) const;
76 
77  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
78  Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond = both) const;
79 
80  const Scalar get_disturbance() const;
81  void set_disturbance(const Scalar disturbance);
82 
83  private:
88  boost::shared_ptr<Base> state_;
92  Scalar disturbance_;
93 
94  protected:
95  using Base::has_limits_;
96  using Base::lb_;
97  using Base::ndx_;
98  using Base::nq_;
99  using Base::nv_;
100  using Base::nx_;
101  using Base::ub_;
102 };
103 
104 } // namespace crocoddyl
105 
106 /* --- Details -------------------------------------------------------------- */
107 /* --- Details -------------------------------------------------------------- */
108 /* --- Details -------------------------------------------------------------- */
109 #include "crocoddyl/core/numdiff/state.hxx"
110 
111 #endif // CROCODDYL_CORE_NUMDIFF_STATE_HPP_
virtual void diff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< VectorXs > dxout) const
Compute the state manifold differentiation.
VectorXs lb_
Lower state limits.
Definition: state-base.hpp:286
VectorXs ub_
Upper state limits.
Definition: state-base.hpp:287
bool has_limits_
Indicates whether any of the state limits is finite.
Definition: state-base.hpp:288
virtual void Jintegrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, const Jcomponent firstsecond=both, const AssignmentOp op=setto) const
This computes the Jacobian of the integrate method by finite differentiation: and ...
virtual void Jdiff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, Jcomponent firstsecond=both) const
This computes the Jacobian of the diff method by finite differentiation: and .
virtual VectorXs zero() const
Generate a zero state.
virtual void integrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< VectorXs > xout) const
Compute the state manifold integration.
std::size_t nx_
State dimension.
Definition: state-base.hpp:282
std::size_t nv_
Velocity dimension.
Definition: state-base.hpp:285
virtual VectorXs rand() const
Generate a random state.
virtual void JintegrateTransport(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jin, const Jcomponent firstsecond=both) const
Parallel transport from integrate(x, dx) to x.
std::size_t ndx_
State rate dimension.
Definition: state-base.hpp:283
std::size_t nq_
Configuration dimension.
Definition: state-base.hpp:284