hpp-affordance  4.14.0
Implementation to Extract Whole-Body Affordances for Mutli-Contact Planning.
operations.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Anna Seppala
4 //
5 // This file is part of hpp-affordance
6 // hpp-affordance is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-affordance is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-affordance If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_AFFORDANCE_OPERATIONS_HH
20 #define HPP_AFFORDANCE_OPERATIONS_HH
21 
22 #include <hpp/fcl/data_types.h>
23 
24 namespace hpp {
25 namespace affordance {
28 
33  public:
39  : zWorld_(0, 0, 1),
40  margin_(0.3),
42  minArea_(0.05),
43  affordance_("noAffordance") {}
53  explicit OperationBase(const double margin = 0.3,
54  const double nbTriMargin = 0.3,
55  const double minArea = 0.05,
56  const char* affordanceName = "noAffordance")
57  : zWorld_(0, 0, 1),
58  margin_(margin),
59  neighbouringTriangleMargin_(nbTriMargin),
60  minArea_(minArea),
61  affordance_(affordanceName) {}
66  virtual bool requirement(const fcl::Vec3f& normal) = 0;
69  const fcl::Vec3f zWorld_;
71  const double margin_;
77  const double minArea_;
79  const char* affordance_;
80 }; // class OperationBase
84  public:
93  explicit SupportOperation(const double margin = 0.3,
94  const double nbTriMargin = 0.3,
95  const double minArea = 0.05,
96  const char* affordanceName = "Support")
97  : OperationBase(margin, nbTriMargin, minArea, affordanceName) {}
101  bool requirement(const fcl::Vec3f& normal) {
102  return ((zWorld_ - normal).squaredNorm() < margin_);
103  }
104 }; // class SupportOperation
107 class LeanOperation : public OperationBase {
108  public:
117  explicit LeanOperation(const double margin = 0.3,
118  const double nbTriMargin = 0.3,
119  const double minArea = 0.05,
120  const char* affordanceName = "Lean")
121  : OperationBase(margin, nbTriMargin, minArea, affordanceName) {}
125  bool requirement(const fcl::Vec3f& normal) {
126  return (fabs(normal.dot(zWorld_)) < margin_);
127  }
128 }; // class LeanOperation
129 
133  public:
142  explicit Support45Operation(const double margin = 0.3,
143  const double nbTriMargin = 0.3,
144  const double minArea = 0.05,
145  const char* affordanceName = "Support45")
146  : OperationBase(margin, 0.05, minArea, affordanceName),
147  axis45_(fcl::Vec3f(1. / sqrt(2.), 0, 1. / sqrt(2.))) {}
148 
149  private:
150  const fcl::Vec3f axis45_;
151 
156  bool requirement(const fcl::Vec3f& normal) {
157  fcl::Vec3f projectedNormal(0, 0, normal[2]); // project normal in x,z plan
158  projectedNormal[0] = sqrt(normal[0] * normal[0] + normal[1] * normal[1]);
159  return ((axis45_ - projectedNormal).squaredNorm() < margin_);
160  }
161 }; // class LeanOperation
162 
164 } // namespace affordance
165 } // namespace hpp
166 
167 #endif // HPP_AFFORDANCE_OPERATIONS_HH
const fcl::Vec3f zWorld_
Definition: operations.hh:69
const double minArea_
Definition: operations.hh:77
Support45Operation(const double margin=0.3, const double nbTriMargin=0.3, const double minArea=0.05, const char *affordanceName="Support45")
Definition: operations.hh:142
Definition: affordance-extraction.hh:25
Definition: operations.hh:32
Definition: operations.hh:132
const double margin_
The error margin within which the requirement function must be fullfilled.
Definition: operations.hh:71
OperationBase()
Definition: operations.hh:38
bool requirement(const fcl::Vec3f &normal)
Definition: operations.hh:125
bool requirement(const fcl::Vec3f &normal)
Definition: operations.hh:101
Definition: operations.hh:83
Definition: operations.hh:107
SupportOperation(const double margin=0.3, const double nbTriMargin=0.3, const double minArea=0.05, const char *affordanceName="Support")
Definition: operations.hh:93
const char * affordance_
Name of the affordance type for which te requirement exists.
Definition: operations.hh:79
const double neighbouringTriangleMargin_
Definition: operations.hh:74
LeanOperation(const double margin=0.3, const double nbTriMargin=0.3, const double minArea=0.05, const char *affordanceName="Lean")
Definition: operations.hh:117
virtual bool requirement(const fcl::Vec3f &normal)=0
OperationBase(const double margin=0.3, const double nbTriMargin=0.3, const double minArea=0.05, const char *affordanceName="noAffordance")
Definition: operations.hh:53