guidance-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2026, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.

#ifndef CROCODDYL_CORE_GUIDANCE_BASE_HPP_
#define CROCODDYL_CORE_GUIDANCE_BASE_HPP_

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/mathbase.hpp"

namespace crocoddyl {

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

 CROCODDYL_BASE_CAST(GuidanceModelBase, GuidanceModelAbstractTpl)
};

template <typename _Scalar>
class GuidanceModelAbstractTpl : public GuidanceModelBase {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef GuidanceDataAbstractTpl<Scalar> GuidanceDataAbstract;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 explicit GuidanceModelAbstractTpl(const std::size_t nr);
 virtual ~GuidanceModelAbstractTpl() = default;

 virtual std::shared_ptr<GuidanceDataAbstract> createData() const;

 virtual void calc(const std::shared_ptr<GuidanceDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& error) const = 0;

 virtual void calcDiff(const std::shared_ptr<GuidanceDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& error) const = 0;

 std::size_t get_nr() const;

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

 protected:
 void checkErrorDimension(const Eigen::Ref<const VectorXs>& error) const;
 void checkRateDimension(const Eigen::Ref<const VectorXs>& g) const;
 void checkJacobianDimension(const Eigen::Ref<const MatrixXs>& Ge) const;

 std::size_t nr_;
};

template <typename Scalar>
std::ostream& operator<<(std::ostream& os,
 const GuidanceModelAbstractTpl<Scalar>& model);

template <typename _Scalar>
struct GuidanceDataAbstractTpl {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef GuidanceModelAbstractTpl<Scalar> GuidanceModelAbstract;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 explicit GuidanceDataAbstractTpl(const GuidanceModelAbstract* const model)
 : g(model->get_nr()), Ge(model->get_nr(), model->get_nr()) {
 g.setZero();
 Ge.setZero();
 }
 virtual ~GuidanceDataAbstractTpl() = default;

 VectorXs g;
 MatrixXs Ge;
};

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/guidance-base.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::GuidanceModelAbstractTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::GuidanceDataAbstractTpl)

#endif // CROCODDYL_CORE_GUIDANCE_BASE_HPP_