Crocoddyl
 
Loading...
Searching...
No Matches
wrench-cone.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-2025, University of Edinburgh, University of Oxford,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
11#define CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
12
13#include "crocoddyl/core/utils/deprecate.hpp"
14#include "crocoddyl/multibody/fwd.hpp"
15
16namespace crocoddyl {
17
31template <typename _Scalar>
33 public:
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35
36 typedef _Scalar Scalar;
38 typedef typename MathBase::Vector2s Vector2s;
39 typedef typename MathBase::Vector3s Vector3s;
40 typedef typename MathBase::Matrix3s Matrix3s;
41 typedef typename MathBase::Matrix6s Matrix6s;
42 typedef typename MathBase::VectorXs VectorXs;
43 typedef typename MathBase::MatrixXs MatrixXs;
44 typedef typename MathBase::MatrixX3s MatrixX3s;
45 typedef typename MathBase::MatrixX6s MatrixX6s;
46 typedef typename MathBase::Quaternions Quaternions;
47
62 const Matrix3s& R, const Scalar mu, const Vector2s& box,
63 const std::size_t nf = 4, const bool inner_appr = true,
64 const Scalar min_nforce = Scalar(0.),
65 const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());
66 DEPRECATED("Use constructor that includes inner_appr",
67 WrenchConeTpl(const Matrix3s& R, const Scalar mu,
68 const Vector2s& box, std::size_t nf,
69 const Scalar min_nforce,
70 const Scalar max_nforce =
71 std::numeric_limits<Scalar>::infinity());)
72
79
83 explicit WrenchConeTpl();
85
95 void update();
96 DEPRECATED("Use update().",
97 void update(const Matrix3s& R, const Scalar mu,
98 const Vector2s& box,
99 const Scalar min_nforce = Scalar(0.),
100 const Scalar max_nforce =
101 std::numeric_limits<Scalar>::infinity()));
102
111 template <typename NewScalar>
113
117 const MatrixX6s& get_A() const;
118
122 const VectorXs& get_lb() const;
123
127 const VectorXs& get_ub() const;
128
132 std::size_t get_nf() const;
133
138 const Matrix3s& get_R() const;
139
143 const Vector2s& get_box() const;
144
148 const Scalar get_mu() const;
149
154 bool get_inner_appr() const;
155
159 const Scalar get_min_nforce() const;
160
164 const Scalar get_max_nforce() const;
165
173 void set_R(const Matrix3s& R);
174
181 void set_box(const Vector2s& box);
182
189 void set_mu(const Scalar mu);
190
198 void set_inner_appr(const bool inner_appr);
199
206 void set_min_nforce(const Scalar min_nforce);
207
214 void set_max_nforce(const Scalar max_nforce);
215
216 WrenchConeTpl<Scalar>& operator=(const WrenchConeTpl<Scalar>& other);
217
218 template <class Scalar>
219 friend std::ostream& operator<<(std::ostream& os,
220 const WrenchConeTpl<Scalar>& X);
221
222 private:
223 std::size_t nf_;
224 MatrixX6s A_;
225 VectorXs ub_;
226 VectorXs lb_;
227 Matrix3s R_;
228 Vector2s box_;
229 Scalar mu_;
230 bool inner_appr_;
232 Scalar min_nforce_;
233 Scalar max_nforce_;
234};
235
236} // namespace crocoddyl
237
238#include "crocoddyl/multibody/wrench-cone.hxx"
239
240CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::WrenchConeTpl)
241
242#endif // CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
This class encapsulates a wrench cone.
void set_max_nforce(const Scalar max_nforce)
Modify the maximum normal force.
const VectorXs & get_ub() const
Return the upper bound of inequalities.
bool get_inner_appr() const
Return the label that describes the type of friction cone approximation (inner/outer)
const Vector2s & get_box() const
Return dimension of the foot surface dim = (length, width)
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())
Initialize the wrench cone.
const Matrix3s & get_R() const
Return the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
void set_box(const Vector2s &box)
Modify dimension of the foot surface dim = (length, width)
const Scalar get_max_nforce() const
Return the maximum normal force.
WrenchConeTpl()
Initialize the wrench cone.
void set_inner_appr(const bool inner_appr)
Modify the label that describes the type of friction cone approximation (inner/outer)
const Scalar get_mu() const
Return friction coefficient.
void set_min_nforce(const Scalar min_nforce)
Modify the minium normal force.
std::size_t get_nf() const
Return the number of facets.
void set_R(const Matrix3s &R)
Modify the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
const VectorXs & get_lb() const
Return the lower bound of inequalities.
WrenchConeTpl< NewScalar > cast() const
Cast the wrench cone to a different scalar type.
const Scalar get_min_nforce() const
Return the minimum normal force.
void set_mu(const Scalar mu)
Modify friction coefficient.
const MatrixX6s & get_A() const
Return the matrix of wrench cone.
void update()
Update the matrix of wrench cone inequalities in the world frame.
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)
Initialize the wrench cone.