cop-support.hpp

// BSD 3-Clause License
//
// Copyright (C) 2021-2025, 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/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_