GCC Code Coverage Report

Directory: ./
File: src/constraint-set.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 45 76 59.2%
Branches: 37 114 32.5%
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 #include <boost/serialization/vector.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/core/config-projector.hh>
33 #include <hpp/core/constraint-set.hh>
34 #include <hpp/pinocchio/configuration.hh>
35 #include <hpp/util/debug.hh>
36 #include <hpp/util/serialization.hh>
37
38 namespace hpp {
39 namespace core {
40 1861337 bool ConstraintSet::impl_compute(ConfigurationOut_t configuration) {
41 1861337 for (Constraints_t::iterator itConstraint = constraints_.begin();
42
2/2
✓ Branch 3 taken 1861337 times.
✓ Branch 4 taken 1861301 times.
 3722638 itConstraint != constraints_.end(); ++itConstraint) {
43
4/6
✓ Branch 3 taken 1861337 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1861337 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 36 times.
✓ Branch 9 taken 1861301 times.
 1861337 if (!(*itConstraint)->impl_compute(configuration)) return false;
44 }
45 1861301 return true;
46 }
47
48
1/2
✓ Branch 2 taken 3705931 times.
✗ Branch 3 not taken.
 3705931 ConstraintPtr_t ConstraintSet::copy() const { return createCopy(weak_.lock()); }
49
50 69 ConstraintSet::ConstraintSet(const DevicePtr_t& robot, const std::string& name)
51 69 : Constraint(name), constraints_(), configProjI_(-1) {
52
3/6
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 69 times.
✗ Branch 10 not taken.
 69 constraints_.push_back(ConfigProjector::create(robot, "Trivial", 1e-12, 0));
53 69 }
54
55 3705931 ConstraintSet::ConstraintSet(const ConstraintSet& other)
56 3705931 : Constraint(other), constraints_(), configProjI_(other.configProjI_) {
57 3705931 for (Constraints_t::const_iterator it = other.constraints_.begin();
58
2/2
✓ Branch 2 taken 3705931 times.
✓ Branch 3 taken 3705931 times.
 7411862 it != other.constraints_.end(); ++it)
59
2/4
✓ Branch 3 taken 3705931 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3705931 times.
✗ Branch 7 not taken.
 3705931 constraints_.push_back((*it)->copy());
60 3705931 }
61
62 9 void ConstraintSet::addConstraint(const ConstraintPtr_t& c) {
63 9 ConfigProjectorPtr_t cp(HPP_DYNAMIC_PTR_CAST(ConfigProjector, c));
64 9 ConstraintSetPtr_t cs(HPP_DYNAMIC_PTR_CAST(ConstraintSet, c));
65
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (cp) {
66
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 9 times.
 9 if (configProjI_ >= 0)
67 ✗ throw std::runtime_error("Constraint set " + name() +
68 " cannot store "
69 ✗ "more than one config-projector.");
70
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 constraints_.erase(constraints_.begin());
71 9 configProjI_ = static_cast<int>(constraints_.size());
72 ✗ } else if (cs) {
73 ✗ for (Constraints_t::iterator _c = cs->begin(); _c != cs->end(); ++_c)
74 ✗ addConstraint(*_c);
75 ✗ return;
76 }
77
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 constraints_.push_back(c);
78
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 }
79
80 24092958 ConfigProjectorPtr_t ConstraintSet::configProjector() const {
81 24092958 return (configProjI_ >= 0
82 24085323 ? HPP_STATIC_PTR_CAST(ConfigProjector, constraints_[configProjI_])
83
2/2
✓ Branch 0 taken 24085323 times.
✓ Branch 1 taken 7635 times.
 48178281 : ConfigProjectorPtr_t());
84 }
85
86 7419872 bool ConstraintSet::isSatisfied(ConfigurationIn_t configuration) {
87 7419872 for (Constraints_t::iterator itConstraint = constraints_.begin();
88
2/2
✓ Branch 3 taken 7419872 times.
✓ Branch 4 taken 7419872 times.
 14839744 itConstraint != constraints_.end(); ++itConstraint) {
89
3/6
✓ Branch 3 taken 7419872 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7419872 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 7419872 times.
 7419872 if (!(*itConstraint)->isSatisfied(configuration)) {
90 ✗ return false;
91 }
92 }
93 7419872 return true;
94 }
95
96 56 bool ConstraintSet::isSatisfied(ConfigurationIn_t configuration,
97 vector_t& error) {
98 56 bool result = true;
99
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 56 error.resize(0);
100
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 56 vector_t localError;
101 56 for (Constraints_t::iterator itConstraint = constraints_.begin();
102
2/2
✓ Branch 2 taken 56 times.
✓ Branch 3 taken 56 times.
 112 itConstraint != constraints_.end(); ++itConstraint) {
103
3/6
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 56 times.
 56 if (!(*itConstraint)->isSatisfied(configuration, localError)) {
104 ✗ result = false;
105 }
106
1/2
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
 56 error.conservativeResize(error.size() + localError.size());
107
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
 56 error.tail(localError.size()) = localError;
108 }
109 56 return result;
110 56 }
111
112 ✗ size_type ConstraintSet::numberNonLockedDof() const {
113 ✗ return _configProj()->numberFreeVariables();
114 }
115
116 ✗ void ConstraintSet::compressVector(vectorIn_t normal, vectorOut_t small) const {
117 ✗ _configProj()->compressVector(normal, small);
118 }
119
120 ✗ void ConstraintSet::uncompressVector(vectorIn_t small,
121 vectorOut_t normal) const {
122 ✗ _configProj()->uncompressVector(small, normal);
123 }
124
125 ✗ void ConstraintSet::compressMatrix(matrixIn_t normal, matrixOut_t small,
126 bool rows) const {
127 ✗ _configProj()->compressMatrix(normal, small, rows);
128 }
129
130 ✗ void ConstraintSet::uncompressMatrix(matrixIn_t small, matrixOut_t normal,
131 bool rows) const {
132 ✗ _configProj()->uncompressMatrix(small, normal, rows);
133 }
134
135 ✗ std::ostream& ConstraintSet::print(std::ostream& os) const {
136 ✗ os << "Constraint set " << name() << ", contains" << incindent;
137 ✗ for (Constraints_t::const_iterator itConstraint = constraints_.begin();
138 ✗ itConstraint != constraints_.end(); itConstraint++) {
139 ✗ os << iendl << **itConstraint;
140 }
141 ✗ return os << decindent;
142 }
143
144 ✗ ConfigProjectorPtr_t ConstraintSet::_configProj() const {
145 ✗ return HPP_STATIC_PTR_CAST(
146 ConfigProjector, constraints_[configProjI_ >= 0 ? configProjI_ : 0]);
147 }
148
149 template <class Archive>
150 ✗ void ConstraintSet::serialize(Archive& ar, const unsigned int version) {
151 using namespace boost::serialization;
152 (void)version;
153 ✗ ar& make_nvp("base", base_object<Constraint>(*this));
154 ✗ ar& BOOST_SERIALIZATION_NVP(constraints_);
155 ✗ ar& BOOST_SERIALIZATION_NVP(configProjI_);
156 ✗ ar& BOOST_SERIALIZATION_NVP(weak_);
157 }
158
159 HPP_SERIALIZATION_IMPLEMENT(ConstraintSet);
160 } // namespace core
161 } // namespace hpp
162
163 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::core::ConstraintSet)
164