hpp-core  4.10.1
Implement basic classes for canonical path planning for kinematic chains.
body-pair-collision.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014,2015,2016,2018 CNRS
3 // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
4 //
5 // This file is part of hpp-core
6 // hpp-core 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-core 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-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
20 # define HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
21 
22 # include <boost/icl/continuous_interval.hpp>
23 # include <boost/icl/interval_set.hpp>
24 
25 # include <hpp/fcl/collision_data.h>
26 
29 
30 namespace hpp {
31  namespace core {
32  namespace continuousValidation {
52  {
53  public:
54  typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
55  typedef std::vector<CollisionPair_t> CollisionPairs_t;
56 
68  bool validateConfiguration (const value_type& t, interval_t& interval,
69  ValidationReportPtr_t& report,
70  const pinocchio::DeviceData& data);
71 
72  // Get pairs checked for collision
73  const CollisionPairs_t& pairs () const
74  {
75  return m_->pairs;
76  }
77 
78  // Get pairs checked for collision
80  {
81  return m_->pairs;
82  }
83 
84  // Get maximal velocity
86  {
87  return maximalVelocity_;
88  }
89 
91  virtual size_type indexJointA () const { return -1; }
93  virtual size_type indexJointB () const { return -1; }
94 
95  virtual bool removeObjectTo_b (const CollisionObjectConstPtr_t& /*object*/) { return false;}
96  virtual void addCollisionPair (const CollisionObjectConstPtr_t& /*left*/,
97  const CollisionObjectConstPtr_t &/*right*/) {}
98 
99  virtual std::string name () const = 0;
100  virtual std::ostream& print (std::ostream& os) const = 0;
101 
104 
107  {
108  return collisionRequest_.security_margin;
109  }
112  {
113  collisionRequest_.security_margin = securityMargin;
114  }
116  protected:
121  IntervalValidation(tolerance), m_ (new Model),
122  collisionRequest_(fcl::DISTANCE_LOWER_BOUND, 1), maximalVelocity_(0)
123  {
124  collisionRequest_.enable_cached_gjk_guess = true;
125  }
126 
129  IntervalValidation(other), m_(other.m_),
130  collisionRequest_(other.collisionRequest_),
131  maximalVelocity_(other.maximalVelocity_)
132  {}
133 
134  virtual void setReport (ValidationReportPtr_t& report,
135  fcl::CollisionResult result,
136  CollisionPair_t _pair) const
137  {
139  (new CollisionValidationReport (_pair, result));
140  }
141 
142  private:
143  struct Model {
145  };
146  boost::shared_ptr<Model> m_;
147  fcl::CollisionRequest collisionRequest_;
148 
149  mutable vector_t Vb_;
150  value_type maximalVelocity_;
151 
154  virtual value_type computeMaximalVelocity(vector_t& Vb) const = 0;
155 
158  virtual void setupPath();
159 
165  value_type collisionFreeInterval(const value_type &t,
166  const value_type &distanceLowerBound,
167  value_type &maxVelocity) const;
168 
173  virtual bool computeDistanceLowerBound(value_type &distanceLowerBound,
174  ValidationReportPtr_t& report,
175  const pinocchio::DeviceData& data);
176 
177  }; // class BodyPairCollision
178 
179  inline std::ostream& operator<< (std::ostream& os,
180  const BodyPairCollision& pair)
181  {
182  return pair.print (os);
183  }
184  } // namespace continuousValidation
185  } // namespace core
186 } // namespace hpp
187 #endif // HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
hpp::core::size_type
pinocchio::size_type size_type
Definition: fwd.hh:156
hpp::core::continuousValidation::BodyPairCollision::BodyPairCollision
BodyPairCollision(value_type tolerance)
Definition: body-pair-collision.hh:120
hpp::core::continuousValidation::BodyPairCollision::indexJointB
virtual size_type indexJointB() const
Returns joint B index or -1 if no such joint exists.
Definition: body-pair-collision.hh:93
hpp::core::continuousValidation::BodyPairCollision
Definition: body-pair-collision.hh:51
hpp::core::continuousValidation::operator<<
std::ostream & operator<<(std::ostream &os, const BodyPairCollision &pair)
Definition: body-pair-collision.hh:179
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition: fwd.hh:201
hpp::core::continuousValidation::IntervalValidation
Definition: interval-validation.hh:57
hpp::core::continuousValidation::BodyPairCollision::maximalVelocity
value_type maximalVelocity() const
Definition: body-pair-collision.hh:85
hpp::core::continuousValidation::BodyPairCollision::indexJointA
virtual size_type indexJointA() const
Returns joint A index or -1 if no such joint exists.
Definition: body-pair-collision.hh:91
hpp::core::continuousValidation::BodyPairCollision::CollisionPairs_t
std::vector< CollisionPair_t > CollisionPairs_t
Definition: body-pair-collision.hh:55
hpp::core::continuousValidation::BodyPairCollision::BodyPairCollision
BodyPairCollision(const BodyPairCollision &other)
Copy constructor.
Definition: body-pair-collision.hh:128
hpp::core::CollisionValidationReport
Definition: collision-validation-report.hh:35
hpp::core::CollisionObjectConstPtr_t
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90
hpp::core::continuousValidation::BodyPairCollision::setReport
virtual void setReport(ValidationReportPtr_t &report, fcl::CollisionResult result, CollisionPair_t _pair) const
Definition: body-pair-collision.hh:134
hpp::core::CollisionPairs_t
std::vector< CollisionPair_t > CollisionPairs_t
Definition: fwd.hh:220
hpp
Definition: basic-configuration-shooter.hh:26
hpp::core::continuousValidation::BodyPairCollision::validateConfiguration
bool validateConfiguration(const value_type &t, interval_t &interval, ValidationReportPtr_t &report, const pinocchio::DeviceData &data)
hpp::core::ValidationReportPtr_t
boost::shared_ptr< ValidationReport > ValidationReportPtr_t
Definition: fwd.hh:206
hpp::core::continuousValidation::BodyPairCollision::securityMargin
value_type securityMargin() const
Get security margin.
Definition: body-pair-collision.hh:106
hpp::core::continuousValidation::BodyPairCollision::name
virtual std::string name() const =0
hpp::core::continuousValidation::BodyPairCollision::print
virtual std::ostream & print(std::ostream &os) const =0
hpp::core::continuousValidation::IntervalValidation::tolerance
value_type tolerance() const
Definition: interval-validation.hh:90
hpp::core::CollisionValidationReportPtr_t
boost::shared_ptr< CollisionValidationReport > CollisionValidationReportPtr_t
Definition: fwd.hh:86
hpp::core::continuousValidation::BodyPairCollision::addCollisionPair
virtual void addCollisionPair(const CollisionObjectConstPtr_t &, const CollisionObjectConstPtr_t &)
Definition: body-pair-collision.hh:96
hpp::core::continuousValidation::BodyPairCollision::CollisionPair_t
std::pair< CollisionObjectConstPtr_t, CollisionObjectConstPtr_t > CollisionPair_t
Definition: body-pair-collision.hh:54
hpp::core::continuousValidation::BodyPairCollision::securityMargin
void securityMargin(const value_type &securityMargin)
Set security margin.
Definition: body-pair-collision.hh:111
hpp::core::continuousValidation::BodyPairCollision::pairs
CollisionPairs_t & pairs()
Definition: body-pair-collision.hh:79
hpp::core::value_type
pinocchio::value_type value_type
Definition: fwd.hh:157
hpp::core::continuousValidation::BodyPairCollision::pairs
const CollisionPairs_t & pairs() const
Definition: body-pair-collision.hh:73
hpp::core::continuousValidation::BodyPairCollision::removeObjectTo_b
virtual bool removeObjectTo_b(const CollisionObjectConstPtr_t &)
Definition: body-pair-collision.hh:95
collision-validation-report.hh
interval-validation.hh