GCC Code Coverage Report

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