state-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh, INRIA,
// Heriot-Watt University
// 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 "crocoddyl/core/fwd.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);
}

class StateBase {
 public:
 virtual ~StateBase() = default;

 CROCODDYL_BASE_CAST(StateBase, StateAbstractTpl)
};

template <typename _Scalar>
class StateAbstractTpl : public StateBase {
 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 safe_diff(const Eigen::Ref<const VectorXs>& x0,
 const Eigen::Ref<const VectorXs>& x1,
 Eigen::Ref<VectorXs> dxout) const;

 virtual void safe_integrate(const Eigen::Ref<const VectorXs>& x,
 const Eigen::Ref<const VectorXs>& dx,
 Eigen::Ref<VectorXs> xout) const;

 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);

 template <class Scalar>
 friend std::ostream& operator<<(std::ostream& os,
 const ActionModelAbstractTpl<Scalar>& model);

 virtual void print(std::ostream& os) const;

 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"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::StateAbstractTpl)

#endif // CROCODDYL_CORE_STATE_BASE_HPP_