Directory: | ./ |
---|---|
File: | include/hpp/core/continuous-validation/solid-solid-collision.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 9 | 10 | 90.0% |
Branches: | 3 | 4 | 75.0% |
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_SOLID_SOLID_COLLISION_HH | ||
31 | #define HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH | ||
32 | |||
33 | #include <hpp/core/continuous-validation/body-pair-collision.hh> | ||
34 | |||
35 | namespace hpp { | ||
36 | namespace core { | ||
37 | namespace continuousValidation { | ||
38 | struct CoefficientVelocity { | ||
39 | 265 | CoefficientVelocity() : value_(0) {} | |
40 | /// Joint the degrees of freedom of which the bounds correspond to. | ||
41 | JointPtr_t joint_; | ||
42 | value_type value_; | ||
43 | }; // struct CoefficientVelocity | ||
44 | typedef std::vector<CoefficientVelocity> CoefficientVelocities_t; | ||
45 | |||
46 | /// Computation of collision-free sub-intervals of a path | ||
47 | /// | ||
48 | /// This class derives from BodyPairCollision and validates a subinterval | ||
49 | /// of a path for collision between to robot bodies of between a robot | ||
50 | /// body and the environment. The robot body should be part of an open | ||
51 | /// kinematic chain. | ||
52 | /// | ||
53 | /// See <a href="continuous-validation.pdf"> this document </a> | ||
54 | /// for details. | ||
55 | class HPP_CORE_DLLAPI SolidSolidCollision : public BodyPairCollision { | ||
56 | public: | ||
57 | /// Create instance and return shared pointer | ||
58 | /// | ||
59 | /// \param joint_a joint of the body to test for collision with the | ||
60 | /// environment \param objects_b environment objects for the collision | ||
61 | /// checking \param tolerance allowed penetration should be positive \pre | ||
62 | /// objects_b should not be attached to a joint | ||
63 | static SolidSolidCollisionPtr_t create( | ||
64 | const JointPtr_t& joint_a, const ConstObjectStdVector_t& objects_b, | ||
65 | value_type tolerance); | ||
66 | |||
67 | /// Create instance and return shared pointer | ||
68 | /// | ||
69 | /// \param joint_a, joint_b joint of the bodies to test for collision | ||
70 | /// \param tolerance allowed penetrationd be positive | ||
71 | static SolidSolidCollisionPtr_t create(const JointPtr_t& joint_a, | ||
72 | const JointPtr_t& joint_b, | ||
73 | value_type tolerance); | ||
74 | |||
75 | /// Copy instance and return shared pointer. | ||
76 | static SolidSolidCollisionPtr_t createCopy( | ||
77 | const SolidSolidCollisionPtr_t& other); | ||
78 | |||
79 | value_type computeMaximalVelocity(vector_t& Vb) const; | ||
80 | |||
81 | bool removeObjectTo_b(const CollisionObjectConstPtr_t& object); | ||
82 | |||
83 | /// Set the break distance of each hpp::fcl::CollisionRequest | ||
84 | /// \sa void ContinuousValidation::breakDistance(value_type) const | ||
85 | void breakDistance(value_type distance); | ||
86 | |||
87 | std::string name() const; | ||
88 | |||
89 | std::ostream& print(std::ostream& os) const; | ||
90 | |||
91 | /// \note The left object should belong to joint_a and | ||
92 | /// the right one should belong to joint_b, or vice-versa. | ||
93 | /// This is not checked. | ||
94 | void addCollisionPair(const CollisionObjectConstPtr_t& left, | ||
95 | const CollisionObjectConstPtr_t& right); | ||
96 | |||
97 | // Get coefficients and joints | ||
98 | 4 | const CoefficientVelocities_t& coefficients() const { | |
99 | 4 | return m_->coefficients; | |
100 | } | ||
101 | |||
102 | /// Get joint a | ||
103 | ✗ | const JointPtr_t& joint_a() const { return m_->joint_a; } | |
104 | /// Get joint b | ||
105 | const JointPtr_t& joint_b() const { return m_->joint_b; } | ||
106 | |||
107 | /// Returns joint A index or -1 if no such joint exists. | ||
108 | 39 | size_type indexJointA() const { | |
109 |
1/2✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
|
39 | return (m_->joint_a ? m_->joint_a->index() : 0); |
110 | } | ||
111 | /// Returns joint B index or -1 if no such joint exists. | ||
112 | 39 | size_type indexJointB() const { | |
113 |
2/2✓ Branch 2 taken 27 times.
✓ Branch 3 taken 12 times.
|
39 | return (m_->joint_b ? m_->joint_b->index() : 0); |
114 | } | ||
115 | |||
116 | IntervalValidationPtr_t copy() const; | ||
117 | |||
118 | protected: | ||
119 | /// Constructor of inter-body collision checking | ||
120 | /// | ||
121 | /// \param joint_a, joint_b joint of the bodies to test for collision | ||
122 | /// \param tolerance allowed penetration should be positive | ||
123 | /// \pre joint_a and joint_b should not be nul pointers. | ||
124 | SolidSolidCollision(const JointPtr_t& joint_a, const JointPtr_t& joint_b, | ||
125 | value_type tolerance); | ||
126 | |||
127 | /// Constructor of collision checking with the environment | ||
128 | /// | ||
129 | /// \param joint_a joint of the body to test for collision with the | ||
130 | /// environment \param objects_b environment objects for the collision | ||
131 | /// checking \param tolerance allowed penetration should be positive \pre | ||
132 | /// objects_b should not be attached to a joint | ||
133 | SolidSolidCollision(const JointPtr_t& joint_a, | ||
134 | const ConstObjectStdVector_t& objects_b, | ||
135 | value_type tolerance); | ||
136 | |||
137 | /// Copy constructor | ||
138 | 153 | SolidSolidCollision(const SolidSolidCollision& other) | |
139 | 153 | : BodyPairCollision(other), m_(other.m_) {} | |
140 | |||
141 | void init(const SolidSolidCollisionWkPtr_t& weak); | ||
142 | |||
143 | private: | ||
144 | typedef pinocchio::JointIndex JointIndex; | ||
145 | typedef std::vector<JointIndex> JointIndices_t; | ||
146 | |||
147 | struct Model { | ||
148 | JointPtr_t joint_a; | ||
149 | JointPtr_t joint_b; | ||
150 | CoefficientVelocities_t coefficients; | ||
151 | CoefficientVelocities_t coefficients_reverse; | ||
152 | JointIndices_t computeSequenceOfJoints() const; | ||
153 | CoefficientVelocities_t computeCoefficients( | ||
154 | const JointIndices_t& joints) const; | ||
155 | void setCoefficients(const JointIndices_t& joints); | ||
156 | }; | ||
157 | shared_ptr<Model> m_; | ||
158 | SolidSolidCollisionWkPtr_t weak_; | ||
159 | }; // class SolidSolidCollision | ||
160 | } // namespace continuousValidation | ||
161 | } // namespace core | ||
162 | } // namespace hpp | ||
163 | #endif // HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH | ||
164 |