GCC Code Coverage Report


Directory: ./
File: include/hpp/core/continuous-validation/body-pair-collision.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 12 20 60.0%
Branches: 5 12 41.7%

Line Branch Exec Source
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
41 namespace hpp {
42 namespace core {
43 namespace continuousValidation {
44 /// Computation of collision-free sub-intervals of a path.
45 ///
46 /// This class aims at validating a path for the absence of collision
47 /// between two bodies of a robot, or between a robot body and the
48 /// environment. Bodies are considered as rigid.
49 ///
50 /// If the bodies are part of an open kinematic chain, the
51 /// computations are performed by class SolidSolidCollision.
52 ///
53 /// With this abstraction, other bodies (like legs of a parallel robot)
54 /// can also be checked for collision. In this case, the specialized
55 /// class needs to implement method \link
56 /// BodyPairCollision::computeMaximalVelocity computeMaximalVelocity
57 /// \endlink, taking into account the constrained motion of the legs
58 /// implied by the closure of the kinematic chain.
59 ///
60 /// See <a href="continuous-validation.pdf"> this document </a>
61 /// for details.
62 class BodyPairCollision : public IntervalValidation {
63 public:
64 /// Validate interval centered on a path parameter
65 /// \param t parameter value in the path interval of definition
66 /// \param[in,out] interval as input, interval over which
67 /// collision checking must be performed.
68 /// As output, interval over which pair is collision-free,
69 /// not necessarily included in definition interval.
70 /// \param report the collision validation report
71 /// \return true if the body pair is collision free for this parameter
72 /// value, false if the body pair is in collision.
73 /// \note object should be in the positions defined by the configuration
74 /// of parameter t on the path.
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 88711 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 88799 CollisionRequests_t& requests() { return m_->requests; }
90
91 // Get maximal velocity
92 value_type maximalVelocity() const { return maximalVelocity_; }
93
94 /// Returns joint A index or -1 if no such joint exists.
95 virtual size_type indexJointA() const { return -1; }
96 /// Returns joint B index or -1 if no such joint exists.
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
108 /// \name Security margin
109 /// \{
110
111 /// Get security margin of the first pair of object.
112 value_type securityMargin() const { return m_->requests[0].security_margin; }
113 /// Set security margin
114 void securityMargin(const value_type& securityMargin) {
115 for (auto& request : m_->requests) request.security_margin = securityMargin;
116 }
117 /// \}
118 protected:
119 /// Constructor of body pair collision
120 ///
121 /// \param tolerance allowed penetration should be positive
122 49 BodyPairCollision(value_type tolerance)
123
3/6
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 49 times.
✗ Branch 10 not taken.
49 : IntervalValidation(tolerance), m_(new Model), maximalVelocity_(0) {}
124
125 /// Copy constructor
126 153 BodyPairCollision(const BodyPairCollision& other)
127 153 : IntervalValidation(other),
128 153 m_(other.m_),
129
1/2
✓ Branch 3 taken 153 times.
✗ Branch 4 not taken.
153 maximalVelocity_(other.maximalVelocity_) {}
130
131 95 virtual void setReport(ValidationReportPtr_t& report,
132 coal::CollisionResult result,
133 CollisionPair_t _pair) const {
134 190 report = CollisionValidationReportPtr_t(
135
1/2
✓ Branch 2 taken 95 times.
✗ Branch 3 not taken.
190 new CollisionValidationReport(_pair, result));
136 95 }
137
138 private:
139 struct Model {
140 CollisionPairs_t pairs;
141 /// \todo The collision request is not part of the model since the
142 /// request gjk guess is updated at each collision check.
143 CollisionRequests_t requests;
144 };
145 shared_ptr<Model> m_;
146
147 mutable vector_t Vb_;
148 value_type maximalVelocity_;
149
150 /// Compute maximal velocity for a given velocity bound
151 /// \param Vb velocity
152 virtual value_type computeMaximalVelocity(vector_t& Vb) const = 0;
153
154 /// Compute the maximal velocity along the path
155 /// To be called after a new path has been set
156 virtual void setupPath();
157
158 /// Compute a collision free interval around t given a lower bound of
159 /// the distance to obstacle.
160 /// \param t the time in the path to test for a collision free interval
161 /// \return distanceLowerBound the interval half length
162 /// \retval maxVelocity the maximum velocity reached during this interval
163 value_type collisionFreeInterval(const value_type& t,
164 const value_type& distanceLowerBound,
165 value_type& maxVelocity) const;
166
167 /// Compute a lower bound of the distance between the bodies
168 /// \retval distanceLowerBound
169 /// \retval report the collision validation report
170 /// \return true if the bodies are not in collision, else false
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
185