GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraint-base.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 50 104 48.1%
Branches: 24 184 13.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2024, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <boost/core/demangle.hpp>
10 #include <limits>
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 2661 ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
16 boost::shared_ptr<StateAbstract> state,
17 boost::shared_ptr<ResidualModelAbstract> residual, const std::size_t ng,
18 const std::size_t nh)
19 2661 : ng_internal_(ng),
20 2661 nh_internal_(nh),
21 2661 state_(state),
22 2661 residual_(residual),
23
3/4
✓ Branch 0 taken 235 times.
✓ Branch 1 taken 2426 times.
✓ Branch 2 taken 235 times.
✗ Branch 3 not taken.
 2661 type_((ng > 0 && nh > 0) ? ConstraintType::Both
24 2661 : (ng > 0 ? ConstraintType::Inequality
25 : ConstraintType::Equality)),
26
2/4
✓ Branch 2 taken 2661 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2661 times.
✗ Branch 6 not taken.
 2661 lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
27
2/4
✓ Branch 2 taken 2661 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2661 times.
✗ Branch 6 not taken.
 2661 ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
28 2661 nu_(residual->get_nu()),
29 2661 ng_(ng),
30 2661 nh_(nh),
31 3420 T_constraint_(residual->get_q_dependent() || residual->get_v_dependent()
32
3/4
✓ Branch 0 taken 759 times.
✓ Branch 1 taken 1902 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 759 times.
 3420 ? true
33 : false),
34
2/4
✓ Branch 4 taken 2661 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2661 times.
✗ Branch 8 not taken.
 5322 unone_(VectorXs::Zero(residual->get_nu())) {
35
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2661 times.
 2661 if (nh_ > residual_->get_nr()) {
36 ✗ throw_pretty("Invalid argument: "
37 << "the number of equality constraints (nh) is wrong as it is "
38 "bigger than the residual dimension.")
39 }
40 2661 std::size_t max_ng = 2 * (residual_->get_nr() - nh_);
41
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2661 times.
 2661 if (ng_ > max_ng) {
42 ✗ throw_pretty("Invalid argument: "
43 << "the number of inequality constraints (ng) is wrong as it "
44 "should be in the range [0, " +
45 std::to_string(max_ng) + "]");
46 }
47 2661 }
48
49 template <typename Scalar>
50 140 ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
51 boost::shared_ptr<StateAbstract> state, const std::size_t nu,
52 const std::size_t ng, const std::size_t nh, const bool T_const)
53 140 : ng_internal_(ng),
54 140 nh_internal_(nh),
55 140 state_(state),
56
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 residual_(boost::make_shared<ResidualModelAbstract>(state, ng + nh, nu)),
57
3/4
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 70 times.
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 140 type_((ng > 0 && nh > 0) ? ConstraintType::Both
58 140 : (ng > 0 ? ConstraintType::Inequality
59 : ConstraintType::Equality)),
60
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
 140 lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
61
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
 140 ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
62 140 nu_(nu),
63 140 ng_(ng),
64 140 nh_(nh),
65 140 T_constraint_(T_const),
66
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
 280 unone_(VectorXs::Zero(nu)) {}
67
68 template <typename Scalar>
69 ✗ ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
70 boost::shared_ptr<StateAbstract> state, const std::size_t ng,
71 const std::size_t nh, const bool T_const)
72 ✗ : ng_internal_(ng),
73 ✗ nh_internal_(nh),
74 ✗ state_(state),
75 ✗ residual_(boost::make_shared<ResidualModelAbstract>(state, ng + nh)),
76 ✗ type_((ng > 0 && nh > 0) ? ConstraintType::Both
77 ✗ : (ng > 0 ? ConstraintType::Inequality
78 : ConstraintType::Equality)),
79 ✗ lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
80 ✗ ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
81 ✗ nu_(state->get_nv()),
82 ✗ ng_(ng),
83 ✗ nh_(nh),
84 ✗ T_constraint_(T_const),
85 ✗ unone_(VectorXs::Zero(state->get_nv())) {}
86
87 template <typename Scalar>
88 5606 ConstraintModelAbstractTpl<Scalar>::~ConstraintModelAbstractTpl() {}
89
90 template <typename Scalar>
91 ✗ void ConstraintModelAbstractTpl<Scalar>::calc(
92 const boost::shared_ptr<ConstraintDataAbstract>& data,
93 const Eigen::Ref<const VectorXs>& x) {
94 ✗ calc(data, x, unone_);
95 }
96
97 template <typename Scalar>
98 ✗ void ConstraintModelAbstractTpl<Scalar>::calcDiff(
99 const boost::shared_ptr<ConstraintDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>& x) {
101 ✗ calcDiff(data, x, unone_);
102 }
103
104 template <typename Scalar>
105 boost::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
106 ✗ ConstraintModelAbstractTpl<Scalar>::createData(
107 DataCollectorAbstract* const data) {
108 return boost::allocate_shared<ConstraintDataAbstract>(
109 ✗ Eigen::aligned_allocator<ConstraintDataAbstract>(), this, data);
110 }
111
112 template <typename Scalar>
113 ✗ void ConstraintModelAbstractTpl<Scalar>::update_bounds(const VectorXs& lower,
114 const VectorXs& upper) {
115 ✗ if (static_cast<std::size_t>(upper.size()) != ng_internal_ ||
116 ✗ static_cast<std::size_t>(lower.size()) != ng_internal_) {
117 ✗ throw_pretty(
118 "Invalid argument: the dimension of the lower/upper bound is not the "
119 "same to ng.")
120 }
121 ✗ if (((upper - lower).array() <= 0.).any()) {
122 ✗ throw_pretty(
123 "Invalid argument: the upper bound is not higher than the lower bound.")
124 }
125 ✗ if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() ||
126 ✗ (lb_.array() == std::numeric_limits<Scalar>::max()).any()) {
127 ✗ throw_pretty(
128 "Invalid argument: the lower bound cannot contain a positive "
129 "infinity/max value");
130 }
131 ✗ if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() ||
132 ✗ (ub_.array() == -std::numeric_limits<Scalar>::infinity()).any()) {
133 ✗ throw_pretty(
134 "Invalid argument: the lower bound cannot contain a negative "
135 "infinity/min value");
136 }
137 ✗ ng_ = ng_internal_;
138 ✗ nh_ = nh_internal_;
139 ✗ lb_ = lower;
140 ✗ ub_ = upper;
141 ✗ if (nh_ == 0) {
142 ✗ type_ = ConstraintType::Inequality;
143 } else {
144 ✗ type_ = ConstraintType::Both;
145 }
146 }
147
148 template <typename Scalar>
149 ✗ void ConstraintModelAbstractTpl<Scalar>::remove_bounds() {
150 ✗ ng_ = 0;
151 ✗ nh_ = nh_internal_ + ng_internal_;
152 ✗ lb_.setConstant(-std::numeric_limits<Scalar>::infinity());
153 ✗ ub_.setConstant(std::numeric_limits<Scalar>::infinity());
154 ✗ type_ = ConstraintType::Equality;
155 }
156
157 template <typename Scalar>
158 ✗ void ConstraintModelAbstractTpl<Scalar>::print(std::ostream& os) const {
159 ✗ os << boost::core::demangle(typeid(*this).name());
160 }
161
162 template <typename Scalar>
163 const boost::shared_ptr<StateAbstractTpl<Scalar> >&
164 467218 ConstraintModelAbstractTpl<Scalar>::get_state() const {
165 467218 return state_;
166 }
167
168 template <typename Scalar>
169 const boost::shared_ptr<ResidualModelAbstractTpl<Scalar> >&
170 229808 ConstraintModelAbstractTpl<Scalar>::get_residual() const {
171 229808 return residual_;
172 }
173
174 template <typename Scalar>
175 ✗ ConstraintType ConstraintModelAbstractTpl<Scalar>::get_type() const {
176 ✗ return type_;
177 }
178
179 template <typename Scalar>
180 const typename MathBaseTpl<Scalar>::VectorXs&
181 120534 ConstraintModelAbstractTpl<Scalar>::get_lb() const {
182 120534 return lb_;
183 }
184
185 template <typename Scalar>
186 const typename MathBaseTpl<Scalar>::VectorXs&
187 120534 ConstraintModelAbstractTpl<Scalar>::get_ub() const {
188 120534 return ub_;
189 }
190
191 template <typename Scalar>
192 464687 std::size_t ConstraintModelAbstractTpl<Scalar>::get_nu() const {
193 464687 return nu_;
194 }
195
196 template <typename Scalar>
197 1061873 std::size_t ConstraintModelAbstractTpl<Scalar>::get_ng() const {
198 1061873 return ng_;
199 }
200
201 template <typename Scalar>
202 1054568 std::size_t ConstraintModelAbstractTpl<Scalar>::get_nh() const {
203 1054568 return nh_;
204 }
205
206 template <typename Scalar>
207 15922 bool ConstraintModelAbstractTpl<Scalar>::get_T_constraint() const {
208 15922 return T_constraint_;
209 }
210
211 template <class Scalar>
212 ✗ std::ostream& operator<<(std::ostream& os,
213 const ConstraintModelAbstractTpl<Scalar>& model) {
214 ✗ model.print(os);
215 ✗ return os;
216 }
217
218 } // namespace crocoddyl
219