wrench-cone.hpp

// BSD 3-Clause License
//
// Copyright (C) 2020-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_WRENCH_CONE_HPP_
#define CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
 
#include "crocoddyl/core/utils/deprecate.hpp"
#include "crocoddyl/multibody/fwd.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
class WrenchConeTpl {
 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::Matrix3s Matrix3s;
  typedef typename MathBase::Matrix6s Matrix6s;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
  typedef typename MathBase::MatrixX3s MatrixX3s;
  typedef typename MathBase::MatrixX6s MatrixX6s;
  typedef typename MathBase::Quaternions Quaternions;
 
  WrenchConeTpl(
      const Matrix3s& R, const Scalar mu, const Vector2s& box,
      const std::size_t nf = 4, const bool inner_appr = true,
      const Scalar min_nforce = Scalar(0.),
      const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());
  DEPRECATED("Use constructor that includes inner_appr",
             WrenchConeTpl(const Matrix3s& R, const Scalar mu,
                           const Vector2s& box, std::size_t nf,
                           const Scalar min_nforce,
                           const Scalar max_nforce =
                               std::numeric_limits<Scalar>::infinity());)
 
  WrenchConeTpl(const WrenchConeTpl<Scalar>& cone);
 
  explicit WrenchConeTpl();
  ~WrenchConeTpl();
 
  void update();
  DEPRECATED("Use update().",
             void update(const Matrix3s& R, const Scalar mu,
                         const Vector2s& box,
                         const Scalar min_nforce = Scalar(0.),
                         const Scalar max_nforce =
                             std::numeric_limits<Scalar>::infinity()));
 
  template <typename NewScalar>
  WrenchConeTpl<NewScalar> cast() const;
 
  const MatrixX6s& get_A() const;
 
  const VectorXs& get_lb() const;
 
  const VectorXs& get_ub() const;
 
  std::size_t get_nf() const;
 
  const Matrix3s& get_R() const;
 
  const Vector2s& get_box() const;
 
  const Scalar get_mu() const;
 
  bool get_inner_appr() const;
 
  const Scalar get_min_nforce() const;
 
  const Scalar get_max_nforce() const;
 
  void set_R(const Matrix3s& R);
 
  void set_box(const Vector2s& box);
 
  void set_mu(const Scalar mu);
 
  void set_inner_appr(const bool inner_appr);
 
  void set_min_nforce(const Scalar min_nforce);
 
  void set_max_nforce(const Scalar max_nforce);
 
  WrenchConeTpl<Scalar>& operator=(const WrenchConeTpl<Scalar>& other);
 
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os,
                                  const WrenchConeTpl<Scalar>& X);
 
 private:
  std::size_t nf_;   
  MatrixX6s A_;      
  VectorXs ub_;      
  VectorXs lb_;      
  Matrix3s R_;       
  Vector2s box_;     
  Scalar mu_;        
  bool inner_appr_;  
  Scalar min_nforce_;  
  Scalar max_nforce_;  
};
 
}  // namespace crocoddyl
 
#include "crocoddyl/multibody/wrench-cone.hxx"
 
CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::WrenchConeTpl)
 
#endif  // CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_