hpp-core: include/hpp/core/continuous-validation/body-pair-collision.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
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
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
31#define HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
32
33#include <coal/collision_data.h>
34
35#include <boost/icl/continuous_interval.hpp>
36#include <boost/icl/interval_set.hpp>
37#include <hpp/core/collision-pair.hh>
38#include <hpp/core/collision-validation-report.hh>
39#include <hpp/core/continuous-validation/interval-validation.hh>
40
41namespace hpp {
42namespace core {
43 namespace continuousValidation {
62 class BodyPairCollision : public IntervalValidation {
63 public:
75 bool validateConfiguration(const value_type& t, interval_t& interval,
76 ValidationReportPtr_t& report,
77 const pinocchio::DeviceData& data);
78
79 // Get pairs checked for collision
80 const CollisionPairs_t& pairs() const { return m_->pairs; }
81
82 // Get pairs checked for collision
83 CollisionPairs_t& pairs() { return m_->pairs; }
84
85 // Get requests checked for collision
86 const CollisionRequests_t& requests() const { return m_->requests; }
87
88 // Get requests checked for collision
89 CollisionRequests_t& requests() { return m_->requests; }
90
91 // Get maximal velocity
92 value_type maximalVelocity() const { return maximalVelocity_; }
93
95 virtual size_type indexJointA() const { return -1; }
97 virtual size_type indexJointB() const { return -1; }
98
99 virtual bool removeObjectTo_b(const CollisionObjectConstPtr_t& /*object*/) {
100 return false;
101 }
102 virtual void addCollisionPair(const CollisionObjectConstPtr_t& /*left*/,
103 const CollisionObjectConstPtr_t& /*right*/) {}
104
105 virtual std::string name() const = 0;
106 virtual std::ostream& print(std::ostream& os) const = 0;
107
110
112 value_type securityMargin() const { return m_->requests[0].security_margin; }
114 void securityMargin(const value_type& securityMargin) {
115 for (auto& request : m_->requests) request.security_margin = securityMargin;
116 }
118 protected:
122 BodyPairCollision(value_type tolerance)
123 : IntervalValidation(tolerance), m_(new Model), maximalVelocity_(0) {}
124
126 BodyPairCollision(const BodyPairCollision& other)
127 : IntervalValidation(other),
128 m_(other.m_),
129 maximalVelocity_(other.maximalVelocity_) {}
130
131 virtual void setReport(ValidationReportPtr_t& report,
132 coal::CollisionResult result,
133 CollisionPair_t _pair) const {
134 report = CollisionValidationReportPtr_t(
135 new CollisionValidationReport(_pair, result));
136 }
137
138 private:
139 struct Model {
140 CollisionPairs_t pairs;
143 CollisionRequests_t requests;
144 };
145 shared_ptr<Model> m_;
146
147 mutable vector_t Vb_;
148 value_type maximalVelocity_;
149
152 virtual value_type computeMaximalVelocity(vector_t& Vb) const = 0;
153
156 virtual void setupPath();
157
163 value_type collisionFreeInterval(const value_type& t,
164 const value_type& distanceLowerBound,
165 value_type& maxVelocity) const;
166
171 virtual bool computeDistanceLowerBound(value_type& distanceLowerBound,
172 ValidationReportPtr_t& report,
173 const pinocchio::DeviceData& data);
174
175}; // class BodyPairCollision
176
177 inline std::ostream& operator<<(std::ostream& os,
178 const BodyPairCollision& pair) {
179 return pair.print(os);
180}
181} // namespace continuousValidation
182} // namespace core
183} // namespace hpp
184#endif // HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH
hpp::core::continuousValidation::BodyPairCollision
Definition body-pair-collision.hh:62
hpp::core::continuousValidation::BodyPairCollision::setReport
virtual void setReport(ValidationReportPtr_t &report, coal::CollisionResult result, CollisionPair_t _pair) const
Definition body-pair-collision.hh:131
hpp::core::continuousValidation::BodyPairCollision::validateConfiguration
bool validateConfiguration(const value_type &t, interval_t &interval, ValidationReportPtr_t &report, const pinocchio::DeviceData &data)
hpp::core::continuousValidation::BodyPairCollision::addCollisionPair
virtual void addCollisionPair(const CollisionObjectConstPtr_t &, const CollisionObjectConstPtr_t &)
Definition body-pair-collision.hh:102
hpp::core::continuousValidation::BodyPairCollision::removeObjectTo_b
virtual bool removeObjectTo_b(const CollisionObjectConstPtr_t &)
Definition body-pair-collision.hh:99
hpp::core::continuousValidation::BodyPairCollision::requests
const CollisionRequests_t & requests() const
Definition body-pair-collision.hh:86
hpp::core::continuousValidation::BodyPairCollision::pairs
CollisionPairs_t & pairs()
Definition body-pair-collision.hh:83
hpp::core::continuousValidation::BodyPairCollision::securityMargin
value_type securityMargin() const
Get security margin of the first pair of object.
Definition body-pair-collision.hh:112
hpp::core::continuousValidation::BodyPairCollision::name
virtual std::string name() const =0
hpp::core::continuousValidation::BodyPairCollision::pairs
const CollisionPairs_t & pairs() const
Definition body-pair-collision.hh:80
hpp::core::continuousValidation::BodyPairCollision::BodyPairCollision
BodyPairCollision(value_type tolerance)
Definition body-pair-collision.hh:122
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:95
hpp::core::continuousValidation::BodyPairCollision::securityMargin
void securityMargin(const value_type &securityMargin)
Set security margin.
Definition body-pair-collision.hh:114
hpp::core::continuousValidation::BodyPairCollision::BodyPairCollision
BodyPairCollision(const BodyPairCollision &other)
Copy constructor.
Definition body-pair-collision.hh:126
hpp::core::continuousValidation::BodyPairCollision::print
virtual std::ostream & print(std::ostream &os) const =0
hpp::core::continuousValidation::BodyPairCollision::requests
CollisionRequests_t & requests()
Definition body-pair-collision.hh:89
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:97
hpp::core::continuousValidation::BodyPairCollision::maximalVelocity
value_type maximalVelocity() const
Definition body-pair-collision.hh:92
hpp::core::continuousValidation::IntervalValidation
Definition interval-validation.hh:67
hpp::core::continuousValidation::IntervalValidation::tolerance
value_type tolerance() const
Definition interval-validation.hh:95
collision-pair.hh
collision-validation-report.hh
interval-validation.hh
hpp
hpp::core::continuousValidation::operator<<
std::ostream & operator<<(std::ostream &os, const BodyPairCollision &pair)
Definition body-pair-collision.hh:177
hpp::core::CollisionPairs_t
std::vector< CollisionPair_t > CollisionPairs_t
Definition fwd.hh:239
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::CollisionRequests_t
std::vector< coal::CollisionRequest > CollisionRequests_t
Definition collision-pair.hh:43
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::ValidationReportPtr_t
shared_ptr< ValidationReport > ValidationReportPtr_t
Definition fwd.hh:225
hpp::core::CollisionValidationReportPtr_t
shared_ptr< CollisionValidationReport > CollisionValidationReportPtr_t
Definition fwd.hh:96
hpp::core::CollisionObjectConstPtr_t
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition fwd.hh:100
hpp::core::CollisionPair
Definition collision-pair.hh:45
hpp::core::CollisionValidationReport
Definition collision-validation-report.hh:47
hpp::pinocchio::DeviceData