| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2020-2025, University of Edinburgh, Heriot-Watt University | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | namespace crocoddyl { | ||
| 10 | |||
| 11 | template <typename Scalar> | ||
| 12 | ✗ | ConstraintModelManagerTpl<Scalar>::ConstraintModelManagerTpl( | |
| 13 | std::shared_ptr<StateAbstract> state, const std::size_t nu) | ||
| 14 | ✗ | : state_(state), | |
| 15 | ✗ | lb_(0), | |
| 16 | ✗ | ub_(0), | |
| 17 | ✗ | nu_(nu), | |
| 18 | ✗ | ng_(0), | |
| 19 | ✗ | nh_(0), | |
| 20 | ✗ | ng_T_(0), | |
| 21 | ✗ | nh_T_(0) {} | |
| 22 | |||
| 23 | template <typename Scalar> | ||
| 24 | ✗ | ConstraintModelManagerTpl<Scalar>::ConstraintModelManagerTpl( | |
| 25 | std::shared_ptr<StateAbstract> state) | ||
| 26 | ✗ | : state_(state), | |
| 27 | ✗ | lb_(0), | |
| 28 | ✗ | ub_(0), | |
| 29 | ✗ | nu_(state->get_nv()), | |
| 30 | ✗ | ng_(0), | |
| 31 | ✗ | nh_(0), | |
| 32 | ✗ | ng_T_(0), | |
| 33 | ✗ | nh_T_(0) {} | |
| 34 | |||
| 35 | template <typename Scalar> | ||
| 36 | ✗ | ConstraintModelManagerTpl<Scalar>::~ConstraintModelManagerTpl() {} | |
| 37 | |||
| 38 | template <typename Scalar> | ||
| 39 | ✗ | void ConstraintModelManagerTpl<Scalar>::addConstraint( | |
| 40 | const std::string& name, | ||
| 41 | std::shared_ptr<ConstraintModelAbstract> constraint, const bool active) { | ||
| 42 | ✗ | if (constraint->get_nu() != nu_) { | |
| 43 | ✗ | throw_pretty(name << " constraint item doesn't have the same control " | |
| 44 | "dimension (it should be " + | ||
| 45 | std::to_string(nu_) + ")"); | ||
| 46 | } | ||
| 47 | std::pair<typename ConstraintModelContainer::iterator, bool> ret = | ||
| 48 | ✗ | constraints_.insert(std::make_pair( | |
| 49 | name, std::make_shared<ConstraintItem>(name, constraint, active))); | ||
| 50 | ✗ | if (ret.second == false) { | |
| 51 | std::cout << "Warning: we couldn't add the " << name | ||
| 52 | ✗ | << " constraint item, it already existed." << std::endl; | |
| 53 | ✗ | } else if (active) { | |
| 54 | ✗ | ng_ += constraint->get_ng(); | |
| 55 | ✗ | nh_ += constraint->get_nh(); | |
| 56 | ✗ | if (constraint->get_T_constraint()) { | |
| 57 | ✗ | ng_T_ += constraint->get_ng(); | |
| 58 | ✗ | nh_T_ += constraint->get_nh(); | |
| 59 | } | ||
| 60 | ✗ | active_set_.insert(name); | |
| 61 | ✗ | lb_.resize(ng_); | |
| 62 | ✗ | ub_.resize(ng_); | |
| 63 | ✗ | } else if (!active) { | |
| 64 | ✗ | inactive_set_.insert(name); | |
| 65 | } | ||
| 66 | ✗ | } | |
| 67 | |||
| 68 | template <typename Scalar> | ||
| 69 | ✗ | void ConstraintModelManagerTpl<Scalar>::removeConstraint( | |
| 70 | const std::string& name) { | ||
| 71 | ✗ | typename ConstraintModelContainer::iterator it = constraints_.find(name); | |
| 72 | ✗ | if (it != constraints_.end()) { | |
| 73 | ✗ | if (it->second->active) { | |
| 74 | ✗ | ng_ -= it->second->constraint->get_ng(); | |
| 75 | ✗ | nh_ -= it->second->constraint->get_nh(); | |
| 76 | ✗ | if (it->second->constraint->get_T_constraint()) { | |
| 77 | ✗ | ng_T_ -= it->second->constraint->get_ng(); | |
| 78 | ✗ | nh_T_ -= it->second->constraint->get_nh(); | |
| 79 | } | ||
| 80 | ✗ | lb_.resize(ng_); | |
| 81 | ✗ | ub_.resize(ng_); | |
| 82 | ✗ | active_set_.erase(name); | |
| 83 | } else { | ||
| 84 | ✗ | inactive_set_.erase(name); | |
| 85 | } | ||
| 86 | ✗ | constraints_.erase(it); | |
| 87 | } else { | ||
| 88 | std::cout << "Warning: we couldn't remove the " << name | ||
| 89 | ✗ | << " constraint item, it doesn't exist." << std::endl; | |
| 90 | } | ||
| 91 | ✗ | } | |
| 92 | |||
| 93 | template <typename Scalar> | ||
| 94 | ✗ | void ConstraintModelManagerTpl<Scalar>::changeConstraintStatus( | |
| 95 | const std::string& name, bool active) { | ||
| 96 | ✗ | typename ConstraintModelContainer::iterator it = constraints_.find(name); | |
| 97 | ✗ | if (it != constraints_.end()) { | |
| 98 | ✗ | if (active && !it->second->active) { | |
| 99 | ✗ | ng_ += it->second->constraint->get_ng(); | |
| 100 | ✗ | nh_ += it->second->constraint->get_nh(); | |
| 101 | ✗ | if (it->second->constraint->get_T_constraint()) { | |
| 102 | ✗ | ng_T_ += it->second->constraint->get_ng(); | |
| 103 | ✗ | nh_T_ += it->second->constraint->get_nh(); | |
| 104 | } | ||
| 105 | ✗ | active_set_.insert(name); | |
| 106 | ✗ | inactive_set_.erase(name); | |
| 107 | ✗ | it->second->active = active; | |
| 108 | ✗ | lb_.resize(ng_); | |
| 109 | ✗ | ub_.resize(ng_); | |
| 110 | ✗ | } else if (!active && it->second->active) { | |
| 111 | ✗ | ng_ -= it->second->constraint->get_ng(); | |
| 112 | ✗ | nh_ -= it->second->constraint->get_nh(); | |
| 113 | ✗ | if (it->second->constraint->get_T_constraint()) { | |
| 114 | ✗ | ng_T_ -= it->second->constraint->get_ng(); | |
| 115 | ✗ | nh_T_ -= it->second->constraint->get_nh(); | |
| 116 | } | ||
| 117 | ✗ | active_set_.erase(name); | |
| 118 | ✗ | inactive_set_.insert(name); | |
| 119 | ✗ | it->second->active = active; | |
| 120 | ✗ | lb_.resize(ng_); | |
| 121 | ✗ | ub_.resize(ng_); | |
| 122 | } | ||
| 123 | } else { | ||
| 124 | std::cout << "Warning: we couldn't change the status of the " << name | ||
| 125 | ✗ | << " constraint item, it doesn't exist." << std::endl; | |
| 126 | } | ||
| 127 | ✗ | } | |
| 128 | |||
| 129 | template <typename Scalar> | ||
| 130 | ✗ | void ConstraintModelManagerTpl<Scalar>::calc( | |
| 131 | const std::shared_ptr<ConstraintDataManager>& data, | ||
| 132 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { | ||
| 133 | ✗ | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { | |
| 134 | ✗ | throw_pretty( | |
| 135 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 136 | std::to_string(state_->get_nx()) + ")"); | ||
| 137 | } | ||
| 138 | ✗ | if (static_cast<std::size_t>(u.size()) != nu_) { | |
| 139 | ✗ | throw_pretty( | |
| 140 | "Invalid argument: " << "u has wrong dimension (it should be " + | ||
| 141 | std::to_string(nu_) + ")"); | ||
| 142 | } | ||
| 143 | ✗ | if (data->constraints.size() != constraints_.size()) { | |
| 144 | ✗ | throw_pretty( | |
| 145 | "Invalid argument: " | ||
| 146 | << "it doesn't match the number of constraint datas and models"); | ||
| 147 | } | ||
| 148 | ✗ | assert_pretty(static_cast<std::size_t>(data->g.size()) == ng_, | |
| 149 | "the dimension of data.g doesn't correspond with ng=" << ng_); | ||
| 150 | ✗ | assert_pretty(static_cast<std::size_t>(data->h.size()) == nh_, | |
| 151 | "the dimension of data.h doesn't correspond with nh=" << nh_); | ||
| 152 | ✗ | std::size_t ng_i = 0; | |
| 153 | ✗ | std::size_t nh_i = 0; | |
| 154 | |||
| 155 | ✗ | typename ConstraintModelContainer::iterator it_m, end_m; | |
| 156 | ✗ | typename ConstraintDataContainer::iterator it_d, end_d; | |
| 157 | ✗ | for (it_m = constraints_.begin(), end_m = constraints_.end(), | |
| 158 | ✗ | it_d = data->constraints.begin(), end_d = data->constraints.end(); | |
| 159 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 160 | ✗ | const std::shared_ptr<ConstraintItem>& m_i = it_m->second; | |
| 161 | ✗ | if (m_i->active) { | |
| 162 | ✗ | const std::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; | |
| 163 | ✗ | assert_pretty( | |
| 164 | it_m->first == it_d->first, | ||
| 165 | "it doesn't match the constraint name between model and data (" | ||
| 166 | << it_m->first << " != " << it_d->first << ")"); | ||
| 167 | |||
| 168 | ✗ | m_i->constraint->calc(d_i, x, u); | |
| 169 | ✗ | const std::size_t ng = m_i->constraint->get_ng(); | |
| 170 | ✗ | const std::size_t nh = m_i->constraint->get_nh(); | |
| 171 | ✗ | data->g.segment(ng_i, ng) = d_i->g; | |
| 172 | ✗ | data->h.segment(nh_i, nh) = d_i->h; | |
| 173 | ✗ | lb_.segment(ng_i, ng) = m_i->constraint->get_lb(); | |
| 174 | ✗ | ub_.segment(ng_i, ng) = m_i->constraint->get_ub(); | |
| 175 | ✗ | ng_i += ng; | |
| 176 | ✗ | nh_i += nh; | |
| 177 | } | ||
| 178 | } | ||
| 179 | ✗ | } | |
| 180 | |||
| 181 | template <typename Scalar> | ||
| 182 | ✗ | void ConstraintModelManagerTpl<Scalar>::calc( | |
| 183 | const std::shared_ptr<ConstraintDataManager>& data, | ||
| 184 | const Eigen::Ref<const VectorXs>& x) { | ||
| 185 | ✗ | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { | |
| 186 | ✗ | throw_pretty( | |
| 187 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 188 | std::to_string(state_->get_nx()) + ")"); | ||
| 189 | } | ||
| 190 | ✗ | if (data->constraints.size() != constraints_.size()) { | |
| 191 | ✗ | throw_pretty( | |
| 192 | "Invalid argument: " | ||
| 193 | << "it doesn't match the number of constraint datas and models"); | ||
| 194 | } | ||
| 195 | ✗ | assert_pretty(static_cast<std::size_t>(data->g.size()) == ng_T_, | |
| 196 | "the dimension of data.g doesn't correspond with ng=" << ng_T_); | ||
| 197 | ✗ | assert_pretty(static_cast<std::size_t>(data->h.size()) == nh_T_, | |
| 198 | "the dimension of data.h doesn't correspond with nh=" << nh_T_); | ||
| 199 | ✗ | std::size_t ng_i = 0; | |
| 200 | ✗ | std::size_t nh_i = 0; | |
| 201 | |||
| 202 | ✗ | typename ConstraintModelContainer::iterator it_m, end_m; | |
| 203 | ✗ | typename ConstraintDataContainer::iterator it_d, end_d; | |
| 204 | ✗ | for (it_m = constraints_.begin(), end_m = constraints_.end(), | |
| 205 | ✗ | it_d = data->constraints.begin(), end_d = data->constraints.end(); | |
| 206 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 207 | ✗ | const std::shared_ptr<ConstraintItem>& m_i = it_m->second; | |
| 208 | ✗ | if (m_i->active && m_i->constraint->get_T_constraint()) { | |
| 209 | ✗ | const std::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; | |
| 210 | ✗ | assert_pretty( | |
| 211 | it_m->first == it_d->first, | ||
| 212 | "it doesn't match the constraint name between model and data (" | ||
| 213 | << it_m->first << " != " << it_d->first << ")"); | ||
| 214 | |||
| 215 | ✗ | m_i->constraint->calc(d_i, x); | |
| 216 | ✗ | const std::size_t ng = m_i->constraint->get_ng(); | |
| 217 | ✗ | const std::size_t nh = m_i->constraint->get_nh(); | |
| 218 | ✗ | data->g.segment(ng_i, ng) = d_i->g; | |
| 219 | ✗ | data->h.segment(nh_i, nh) = d_i->h; | |
| 220 | ✗ | lb_.segment(ng_i, ng) = m_i->constraint->get_lb(); | |
| 221 | ✗ | ub_.segment(ng_i, ng) = m_i->constraint->get_ub(); | |
| 222 | ✗ | ng_i += ng; | |
| 223 | ✗ | nh_i += nh; | |
| 224 | } | ||
| 225 | } | ||
| 226 | ✗ | } | |
| 227 | |||
| 228 | template <typename Scalar> | ||
| 229 | ✗ | void ConstraintModelManagerTpl<Scalar>::calcDiff( | |
| 230 | const std::shared_ptr<ConstraintDataManager>& data, | ||
| 231 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { | ||
| 232 | ✗ | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { | |
| 233 | ✗ | throw_pretty( | |
| 234 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 235 | std::to_string(state_->get_nx()) + ")"); | ||
| 236 | } | ||
| 237 | ✗ | if (static_cast<std::size_t>(u.size()) != nu_) { | |
| 238 | ✗ | throw_pretty( | |
| 239 | "Invalid argument: " << "u has wrong dimension (it should be " + | ||
| 240 | std::to_string(nu_) + ")"); | ||
| 241 | } | ||
| 242 | ✗ | if (data->constraints.size() != constraints_.size()) { | |
| 243 | ✗ | throw_pretty( | |
| 244 | "Invalid argument: " | ||
| 245 | << "it doesn't match the number of constraint datas and models"); | ||
| 246 | } | ||
| 247 | ✗ | assert_pretty(static_cast<std::size_t>(data->Gx.rows()) == ng_, | |
| 248 | "the dimension of data.Gx doesn't correspond with ng=" << ng_); | ||
| 249 | ✗ | assert_pretty(static_cast<std::size_t>(data->Gu.rows()) == ng_, | |
| 250 | "the dimension of data.Gu doesn't correspond with ng=" << ng_); | ||
| 251 | ✗ | assert_pretty( | |
| 252 | static_cast<std::size_t>(data->Hx.rows()) == nh_, | ||
| 253 | "the dimension of data.Hx,u doesn't correspond with nh=" << nh_); | ||
| 254 | ✗ | const std::size_t ndx = state_->get_ndx(); | |
| 255 | ✗ | std::size_t ng_i = 0; | |
| 256 | ✗ | std::size_t nh_i = 0; | |
| 257 | |||
| 258 | ✗ | typename ConstraintModelContainer::iterator it_m, end_m; | |
| 259 | ✗ | typename ConstraintDataContainer::iterator it_d, end_d; | |
| 260 | ✗ | for (it_m = constraints_.begin(), end_m = constraints_.end(), | |
| 261 | ✗ | it_d = data->constraints.begin(), end_d = data->constraints.end(); | |
| 262 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 263 | ✗ | const std::shared_ptr<ConstraintItem>& m_i = it_m->second; | |
| 264 | ✗ | if (m_i->active) { | |
| 265 | ✗ | const std::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; | |
| 266 | ✗ | assert_pretty( | |
| 267 | it_m->first == it_d->first, | ||
| 268 | "it doesn't match the constraint name between model and data (" | ||
| 269 | << it_m->first << " != " << it_d->first << ")"); | ||
| 270 | |||
| 271 | ✗ | m_i->constraint->calcDiff(d_i, x, u); | |
| 272 | ✗ | const std::size_t ng = m_i->constraint->get_ng(); | |
| 273 | ✗ | const std::size_t nh = m_i->constraint->get_nh(); | |
| 274 | ✗ | data->Gx.block(ng_i, 0, ng, ndx) = d_i->Gx; | |
| 275 | ✗ | data->Gu.block(ng_i, 0, ng, nu_) = d_i->Gu; | |
| 276 | ✗ | data->Hx.block(nh_i, 0, nh, ndx) = d_i->Hx; | |
| 277 | ✗ | data->Hu.block(nh_i, 0, nh, nu_) = d_i->Hu; | |
| 278 | ✗ | ng_i += ng; | |
| 279 | ✗ | nh_i += nh; | |
| 280 | } | ||
| 281 | } | ||
| 282 | ✗ | } | |
| 283 | |||
| 284 | template <typename Scalar> | ||
| 285 | ✗ | void ConstraintModelManagerTpl<Scalar>::calcDiff( | |
| 286 | const std::shared_ptr<ConstraintDataManager>& data, | ||
| 287 | const Eigen::Ref<const VectorXs>& x) { | ||
| 288 | ✗ | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { | |
| 289 | ✗ | throw_pretty( | |
| 290 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 291 | std::to_string(state_->get_nx()) + ")"); | ||
| 292 | } | ||
| 293 | ✗ | if (data->constraints.size() != constraints_.size()) { | |
| 294 | ✗ | throw_pretty( | |
| 295 | "Invalid argument: " | ||
| 296 | << "it doesn't match the number of constraint datas and models"); | ||
| 297 | } | ||
| 298 | ✗ | assert_pretty( | |
| 299 | static_cast<std::size_t>(data->Gx.rows()) == ng_T_, | ||
| 300 | "the dimension of data.Gx,u doesn't correspond with ng=" << ng_T_); | ||
| 301 | ✗ | assert_pretty( | |
| 302 | static_cast<std::size_t>(data->Hx.rows()) == nh_T_, | ||
| 303 | "the dimension of data.Hx,u doesn't correspond with nh=" << nh_T_); | ||
| 304 | ✗ | const std::size_t ndx = state_->get_ndx(); | |
| 305 | ✗ | std::size_t ng_i = 0; | |
| 306 | ✗ | std::size_t nh_i = 0; | |
| 307 | |||
| 308 | ✗ | typename ConstraintModelContainer::iterator it_m, end_m; | |
| 309 | ✗ | typename ConstraintDataContainer::iterator it_d, end_d; | |
| 310 | ✗ | for (it_m = constraints_.begin(), end_m = constraints_.end(), | |
| 311 | ✗ | it_d = data->constraints.begin(), end_d = data->constraints.end(); | |
| 312 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 313 | ✗ | const std::shared_ptr<ConstraintItem>& m_i = it_m->second; | |
| 314 | ✗ | if (m_i->active && m_i->constraint->get_T_constraint()) { | |
| 315 | ✗ | const std::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; | |
| 316 | ✗ | assert_pretty( | |
| 317 | it_m->first == it_d->first, | ||
| 318 | "it doesn't match the constraint name between model and data (" | ||
| 319 | << it_m->first << " != " << it_d->first << ")"); | ||
| 320 | |||
| 321 | ✗ | m_i->constraint->calcDiff(d_i, x); | |
| 322 | ✗ | const std::size_t ng = m_i->constraint->get_ng(); | |
| 323 | ✗ | const std::size_t nh = m_i->constraint->get_nh(); | |
| 324 | ✗ | data->Gx.block(ng_i, 0, ng, ndx) = d_i->Gx; | |
| 325 | ✗ | data->Hx.block(nh_i, 0, nh, ndx) = d_i->Hx; | |
| 326 | ✗ | ng_i += ng; | |
| 327 | ✗ | nh_i += nh; | |
| 328 | } | ||
| 329 | } | ||
| 330 | ✗ | } | |
| 331 | |||
| 332 | template <typename Scalar> | ||
| 333 | std::shared_ptr<ConstraintDataManagerTpl<Scalar> > | ||
| 334 | ✗ | ConstraintModelManagerTpl<Scalar>::createData( | |
| 335 | DataCollectorAbstract* const data) { | ||
| 336 | return std::allocate_shared<ConstraintDataManager>( | ||
| 337 | ✗ | Eigen::aligned_allocator<ConstraintDataManager>(), this, data); | |
| 338 | } | ||
| 339 | |||
| 340 | template <typename Scalar> | ||
| 341 | template <typename NewScalar> | ||
| 342 | ✗ | ConstraintModelManagerTpl<NewScalar> ConstraintModelManagerTpl<Scalar>::cast() | |
| 343 | const { | ||
| 344 | typedef ConstraintModelManagerTpl<NewScalar> ReturnType; | ||
| 345 | typedef ConstraintItemTpl<NewScalar> ConstraintType; | ||
| 346 | ✗ | ReturnType ret(state_->template cast<NewScalar>(), nu_); | |
| 347 | ✗ | typename ConstraintModelContainer::const_iterator it_m, end_m; | |
| 348 | ✗ | for (it_m = constraints_.begin(), end_m = constraints_.end(); it_m != end_m; | |
| 349 | ✗ | ++it_m) { | |
| 350 | ✗ | const std::string name = it_m->first; | |
| 351 | ✗ | const ConstraintType& m_i = it_m->second->template cast<NewScalar>(); | |
| 352 | ✗ | ret.addConstraint(name, m_i.constraint, m_i.active); | |
| 353 | } | ||
| 354 | ✗ | return ret; | |
| 355 | ✗ | } | |
| 356 | |||
| 357 | template <typename Scalar> | ||
| 358 | const std::shared_ptr<StateAbstractTpl<Scalar> >& | ||
| 359 | ✗ | ConstraintModelManagerTpl<Scalar>::get_state() const { | |
| 360 | ✗ | return state_; | |
| 361 | } | ||
| 362 | |||
| 363 | template <typename Scalar> | ||
| 364 | const typename ConstraintModelManagerTpl<Scalar>::ConstraintModelContainer& | ||
| 365 | ✗ | ConstraintModelManagerTpl<Scalar>::get_constraints() const { | |
| 366 | ✗ | return constraints_; | |
| 367 | } | ||
| 368 | |||
| 369 | template <typename Scalar> | ||
| 370 | ✗ | std::size_t ConstraintModelManagerTpl<Scalar>::get_nu() const { | |
| 371 | ✗ | return nu_; | |
| 372 | } | ||
| 373 | |||
| 374 | template <typename Scalar> | ||
| 375 | ✗ | std::size_t ConstraintModelManagerTpl<Scalar>::get_ng() const { | |
| 376 | ✗ | return ng_; | |
| 377 | } | ||
| 378 | |||
| 379 | template <typename Scalar> | ||
| 380 | ✗ | std::size_t ConstraintModelManagerTpl<Scalar>::get_nh() const { | |
| 381 | ✗ | return nh_; | |
| 382 | } | ||
| 383 | |||
| 384 | template <typename Scalar> | ||
| 385 | ✗ | std::size_t ConstraintModelManagerTpl<Scalar>::get_ng_T() const { | |
| 386 | ✗ | return ng_T_; | |
| 387 | } | ||
| 388 | |||
| 389 | template <typename Scalar> | ||
| 390 | ✗ | std::size_t ConstraintModelManagerTpl<Scalar>::get_nh_T() const { | |
| 391 | ✗ | return nh_T_; | |
| 392 | } | ||
| 393 | |||
| 394 | template <typename Scalar> | ||
| 395 | ✗ | const std::set<std::string>& ConstraintModelManagerTpl<Scalar>::get_active_set() | |
| 396 | const { | ||
| 397 | ✗ | return active_set_; | |
| 398 | } | ||
| 399 | |||
| 400 | template <typename Scalar> | ||
| 401 | const std::set<std::string>& | ||
| 402 | ✗ | ConstraintModelManagerTpl<Scalar>::get_inactive_set() const { | |
| 403 | ✗ | return inactive_set_; | |
| 404 | } | ||
| 405 | |||
| 406 | template <typename Scalar> | ||
| 407 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 408 | ✗ | ConstraintModelManagerTpl<Scalar>::get_lb() const { | |
| 409 | ✗ | return lb_; | |
| 410 | } | ||
| 411 | |||
| 412 | template <typename Scalar> | ||
| 413 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 414 | ✗ | ConstraintModelManagerTpl<Scalar>::get_ub() const { | |
| 415 | ✗ | return ub_; | |
| 416 | } | ||
| 417 | |||
| 418 | template <typename Scalar> | ||
| 419 | ✗ | bool ConstraintModelManagerTpl<Scalar>::getConstraintStatus( | |
| 420 | const std::string& name) const { | ||
| 421 | typename ConstraintModelContainer::const_iterator it = | ||
| 422 | ✗ | constraints_.find(name); | |
| 423 | ✗ | if (it != constraints_.end()) { | |
| 424 | ✗ | return it->second->active; | |
| 425 | } else { | ||
| 426 | std::cout << "Warning: we couldn't get the status of the " << name | ||
| 427 | ✗ | << " constraint item, it doesn't exist." << std::endl; | |
| 428 | ✗ | return false; | |
| 429 | } | ||
| 430 | } | ||
| 431 | |||
| 432 | template <typename Scalar> | ||
| 433 | ✗ | std::ostream& operator<<(std::ostream& os, | |
| 434 | const ConstraintModelManagerTpl<Scalar>& model) { | ||
| 435 | ✗ | const std::set<std::string>& active = model.get_active_set(); | |
| 436 | ✗ | const std::set<std::string>& inactive = model.get_inactive_set(); | |
| 437 | ✗ | os << "ConstraintModelManagerTpl:" << std::endl; | |
| 438 | ✗ | os << " Active:" << std::endl; | |
| 439 | ✗ | for (std::set<std::string>::const_iterator it = active.begin(); | |
| 440 | ✗ | it != active.end(); ++it) { | |
| 441 | const std::shared_ptr< | ||
| 442 | typename ConstraintModelManagerTpl<Scalar>::ConstraintItem>& | ||
| 443 | ✗ | constraint_item = model.get_constraints().find(*it)->second; | |
| 444 | ✗ | if (it != --active.end()) { | |
| 445 | ✗ | os << " " << *it << ": " << *constraint_item << std::endl; | |
| 446 | } else { | ||
| 447 | ✗ | os << " " << *it << ": " << *constraint_item << std::endl; | |
| 448 | } | ||
| 449 | } | ||
| 450 | ✗ | os << " Inactive:" << std::endl; | |
| 451 | ✗ | for (std::set<std::string>::const_iterator it = inactive.begin(); | |
| 452 | ✗ | it != inactive.end(); ++it) { | |
| 453 | const std::shared_ptr< | ||
| 454 | typename ConstraintModelManagerTpl<Scalar>::ConstraintItem>& | ||
| 455 | ✗ | constraint_item = model.get_constraints().find(*it)->second; | |
| 456 | ✗ | if (it != --inactive.end()) { | |
| 457 | ✗ | os << " " << *it << ": " << *constraint_item << std::endl; | |
| 458 | } else { | ||
| 459 | ✗ | os << " " << *it << ": " << *constraint_item; | |
| 460 | } | ||
| 461 | } | ||
| 462 | ✗ | return os; | |
| 463 | } | ||
| 464 | |||
| 465 | } // namespace crocoddyl | ||
| 466 |