Crocoddyl
 
Loading...
Searching...
No Matches
constraint-manager.hpp
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.
8
9#ifndef CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
10#define CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
11
12#include "crocoddyl/core/constraint-base.hpp"
13
14namespace crocoddyl {
15
16template <typename _Scalar>
18 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
19
20 typedef _Scalar Scalar;
22
24 ConstraintItemTpl(const std::string& name,
25 std::shared_ptr<ConstraintModelAbstract> constraint,
26 bool active = true)
27 : name(name), constraint(constraint), active(active) {}
28
29 template <typename NewScalar>
30 ConstraintItemTpl<NewScalar> cast() const {
31 typedef ConstraintItemTpl<NewScalar> ReturnType;
32 ReturnType ret(name, constraint->template cast<NewScalar>(), active);
33 return ret;
34 }
35
39 friend std::ostream& operator<<(std::ostream& os,
40 const ConstraintItemTpl<Scalar>& model) {
41 os << "{" << *model.constraint << "}";
42 return os;
43 }
44
45 std::string name;
46 std::shared_ptr<ConstraintModelAbstract> constraint;
47 bool active;
48};
49
68template <typename _Scalar>
70 public:
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72
73 typedef _Scalar Scalar;
81 typedef typename MathBase::VectorXs VectorXs;
82 typedef typename MathBase::MatrixXs MatrixXs;
83
84 typedef std::map<std::string, std::shared_ptr<ConstraintItem> >
85 ConstraintModelContainer;
86 typedef std::map<std::string, std::shared_ptr<ConstraintDataAbstract> >
87 ConstraintDataContainer;
88
95 ConstraintModelManagerTpl(std::shared_ptr<StateAbstract> state,
96 const std::size_t nu);
97
105 explicit ConstraintModelManagerTpl(std::shared_ptr<StateAbstract> state);
107
116 void addConstraint(const std::string& name,
117 std::shared_ptr<ConstraintModelAbstract> constraint,
118 const bool active = true);
119
125 void removeConstraint(const std::string& name);
126
134 void changeConstraintStatus(const std::string& name, bool active);
135
143 void calc(const std::shared_ptr<ConstraintDataManager>& data,
144 const Eigen::Ref<const VectorXs>& x,
145 const Eigen::Ref<const VectorXs>& u);
146
157 void calc(const std::shared_ptr<ConstraintDataManager>& data,
158 const Eigen::Ref<const VectorXs>& x);
159
167 void calcDiff(const std::shared_ptr<ConstraintDataManager>& data,
168 const Eigen::Ref<const VectorXs>& x,
169 const Eigen::Ref<const VectorXs>& u);
170
182 void calcDiff(const std::shared_ptr<ConstraintDataManager>& data,
183 const Eigen::Ref<const VectorXs>& x);
184
196 std::shared_ptr<ConstraintDataManager> createData(
197 DataCollectorAbstract* const data);
198
208 template <typename NewScalar>
210
214 const std::shared_ptr<StateAbstract>& get_state() const;
215
219 const ConstraintModelContainer& get_constraints() const;
220
224 std::size_t get_nu() const;
225
229 std::size_t get_ng() const;
230
234 std::size_t get_nh() const;
235
239 std::size_t get_ng_T() const;
240
244 std::size_t get_nh_T() const;
245
249 const std::set<std::string>& get_active_set() const;
250
254 const std::set<std::string>& get_inactive_set() const;
255
259 const VectorXs& get_lb() const;
260
264 const VectorXs& get_ub() const;
265
271 bool getConstraintStatus(const std::string& name) const;
272
276 template <class Scalar>
277 friend std::ostream& operator<<(
278 std::ostream& os, const ConstraintModelManagerTpl<Scalar>& model);
279
280 private:
281 std::shared_ptr<StateAbstract> state_;
282 ConstraintModelContainer constraints_;
283 VectorXs lb_;
284 VectorXs ub_;
285 std::size_t nu_;
286 std::size_t ng_;
287 std::size_t nh_;
288 std::size_t ng_T_;
289 std::size_t nh_T_;
290 std::set<std::string> active_set_;
291 std::set<std::string>
292 inactive_set_;
293};
294
295template <typename _Scalar>
297 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
298
299 typedef _Scalar Scalar;
303 typedef typename MathBase::VectorXs VectorXs;
304 typedef typename MathBase::MatrixXs MatrixXs;
305
306 template <template <typename Scalar> class Model>
307 ConstraintDataManagerTpl(Model<Scalar>* const model,
308 DataCollectorAbstract* const data)
309 : g_internal(model->get_ng()),
310 Gx_internal(model->get_ng(), model->get_state()->get_ndx()),
311 Gu_internal(model->get_ng(), model->get_nu()),
312 h_internal(model->get_nh()),
313 Hx_internal(model->get_nh(), model->get_state()->get_ndx()),
314 Hu_internal(model->get_nh(), model->get_nu()),
315 shared(data),
316 g(g_internal.data(), model->get_ng()),
317 Gx(Gx_internal.data(), model->get_ng(), model->get_state()->get_ndx()),
318 Gu(Gu_internal.data(), model->get_ng(), model->get_nu()),
319 h(h_internal.data(), model->get_nh()),
320 Hx(Hx_internal.data(), model->get_nh(), model->get_state()->get_ndx()),
321 Hu(Hu_internal.data(), model->get_nh(), model->get_nu()) {
322 g.setZero();
323 Gx.setZero();
324 Gu.setZero();
325 h.setZero();
326 Hx.setZero();
327 Hu.setZero();
328 for (typename ConstraintModelManagerTpl<
329 Scalar>::ConstraintModelContainer::const_iterator it =
330 model->get_constraints().begin();
331 it != model->get_constraints().end(); ++it) {
332 const std::shared_ptr<ConstraintItem>& item = it->second;
333 constraints.insert(
334 std::make_pair(item->name, item->constraint->createData(data)));
335 }
336 }
337
338 template <class ActionData>
339 void shareMemory(ActionData* const data) {
340 // Save memory by setting the internal variables with null dimension
341 g_internal.resize(0);
342 Gx_internal.resize(0, 0);
343 Gu_internal.resize(0, 0);
344 h_internal.resize(0);
345 Hx_internal.resize(0, 0);
346 Hu_internal.resize(0, 0);
347 // Share memory with the differential action data
348 new (&g) Eigen::Map<VectorXs>(data->g.data(), data->g.size());
349 new (&Gx)
350 Eigen::Map<MatrixXs>(data->Gx.data(), data->Gx.rows(), data->Gx.cols());
351 new (&Gu)
352 Eigen::Map<MatrixXs>(data->Gu.data(), data->Gu.rows(), data->Gu.cols());
353 new (&h) Eigen::Map<VectorXs>(data->h.data(), data->h.size());
354 new (&Hx)
355 Eigen::Map<MatrixXs>(data->Hx.data(), data->Hx.rows(), data->Hx.cols());
356 new (&Hu)
357 Eigen::Map<MatrixXs>(data->Hu.data(), data->Hu.rows(), data->Hu.cols());
358 }
359
360 template <class Model>
361 void resize(Model* const model, const bool running_node = true) {
362 const std::size_t ndx = model->get_state()->get_ndx();
363 const std::size_t nu = model->get_nu();
364 const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
365 const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
366 new (&g) Eigen::Map<VectorXs>(g_internal.data(), ng);
367 new (&Gx) Eigen::Map<MatrixXs>(Gx_internal.data(), ng, ndx);
368 new (&Gu) Eigen::Map<MatrixXs>(Gu_internal.data(), ng, nu);
369 new (&h) Eigen::Map<VectorXs>(h_internal.data(), nh);
370 new (&Hx) Eigen::Map<MatrixXs>(Hx_internal.data(), nh, ndx);
371 new (&Hu) Eigen::Map<MatrixXs>(Hu_internal.data(), nh, nu);
372 }
373
374 template <class ActionModel, class ActionData>
375 void resize(ActionModel* const model, ActionData* const data,
376 const bool running_node = true) {
377 const std::size_t ndx = model->get_state()->get_ndx();
378 const std::size_t nu = model->get_nu();
379 const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
380 const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
381 data->g.conservativeResize(ng);
382 data->Gx.conservativeResize(ng, ndx);
383 data->Gu.conservativeResize(ng, nu);
384 data->h.conservativeResize(nh);
385 data->Hx.conservativeResize(nh, ndx);
386 data->Hu.conservativeResize(nh, nu);
387 new (&g) Eigen::Map<VectorXs>(data->g.data(), ng);
388 new (&Gx) Eigen::Map<MatrixXs>(data->Gx.data(), ng, ndx);
389 new (&Gu) Eigen::Map<MatrixXs>(data->Gu.data(), ng, nu);
390 new (&h) Eigen::Map<VectorXs>(data->h.data(), nh);
391 new (&Hx) Eigen::Map<MatrixXs>(data->Hx.data(), nh, ndx);
392 new (&Hu) Eigen::Map<MatrixXs>(data->Hu.data(), nh, nu);
393 }
394
395 VectorXs get_g() const { return g; }
396 MatrixXs get_Gx() const { return Gx; }
397 MatrixXs get_Gu() const { return Gu; }
398 VectorXs get_h() const { return h; }
399 MatrixXs get_Hx() const { return Hx; }
400 MatrixXs get_Hu() const { return Hu; }
401
402 void set_g(const VectorXs& _g) {
403 if (g.size() != _g.size()) {
404 throw_pretty(
405 "Invalid argument: " << "g has wrong dimension (it should be " +
406 std::to_string(g.size()) + ")");
407 }
408 g = _g;
409 }
410 void set_Gx(const MatrixXs& _Gx) {
411 if (Gx.rows() != _Gx.rows() || Gx.cols() != _Gx.cols()) {
412 throw_pretty(
413 "Invalid argument: " << "Gx has wrong dimension (it should be " +
414 std::to_string(Gx.rows()) + ", " +
415 std::to_string(Gx.cols()) + ")");
416 }
417 Gx = _Gx;
418 }
419 void set_Gu(const MatrixXs& _Gu) {
420 if (Gu.rows() != _Gu.rows() || Gu.cols() != _Gu.cols()) {
421 throw_pretty(
422 "Invalid argument: " << "Gu has wrong dimension (it should be " +
423 std::to_string(Gu.rows()) + ", " +
424 std::to_string(Gu.cols()) + ")");
425 }
426 Gu = _Gu;
427 }
428 void set_h(const VectorXs& _h) {
429 if (h.size() != _h.size()) {
430 throw_pretty(
431 "Invalid argument: " << "h has wrong dimension (it should be " +
432 std::to_string(h.size()) + ")");
433 }
434 h = _h;
435 }
436 void set_Hx(const MatrixXs& _Hx) {
437 if (Hx.rows() != _Hx.rows() || Hx.cols() != _Hx.cols()) {
438 throw_pretty(
439 "Invalid argument: " << "Hx has wrong dimension (it should be " +
440 std::to_string(Hx.rows()) + ", " +
441 std::to_string(Hx.cols()) + ")");
442 }
443 Hx = _Hx;
444 }
445 void set_Hu(const MatrixXs& _Hu) {
446 if (Hu.rows() != _Hu.rows() || Hu.cols() != _Hu.cols()) {
447 throw_pretty(
448 "Invalid argument: " << "Hu has wrong dimension (it should be " +
449 std::to_string(Hu.rows()) + ", " +
450 std::to_string(Hu.cols()) + ")");
451 }
452 Hu = _Hu;
453 }
454
455 // Creates internal data in case we don't share it externally
456 VectorXs g_internal;
457 MatrixXs Gx_internal;
458 MatrixXs Gu_internal;
459 VectorXs h_internal;
460 MatrixXs Hx_internal;
461 MatrixXs Hu_internal;
462
463 typename ConstraintModelManagerTpl<Scalar>::ConstraintDataContainer
464 constraints;
465 DataCollectorAbstract* shared;
466 Eigen::Map<VectorXs> g;
467 Eigen::Map<MatrixXs> Gx;
468 Eigen::Map<MatrixXs> Gu;
469 Eigen::Map<VectorXs> h;
470 Eigen::Map<MatrixXs> Hx;
471 Eigen::Map<MatrixXs> Hu;
472};
473
474} // namespace crocoddyl
475
476/* --- Details -------------------------------------------------------------- */
477/* --- Details -------------------------------------------------------------- */
478/* --- Details -------------------------------------------------------------- */
479#include "crocoddyl/core/constraints/constraint-manager.hxx"
480
481CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintItemTpl)
482CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ConstraintModelManagerTpl)
483CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintDataManagerTpl)
484
485#endif // CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
Abstract class for constraint models.
Manage the individual constraint models.
ConstraintModelManagerTpl< NewScalar > cast() const
Cast the constraint-manager model to a different scalar type.
const VectorXs & get_ub() const
Return the state upper bound.
std::shared_ptr< ConstraintDataManager > createData(DataCollectorAbstract *const data)
Create the constraint data.
void removeConstraint(const std::string &name)
Remove a constraint item.
void changeConstraintStatus(const std::string &name, bool active)
Change the constraint status.
std::size_t get_nh_T() const
Return the number of active equality terminal constraints.
std::size_t get_ng_T() const
Return the number of active inequality terminal constraints.
std::size_t get_ng() const
Return the number of active inequality constraints.
const std::set< std::string > & get_active_set() const
Return the names of the set of active constraints.
const std::set< std::string > & get_inactive_set() const
Return the names of the set of inactive constraints.
const ConstraintModelContainer & get_constraints() const
Return the stack of constraint models.
const VectorXs & get_lb() const
Return the state lower bound.
std::size_t get_nh() const
Return the number of active equality constraints.
void calcDiff(const std::shared_ptr< ConstraintDataManager > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian of the total constraint.
void calc(const std::shared_ptr< ConstraintDataManager > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the total constraint value.
friend std::ostream & operator<<(std::ostream &os, const ConstraintModelManagerTpl< Scalar > &model)
Print information on the stack of constraints.
ConstraintModelManagerTpl(std::shared_ptr< StateAbstract > state, const std::size_t nu)
Initialize the constraint-manager model.
const std::shared_ptr< StateAbstract > & get_state() const
Return the state.
ConstraintModelManagerTpl(std::shared_ptr< StateAbstract > state)
Initialize the constraint-manager model.
void calcDiff(const std::shared_ptr< ConstraintDataManager > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian of the total constraint with respect to the state only.
void calc(const std::shared_ptr< ConstraintDataManager > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total constraint value for nodes that depends only on the state.
void addConstraint(const std::string &name, std::shared_ptr< ConstraintModelAbstract > constraint, const bool active=true)
Add a constraint item.
bool getConstraintStatus(const std::string &name) const
Return the status of a given constraint name.
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
friend std::ostream & operator<<(std::ostream &os, const ConstraintItemTpl< Scalar > &model)
Print information on the constraint item.