state-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh, INRIA
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_STATE_BASE_HPP_
#define CROCODDYL_CORE_STATE_BASE_HPP_
 
#include <stdexcept>
#include <string>
#include <vector>
 
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/utils/exception.hpp"
 
namespace crocoddyl {
 
enum Jcomponent { both = 0, first = 1, second = 2 };
 
inline bool is_a_Jcomponent(Jcomponent firstsecond) {
  return (firstsecond == first || firstsecond == second || firstsecond == both);
}
 
template <typename _Scalar>
class StateAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  StateAbstractTpl(const std::size_t nx, const std::size_t ndx);
  StateAbstractTpl();
  virtual ~StateAbstractTpl();
 
  virtual VectorXs zero() const = 0;
 
  virtual VectorXs rand() const = 0;
 
  virtual void diff(const Eigen::Ref<const VectorXs>& x0,
                    const Eigen::Ref<const VectorXs>& x1,
                    Eigen::Ref<VectorXs> dxout) const = 0;
 
  virtual void integrate(const Eigen::Ref<const VectorXs>& x,
                         const Eigen::Ref<const VectorXs>& dx,
                         Eigen::Ref<VectorXs> xout) const = 0;
 
  virtual void Jdiff(const Eigen::Ref<const VectorXs>& x0,
                     const Eigen::Ref<const VectorXs>& x1,
                     Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
                     const Jcomponent firstsecond = both) const = 0;
 
  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 = 0;
 
  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x,
                                   const Eigen::Ref<const VectorXs>& dx,
                                   Eigen::Ref<MatrixXs> Jin,
                                   const Jcomponent firstsecond) const = 0;
 
  VectorXs diff_dx(const Eigen::Ref<const VectorXs>& x0,
                   const Eigen::Ref<const VectorXs>& x1);
 
  VectorXs integrate_x(const Eigen::Ref<const VectorXs>& x,
                       const Eigen::Ref<const VectorXs>& dx);
 
  std::vector<MatrixXs> Jdiff_Js(const Eigen::Ref<const VectorXs>& x0,
                                 const Eigen::Ref<const VectorXs>& x1,
                                 const Jcomponent firstsecond = both);
 
  std::vector<MatrixXs> Jintegrate_Js(const Eigen::Ref<const VectorXs>& x,
                                      const Eigen::Ref<const VectorXs>& dx,
                                      const Jcomponent firstsecond = both);
 
  std::size_t get_nx() const;
 
  std::size_t get_ndx() const;
 
  std::size_t get_nq() const;
 
  std::size_t get_nv() const;
 
  const VectorXs& get_lb() const;
 
  const VectorXs& get_ub() const;
 
  bool get_has_limits() const;
 
  void set_lb(const VectorXs& lb);
 
  void set_ub(const VectorXs& ub);
 
 protected:
  void update_has_limits();
 
  std::size_t nx_;   
  std::size_t ndx_;  
  std::size_t nq_;   
  std::size_t nv_;   
  VectorXs lb_;      
  VectorXs ub_;      
  bool has_limits_;  
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/state-base.hxx"
 
#endif  // CROCODDYL_CORE_STATE_BASE_HPP_