GCC Code Coverage Report

Line	Branch	Exec	Source
1			// Copyright (c) 2014, LAAS-CNRS
2			// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3			//
4
5			// Redistribution and use in source and binary forms, with or without
6			// modification, are permitted provided that the following conditions are
7			// met:
8			//
9			// 1. Redistributions of source code must retain the above copyright
10			// notice, this list of conditions and the following disclaimer.
11			//
12			// 2. Redistributions in binary form must reproduce the above copyright
13			// notice, this list of conditions and the following disclaimer in the
14			// documentation and/or other materials provided with the distribution.
15			//
16			// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17			// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18			// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19			// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20			// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21			// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22			// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23			// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24			// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25			// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26			// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27			// DAMAGE.
28
29			#ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
30			#define HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
31
32			#include <hpp/constraints/config.hh>
33			#include <hpp/constraints/convex-shape.hh>
34			#include <hpp/constraints/deprecated.hh>
35			#include <hpp/constraints/differentiable-function.hh>
36			#include <hpp/constraints/fwd.hh>
37			#include <hpp/constraints/generic-transformation.hh>
38			#include <vector>
39
40			namespace hpp {
41			namespace constraints {
42
43			/// \addtogroup constraints
44			/// \{
45
46			/** The function returns a relative transformation between the two "closest"
47			convex shapes it contains.
48
49			Two sets of convex shapes can be given to this class:
50			\li a set of object contact surfaces, \f$ (o_i)_{i \in I } \f$, which can be
51			in contact with the environment, \li a set of floor contact surfaces, \f$
52			(f_j)_{j \in J } \f$, which can support objects.
53
54			The distance \f$ d (f_j, o_i) \f$ between object surface
55			\f$o_i\f$ and environment surface \f$ f_j \f$ is defined by:
56			\f{equation*}
57			d (f_j, o_i)^2 =
58			\left\lbrace \begin{array}{cl}
59			d_{\parallel}^2 + d_{\perp}^2 &, \text{ if } d_{\parallel} > 0 \\
60			d_{\perp}^2 &, \text{ otherwise}
61			\end{array} \right.
62			\f}
63			where
64			\li \f$P (C_{o_i}, f_j)\f$ is the projection of the center \f$C_{o_i}\f$ of
65			\f$o_i\f$ onto the plane containing \f$ f_j \f$, \li \f$\textbf{n}_{f_j}\f$ is
66			the normal of \f$ f_j \f$, \li \f$d_{\parallel} = d(f_j, P (C_{o_i}, f_j))\f$ is
67			the distance returned by ConvexShapeData::distance, \li \f$d_{\perp} =
68			\textbf{n}_{f_j}.\vec{C_{f_j}C_{o_i}}\f$ is the distance along the normal of \f$
69			f_j \f$,
70
71			\image html convex-shape-contact.svg
72
73			The function first selects the pair \f$(o_i,f_j)\f$ with shortest distance.
74			\f$o_i\f$ is \em inside \f$f_j\f$ if \f$d(i,j) < 0\f$.
75			It returns a value that depends on the contact types:
76
77
78			\| Contact type \| Inside \| Outside \|
79			\| -------------- \| -------- \| ------- \|
80			\| ConvexShapeContact::POINT_ON_PLANE \| \f$(x+m,0,0,0,0)\f$ \|
81			\f$(x+m,y,z,0,0)\f$ \| \| ConvexShapeContact::LINE_ON_PLANE (Unsupported) \|
82			\f$(x+m,0,0,0,rz)\f$ \| \f$(x+m,y,z,0,rz)\f$ \| \|
83			ConvexShapeContact::PLANE_ON_PLANE \| \f$(x+m,0,0,ry,rz)\f$ \|
84			\f$(x+m,y,z,ry,rz)\f$ \|
85
86			where
87			\li \f$m\f$ is the normal margin (used to avoid collisions),
88			\li \f$x,y,z,rx,ry,rz\f$ represents the output of the RelativeTransformation
89			between the element of the pair.
90
91			\sa ConvexShapeContactComplement
92			**/
93			class HPP_CONSTRAINTS_DLLAPI ConvexShapeContact
94			: public DifferentiableFunction {
95			public:
96			friend class ConvexShapeContactComplement;
97			friend class ConvexShapeContactHold;
98
99			/// \cond
100			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
101			/// \endcond
102
103			/// The type of contact between each pair (object shape, floor shape).
104			enum ContactType {
105			/// The object shape is a single point,
106			POINT_ON_PLANE,
107			/// The object shape degenerates to a line,
108			LINE_ON_PLANE,
109			/// The object shape is included in a plane and none of the above case
110			/// apply.
111			PLANE_ON_PLANE
112			};
113
114			/// Represents a contact
115			/// When supportJoint is NULL, the contact is with the environment.
116			/// Otherwise, the contact is between two joints.
117			struct ForceData {
118			JointPtr_t joint;
119			JointPtr_t supportJoint;
120			std::vector<vector3_t> points;
121			vector3_t normal;
122			};
123
124			/// Create instance and return shared pointer
125			/// \param name name of the constraint,
126			/// \param robot robot that holds the contact surface
127			/// \param floorSurfaces, objectSurfaces set of plane polygonal contact
128			/// surfaces.
129			static ConvexShapeContactPtr_t create(const std::string& name,
130			DevicePtr_t robot,
131			const JointAndShapes_t& floorSurfaces,
132			const JointAndShapes_t& objectSurfaces);
133
134			static ConvexShapeContactPtr_t create(const DevicePtr_t& robot);
135
136			/// Get vector of floor contact surfaces
137		1	const ConvexShapes_t& floorContactSurfaces() const {
138		1	return floorConvexShapes_;
139			}
140			/// Get vector of object contact surfaces
141		1	const ConvexShapes_t& objectContactSurfaces() const {
142		1	return objectConvexShapes_;
143			}
144			/// Get radius \f$M\f$
145			/// See class documentation for a definition.
146		10605	value_type radius() const { return M_; }
147			/// Set the normal margin, i.e. the desired distance between matching
148			/// object and nd floor shapes.
149			/// Default to 0
150			void setNormalMargin(const value_type& margin);
151
152			/// Compute the contact points
153			std::vector<ForceData> computeContactPoints(
154			ConfigurationIn_t q, const value_type& normalMargin) const;
155
156			/// Display object in a stream
157			std::ostream& print(std::ostream& o) const;
158
159			/// Return pair of joints the relative pose between which
160			/// modifies the function value if any
161			///
162			/// Currently only supports the case when all the joints for floors are the
163			/// same and all the joints for objects involved are the same \return pair of
164			/// joints whose relative pose determines the value of the contact function,
165			/// arranged in order of increasing joint index, or a pair of empty shared
166			/// pointers.
167		3	std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
168			DeviceConstPtr_t /robot/) const {
169	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✗ Branch 4 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 7 taken 3 times.	3	if ((floorConvexShapes_.size() == 0) \|\| (objectConvexShapes_.size() == 0)) {
170		✗	return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
171			}
172		3	JointConstPtr_t floor0_joint = floorConvexShapes_[0].joint_;
173		3	JointConstPtr_t object0_joint = objectConvexShapes_[0].joint_;
174
175		3	size_type index1 = Joint::index(floor0_joint);
176		3	size_type index2 = Joint::index(object0_joint);
177			// check that all the joints involved are the same
178		3	for (ConvexShapes_t::const_iterator it(floorConvexShapes_.begin());
179	2/2 ✓ Branch 3 taken 6 times. ✓ Branch 4 taken 3 times.	9	it != floorConvexShapes_.end(); ++it) {
180		6	size_type jointIndex = Joint::index(it->joint_);
181	1/2 ✗ Branch 0 not taken. ✓ Branch 1 taken 6 times.	6	if (jointIndex != index1) {
182		✗	return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
183			}
184			}
185
186		3	for (ConvexShapes_t::const_iterator it(objectConvexShapes_.begin());
187	2/2 ✓ Branch 3 taken 6 times. ✓ Branch 4 taken 3 times.	9	it != objectConvexShapes_.end(); ++it) {
188		6	size_type jointIndex = Joint::index(it->joint_);
189	1/2 ✗ Branch 0 not taken. ✓ Branch 1 taken 6 times.	6	if (jointIndex != index2) {
190		✗	return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
191			}
192			}
193
194	1/2 ✓ Branch 0 taken 3 times. ✗ Branch 1 not taken.	3	if (index1 <= index2) {
195			return std::pair<JointConstPtr_t, JointConstPtr_t>(floor0_joint,
196		3	object0_joint);
197			} else {
198			return std::pair<JointConstPtr_t, JointConstPtr_t>(object0_joint,
199		✗	floor0_joint);
200			}
201		3	};
202
203			protected:
204			/// Constructor
205			/// \param name name of the constraint,
206			/// \param robot robot that holds the contact surface
207			/// \param floorSurfaces, objectSurfaces set of plane polygonal contact
208			/// surfaces.
209			ConvexShapeContact(const std::string& name, DevicePtr_t robot,
210			const JointAndShapes_t& floorSurfaces,
211			const JointAndShapes_t& objectSurfaces);
212
213			bool isEqual(const DifferentiableFunction& other) const;
214
215			private:
216			/// Add a ConvexShape as an object.
217			void addObject(const ConvexShape& t);
218
219			/// Add a ConvexShape as a floor.
220			///
221			/// The convex shape will be reverted using ConvexShape::reverse
222			/// so that the normal points inside the floor object.
223			void addFloor(const ConvexShape& t);
224			void computeRadius();
225
226			void impl_compute(LiegroupElementRef result,
227			ConfigurationIn_t argument) const;
228			void computeInternalValue(const ConfigurationIn_t& argument, bool& isInside,
229			ContactType& type, vector6_t& value,
230			std::size_t& iobject, std::size_t& ifloor) const;
231
232			void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
233			void computeInternalJacobian(const ConfigurationIn_t& argument,
234			bool& isInside, ContactType& type,
235			matrix_t& jacobian) const;
236
237			/// Find floor and object surfaces that are the closest.
238			/// \retval iobject, ifloor indices in internal vectors
239			/// objectConvexShapes_ and floorConvexShapes_
240			/// \return true if the contact is created.
241			bool selectConvexShapes(const pinocchio::DeviceData& data,
242			std::size_t& iobject, std::size_t& ifloor) const;
243			ContactType contactType(const ConvexShape& object,
244			const ConvexShape& floor) const;
245
246			DevicePtr_t robot_;
247			mutable GenericTransformationModel<true> relativeTransformationModel_;
248
249			ConvexShapes_t objectConvexShapes_;
250			ConvexShapes_t floorConvexShapes_;
251
252			value_type normalMargin_;
253			// upper bound of distance between center of polygon and vectices for
254			// all floor polygons.
255			value_type M_;
256			};
257
258			/** Complement to full transformation constraint of ConvexShapeContact
259
260			The value returned by this class is:
261
262			\| Contact type \| Inside \| Outside \|
263			\| -------------- \| -------- \| ------- \|
264			\| ConvexShapeContact::POINT_ON_PLANE (Unsupported) \| \f$(y,z,rx)\f$ \|
265			\f$(0,0,rx)\f$ \| \| ConvexShapeContact::LINE_ON_PLANE (Unsupported) \|
266			\f$(y,z,rx)\f$ \| \f$(0,0,rx)\f$ \| \| ConvexShapeContact::PLANE_ON_PLANE \|
267			\f$(y+2jM,z+2iM,rx)\f$ \| \f$(2jM,2iM,rx)\f$ \|
268
269			where
270			\li \f$M\f$ is an upper bound on the radius of all floor polygons,
271			\li \f$i\f$ and \f$j\f$ are the indices of object and floor contact
272			surfaces that minimize
273			\f{equation}
274			d(o_i,f_j)
275			\f}
276
277			\sa ConvexShapeContact
278			**/
279			class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactComplement
280			: public DifferentiableFunction {
281			public:
282			friend class ConvexShapeContactHold;
283			/// Create a pair of constraints
284			/// \param name name of the sibling ConvexShapeContact constraint,
285			/// "/complement" is added to the name of this constraint
286			/// \param robot robot that holds the contact surface
287			/// \param floorSurfaces, objectSurfaces set of plane polygonal contact
288			/// surfaces.
289			static std::pair<ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t>
290			createPair(const std::string& name, DevicePtr_t robot,
291			const JointAndShapes_t& floorSurfaces,
292			const JointAndShapes_t& objectSurfaces);
293
294			/// Compute parameters and right hand side of relative pose
295			/// \param rhs right hand side of this constraint,
296			/// \retval ifloor, iobject indices of floor and object contact surface,
297			/// \retval relativePoseRhs right hand side (implicit representation) of
298			/// the relative pose constraint corresponding to contact of
299			/// surfaces ifloor with iobject.
300			void computeRelativePoseRightHandSide(
301			LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
302			LiegroupElementRef relativePoseRhs) const;
303
304			/// Return pair of joints the relative pose between which
305			/// modifies the function value if any
306			///
307			/// Currently only supports the case when all the joints for floors are the
308			/// same and all the joints for objects involved are the same \return pair of
309			/// joints whose relative pose determines the value of the contact function,
310			/// arranged in order of increasing joint index, or a pair of empty shared
311			/// pointers.
312		1	std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
313			DeviceConstPtr_t /robot/) const {
314	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	return sibling_->dependsOnRelPoseBetween(nullptr);
315			};
316
317			protected:
318			/// Constructor
319			/// \param name name of the sibling ConvexShapeContact constraint,
320			/// "/complement" is added to the name of this constraint
321			/// \param robot robot that holds the contact surface
322			/// \param floorSurfaces, objectSurfaces set of plane polygonal contact
323			/// surfaces.
324			ConvexShapeContactComplement(const std::string& name, DevicePtr_t robot,
325			const JointAndShapes_t& floorSurfaces,
326			const JointAndShapes_t& objectSurfaces);
327
328			bool isEqual(const DifferentiableFunction& other) const;
329
330			private:
331			void impl_compute(LiegroupElementRef result,
332			ConfigurationIn_t argument) const;
333
334			void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
335
336			ConvexShapeContactPtr_t sibling_;
337			}; // class ConvexShapeContactComplement
338
339			/// Combination of ConvexShapeContact and complement constaints
340			///
341			/// Create one instance of
342			/// \li ConvexShapeContact,
343			/// \li ConvexShapeContactComplement,
344			///
345			/// and concatenate their values.
346			class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactHold
347			: public DifferentiableFunction {
348			public:
349			/// Create instance and return shared pointer
350			/// \param constraintName name of the ConvexShapeContact instance,
351			/// \param complementName name of the ConvexShapeContactComplement
352			/// instance,
353			/// \param holdName name of this DifferentiableFunction instance
354			/// \param robot the input space of the function is the robot
355			/// configuration space.
356			static ConvexShapeContactHoldPtr_t create(
357			const std::string& name, DevicePtr_t robot,
358			const JointAndShapes_t& floorSurfaces,
359			const JointAndShapes_t& objectSurfaces);
360
361		5	ConvexShapeContactPtr_t contactConstraint() const { return constraint_; }
362		401	ConvexShapeContactComplementPtr_t complement() const { return complement_; }
363
364			/// Return pair of joints the relative pose between which
365			/// modifies the function value if any
366			///
367			/// Currently only supports the case when all the joints for floors are the
368			/// same and all the joints for objects involved are the same \return pair of
369			/// joints whose relative pose determines the value of the contact function,
370			/// arranged in order of increasing joint index, or a pair of empty shared
371			/// pointers.
372		1	std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
373			DeviceConstPtr_t /robot/) const {
374	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	return constraint_->dependsOnRelPoseBetween(nullptr);
375			}
376
377			protected:
378			/// Constructor
379			/// \param constraintName name of the ConvexShapeContact instance,
380			/// \param complementName name of the ConvexShapeContactComplement
381			/// instance,
382			/// \param holdName name of this DifferentiableFunction instance
383			/// \param robot the input space of the function is the robot
384			/// configuration space.
385			ConvexShapeContactHold(const std::string& name, DevicePtr_t robot,
386			const JointAndShapes_t& floorSurfaces,
387			const JointAndShapes_t& objectSurfaces);
388
389			virtual void impl_compute(LiegroupElementRef result,
390			vectorIn_t argument) const;
391			virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const;
392
393			bool isEqual(const DifferentiableFunction& other) const;
394
395			private:
396			ConvexShapeContactPtr_t constraint_;
397			ConvexShapeContactComplementPtr_t complement_;
398			}; // class ConvexShapeContactHold
399			/// \}
400			} // namespace constraints
401			} // namespace hpp
402
403			#endif // HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
404