14 template <
typename Scalar>
15 FrictionConeTpl<Scalar>::FrictionConeTpl() : nf_(4) {
16 A_.resize(nf_ + 1, 3);
20 update(Vector3s(0, 0, 1), 0.7,
true, 0., std::numeric_limits<Scalar>::max());
23 template <
typename Scalar>
24 FrictionConeTpl<Scalar>::FrictionConeTpl(
const Vector3s& normal,
const Scalar& mu, std::size_t nf,
bool inner_appr,
25 const Scalar& min_nforce,
const Scalar& max_nforce)
29 std::cerr <<
"Warning: nf has to be an even number, set to 4" << std::endl;
31 A_.resize(nf_ + 1, 3);
36 update(normal, mu, inner_appr, min_nforce, max_nforce);
39 template <
typename Scalar>
40 FrictionConeTpl<Scalar>::FrictionConeTpl(
const FrictionConeTpl<Scalar>& cone)
44 nsurf_(cone.get_nsurf()),
47 inner_appr_(cone.get_inner_appr()),
48 min_nforce_(cone.get_min_nforce()),
49 max_nforce_(cone.get_max_nforce()) {}
51 template <
typename Scalar>
52 FrictionConeTpl<Scalar>::~FrictionConeTpl() {}
54 template <
typename Scalar>
55 void FrictionConeTpl<Scalar>::update(
const Vector3s& normal,
const Scalar& mu,
bool inner_appr,
56 const Scalar& min_nforce,
const Scalar& max_nforce) {
59 inner_appr_ = inner_appr;
60 min_nforce_ = min_nforce;
61 max_nforce_ = max_nforce;
64 if (!normal.isUnitary()) {
65 nsurf_ /= normal.norm();
66 std::cerr <<
"Warning: normal is not an unitary vector, then we normalized it" << std::endl;
68 if (min_nforce < 0.) {
70 std::cerr <<
"Warning: min_nforce has to be a positive value, set to 0" << std::endl;
72 if (max_nforce < 0.) {
73 max_nforce_ = std::numeric_limits<Scalar>::max();
74 std::cerr <<
"Warning: max_nforce has to be a positive value, set to maximun value" << std::endl;
77 Scalar theta = 2 * M_PI /
static_cast<Scalar
>(nf_);
79 mu_ *= cos(theta / 2.);
82 Matrix3s c_R_o = Quaternions::FromTwoVectors(nsurf_, Vector3s::UnitZ()).toRotationMatrix();
83 for (std::size_t i = 0; i < nf_ / 2; ++i) {
84 Scalar theta_i = theta *
static_cast<Scalar
>(i);
85 Vector3s tsurf_i = Vector3s(cos(theta_i), sin(theta_i), 0.);
86 A_.row(2 * i) = (-mu_ * Vector3s::UnitZ() + tsurf_i).transpose() * c_R_o;
87 A_.row(2 * i + 1) = (-mu_ * Vector3s::UnitZ() - tsurf_i).transpose() * c_R_o;
88 lb_(2 * i) = -std::numeric_limits<Scalar>::max();
89 lb_(2 * i + 1) = -std::numeric_limits<Scalar>::max();
93 A_.row(nf_) = nsurf_.transpose();
94 lb_(nf_) = min_nforce_;
95 ub_(nf_) = max_nforce_;
98 template <
typename Scalar>
99 const typename MathBaseTpl<Scalar>::MatrixX3s& FrictionConeTpl<Scalar>::get_A()
const {
103 template <
typename Scalar>
104 const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_lb()
const {
108 template <
typename Scalar>
109 const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_ub()
const {
113 template <
typename Scalar>
114 const typename MathBaseTpl<Scalar>::Vector3s& FrictionConeTpl<Scalar>::get_nsurf()
const {
118 template <
typename Scalar>
119 const Scalar& FrictionConeTpl<Scalar>::get_mu()
const {
123 template <
typename Scalar>
124 const std::size_t& FrictionConeTpl<Scalar>::get_nf()
const {
128 template <
typename Scalar>
129 const bool& FrictionConeTpl<Scalar>::get_inner_appr()
const {
133 template <
typename Scalar>
134 const Scalar& FrictionConeTpl<Scalar>::get_min_nforce()
const {
138 template <
typename Scalar>
139 const Scalar& FrictionConeTpl<Scalar>::get_max_nforce()
const {
143 template <
typename Scalar>
144 std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X) {
145 os <<
" normal: " << X.get_nsurf().transpose() << std::endl;
146 os <<
" mu: " << X.get_mu() << std::endl;
147 os <<
" nf: " << X.get_nf() << std::endl;
148 os <<
"inner_appr: " << X.get_inner_appr() << std::endl;
149 os <<
" min_force: " << X.get_min_nforce() << std::endl;
150 os <<
" max_force: " << X.get_max_nforce() << std::endl;
Definition: action-base.hxx:11