wrench-cone.hpp

// BSD 3-Clause License
//
// Copyright (C) 2020-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_WRENCH_CONE_HPP_
#define CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_

#include "crocoddyl/core/utils/conversions.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;

 template <typename>
 friend class WrenchConeTpl;

 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_