cop-support.hpp

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

#ifndef CROCODDYL_MULTIBODY_COP_SUPPORT_HPP_
#define CROCODDYL_MULTIBODY_COP_SUPPORT_HPP_

#include "crocoddyl/core/utils/conversions.hpp"
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/multibody/wrench-cone.hpp"

namespace crocoddyl {

template <typename _Scalar>
class CoPSupportTpl {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::Vector2s Vector2s;
 typedef typename MathBase::Vector3s Vector3s;
 typedef typename MathBase::Vector4s Vector4s;
 typedef typename MathBase::Matrix3s Matrix3s;
 typedef typename MathBase::Matrix46s Matrix46s;
 typedef typename MathBase::Quaternions Quaternions;

 CoPSupportTpl(const Matrix3s& R, const Vector2s& box);

 CoPSupportTpl(const WrenchConeTpl<Scalar>& support);

 CoPSupportTpl(const CoPSupportTpl<Scalar>& support);

 explicit CoPSupportTpl();
 ~CoPSupportTpl();

 void update();

 template <typename NewScalar>
 CoPSupportTpl<NewScalar> cast() const;

 const Matrix46s& get_A() const;

 const Vector4s& get_ub() const;

 const Vector4s& get_lb() const;

 const Vector2s& get_box() const;

 const Matrix3s& get_R() const;

 void set_R(const Matrix3s& R);

 void set_box(const Vector2s& box);

 CoPSupportTpl<Scalar>& operator=(const CoPSupportTpl<Scalar>& other);

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

 private:
 Matrix46s A_;
 Vector4s ub_;
 Vector4s lb_;
 Matrix3s R_;
 Vector2s box_;
};

} // namespace crocoddyl

#include "crocoddyl/multibody/cop-support.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CoPSupportTpl)

#endif // CROCODDYL_MULTIBODY_COP_SUPPORT_HPP_