Directory: | ./ |
---|---|
File: | include/hpp/core/constraint-set.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 15 | 18 | 83.3% |
Branches: | 2 | 4 | 50.0% |
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 |