GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2014 CNRS
3			// Authors: Florent Lamiraux
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_CONSTRAINT_SET_HH
31			#define HPP_CORE_CONSTRAINT_SET_HH
32
33			#include <deque>
34			#include <hpp/core/constraint.hh>
35
36			namespace hpp {
37			namespace core {
38			/// \addtogroup constraints
39			/// \{
40
41			/// Set of constraints applicable to a robot configuration
42			///
43			/// \warning If the set is to contain a ConfigProjector and several
44			/// LockedJoint instances, the configProjector should be inserted first
45			/// since following numerical projections might affect locked degrees of
46			/// freedom.
47			class HPP_CORE_DLLAPI ConstraintSet : public Constraint {
48			public:
49			/// Return shared pointer to new object
50		69	static ConstraintSetPtr_t create(const DevicePtr_t& robot,
51			const std::string& name) {
52	1/2 ✓ Branch 2 taken 69 times. ✗ Branch 3 not taken.	69	ConstraintSet* ptr = new ConstraintSet(robot, name);
53		69	ConstraintSetPtr_t shPtr(ptr);
54		69	ptr->init(shPtr);
55		69	return shPtr;
56			}
57
58			/// Return shared pointer to new object
59		3705931	static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t& cs) {
60	1/2 ✓ Branch 3 taken 3705931 times. ✗ Branch 4 not taken.	3705931	ConstraintSet* ptr = new ConstraintSet(*cs);
61		3705931	ConstraintSetPtr_t shPtr(ptr);
62		3705931	ptr->init(shPtr);
63		3705931	return shPtr;
64			}
65
66			/// return shared pointer to copy
67			virtual ConstraintPtr_t copy() const;
68
69			/// Add a constraint to the set
70			void addConstraint(const ConstraintPtr_t& constraint);
71
72			/// Return pointer to config projector if any
73			ConfigProjectorPtr_t configProjector() const;
74
75			/// Iterator over the constraints
76		✗	Constraints_t::iterator begin() { return constraints_.begin(); }
77			/// Iterator over the constraints
78		✗	Constraints_t::iterator end() { return constraints_.end(); }
79
80			/// Check whether a configuration statisfies the constraint.
81			virtual bool isSatisfied(ConfigurationIn_t config);
82
83			/// Check whether a configuration satisfies the constraint.
84			///
85			/// \param config the configuration to check
86			/// \retval error concatenation of errors of each constraint.
87			virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
88
89			/// \name Compression of locked degrees of freedom
90			///
91			/// Degrees of freedom related to locked joint are not taken into
92			/// account in numerical constraint resolution. The following methods
93			/// Compress or uncompress vectors or matrices by removing lines and
94			/// columns corresponding to locked degrees of freedom.
95			/// \{
96
97			/// Get number of non-locked degrees of freedom
98			size_type numberNonLockedDof() const;
99
100			/// Compress Velocity vector by removing locked degrees of freedom
101			///
102			/// \param normal input velocity vector
103			/// \retval small compressed velocity vectors
104			void compressVector(vectorIn_t normal, vectorOut_t small) const;
105
106			/// Expand compressed velocity vector
107			///
108			/// \param small compressed velocity vector without locked degrees of
109			/// freedom,
110			/// \retval normal uncompressed velocity vector.
111			/// \note locked degree of freedom are not set. They should be initialized
112			/// to zero.
113			void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
114
115			/// Compress matrix
116			///
117			/// \param normal input matrix
118			/// \retval small compressed matrix
119			/// \param rows whether to compress rows and colums or only columns
120			void compressMatrix(matrixIn_t normal, matrixOut_t small,
121			bool rows = true) const;
122
123			/// Uncompress matrix
124			///
125			/// \param small input matrix
126			/// \retval normal uncompressed matrix
127			/// \param rows whether to uncompress rows and colums or only columns
128			void uncompressMatrix(matrixIn_t small, matrixOut_t normal,
129			bool rows = true) const;
130			/// \}
131
132			protected:
133			ConstraintSet(const DevicePtr_t& robot, const std::string& name);
134			/// Copy constructor
135			ConstraintSet(const ConstraintSet& other);
136			/// Store weak pointer to itself.
137		3706000	void init(const ConstraintSetPtr_t& self) {
138		3706000	Constraint::init(self);
139		3706000	weak_ = self;
140		3706000	}
141			virtual bool impl_compute(ConfigurationOut_t configuration);
142
143			virtual std::ostream& print(std::ostream& os) const;
144
145			/// Constructor for serialization only.
146		✗	ConstraintSet() {}
147
148			private:
149			ConfigProjectorPtr_t _configProj() const;
150
151			Constraints_t constraints_;
152			int configProjI_;
153			ConstraintSetWkPtr_t weak_;
154
155			friend class constraints::LockedJoint;
156			friend class Constraint;
157			friend class ConfigProjector;
158
159			HPP_SERIALIZABLE();
160			}; // class ConstraintSet
161			/// \}
162			} // namespace core
163			} // namespace hpp
164
165		18	BOOST_CLASS_EXPORT_KEY(hpp::core::ConstraintSet)
166
167			#endif // HPP_CORE_CONSTRAINT_SET_HH
168