hpp-core  4.11.0
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 
30 
31 namespace hpp {
32  namespace core {
33  namespace continuousValidation {
53  {
54  public:
66  bool validateConfiguration (const value_type& t, interval_t& interval,
67  ValidationReportPtr_t& report,
68  const pinocchio::DeviceData& data);
69 
70  // Get pairs checked for collision
71  const CollisionPairs_t& pairs () const
72  {
73  return m_->pairs;
74  }
75 
76  // Get pairs checked for collision
78  {
79  return m_->pairs;
80  }
81 
82  // Get requests checked for collision
83  const CollisionRequests_t& requests () const
84  {
85  return m_->requests;
86  }
87 
88  // Get requests checked for collision
90  {
91  return m_->requests;
92  }
93 
94  // Get maximal velocity
96  {
97  return maximalVelocity_;
98  }
99 
101  virtual size_type indexJointA () const { return -1; }
103  virtual size_type indexJointB () const { return -1; }
104 
105  virtual bool removeObjectTo_b (const CollisionObjectConstPtr_t& /*object*/) { return false;}
106  virtual void addCollisionPair (const CollisionObjectConstPtr_t& /*left*/,
107  const CollisionObjectConstPtr_t &/*right*/) {}
108 
109  virtual std::string name () const = 0;
110  virtual std::ostream& print (std::ostream& os) const = 0;
111 
114 
117  {
118  return m_->requests[0].security_margin;
119  }
122  {
123  for (auto& request : m_->requests)
124  request.security_margin = securityMargin;
125  }
127  protected:
132  IntervalValidation(tolerance), m_ (new Model),
133  maximalVelocity_(0)
134  {}
135 
138  IntervalValidation(other), m_(other.m_),
139  maximalVelocity_(other.maximalVelocity_)
140  {}
141 
142  virtual void setReport (ValidationReportPtr_t& report,
143  fcl::CollisionResult result,
144  CollisionPair_t _pair) const
145  {
147  (new CollisionValidationReport (_pair, result));
148  }
149 
150  private:
151  struct Model {
156  };
157  shared_ptr<Model> m_;
158 
159  mutable vector_t Vb_;
160  value_type maximalVelocity_;
161 
164  virtual value_type computeMaximalVelocity(vector_t& Vb) const = 0;
165 
168  virtual void setupPath();
169 
175  value_type collisionFreeInterval(const value_type &t,
176  const value_type &distanceLowerBound,
177  value_type &maxVelocity) const;
178 
183  virtual bool computeDistanceLowerBound(value_type &distanceLowerBound,
184  ValidationReportPtr_t& report,
185  const pinocchio::DeviceData& data);
186 
187  }; // class BodyPairCollision
188 
189  inline std::ostream& operator<< (std::ostream& os,
190  const BodyPairCollision& pair)
191  {
192  return pair.print (os);
193  }
194  } // namespace continuousValidation
195  } // namespace core
196 } // namespace hpp
197 #endif // HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
CollisionRequests_t & requests()
Definition: body-pair-collision.hh:89
void securityMargin(const value_type &securityMargin)
Set security margin.
Definition: body-pair-collision.hh:121
virtual std::ostream & print(std::ostream &os) const =0
value_type tolerance() const
Definition: interval-validation.hh:90
Definition: bi-rrt-planner.hh:24
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
virtual void addCollisionPair(const CollisionObjectConstPtr_t &, const CollisionObjectConstPtr_t &)
Definition: body-pair-collision.hh:106
pinocchio::size_type size_type
Definition: fwd.hh:156
virtual void setReport(ValidationReportPtr_t &report, fcl::CollisionResult result, CollisionPair_t _pair) const
Definition: body-pair-collision.hh:142
const CollisionRequests_t & requests() const
Definition: body-pair-collision.hh:83
shared_ptr< CollisionValidationReport > CollisionValidationReportPtr_t
Definition: fwd.hh:86
std::vector< CollisionPair_t > CollisionPairs_t
Definition: fwd.hh:221
std::vector< fcl::CollisionRequest > CollisionRequests_t
Definition: collision-pair.hh:32
Definition: collision-pair.hh:34
value_type securityMargin() const
Get security margin of the first pair of object.
Definition: body-pair-collision.hh:116
CollisionPairs_t & pairs()
Definition: body-pair-collision.hh:77
BodyPairCollision(value_type tolerance)
Definition: body-pair-collision.hh:131
Definition: interval-validation.hh:57
shared_ptr< ValidationReport > ValidationReportPtr_t
Definition: fwd.hh:207
virtual size_type indexJointA() const
Returns joint A index or -1 if no such joint exists.
Definition: body-pair-collision.hh:101
bool validateConfiguration(const value_type &t, interval_t &interval, ValidationReportPtr_t &report, const pinocchio::DeviceData &data)
pinocchio::vector_t vector_t
Definition: fwd.hh:202
pinocchio::value_type value_type
Definition: fwd.hh:157
Definition: collision-validation-report.hh:36
virtual bool removeObjectTo_b(const CollisionObjectConstPtr_t &)
Definition: body-pair-collision.hh:105
value_type maximalVelocity() const
Definition: body-pair-collision.hh:95
Definition: body-pair-collision.hh:52
BodyPairCollision(const BodyPairCollision &other)
Copy constructor.
Definition: body-pair-collision.hh:137
const CollisionPairs_t & pairs() const
Definition: body-pair-collision.hh:71
virtual size_type indexJointB() const
Returns joint B index or -1 if no such joint exists.
Definition: body-pair-collision.hh:103
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90
std::ostream & operator<<(std::ostream &os, const BodyPairCollision &pair)
Definition: body-pair-collision.hh:189