| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/core/actuation/squashing-base.hpp |
| Date: | 2025-01-16 08:47:40 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 13 | 18 | 72.2% |
| Branches: | 9 | 28 | 32.1% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2020, University of Edinburgh, IRI: CSIC-UPC | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #ifndef CROCODDYL_CORE_SQUASHING_BASE_HPP_ | ||
| 10 | #define CROCODDYL_CORE_SQUASHING_BASE_HPP_ | ||
| 11 | |||
| 12 | #include <boost/make_shared.hpp> | ||
| 13 | #include <boost/shared_ptr.hpp> | ||
| 14 | #include <stdexcept> | ||
| 15 | |||
| 16 | #include "crocoddyl/core/fwd.hpp" | ||
| 17 | #include "crocoddyl/core/mathbase.hpp" | ||
| 18 | #include "crocoddyl/core/utils/exception.hpp" | ||
| 19 | |||
| 20 | namespace crocoddyl { | ||
| 21 | |||
| 22 | template <typename _Scalar> | ||
| 23 | class SquashingModelAbstractTpl { | ||
| 24 | public: | ||
| 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 26 | |||
| 27 | typedef _Scalar Scalar; | ||
| 28 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 29 | typedef SquashingDataAbstractTpl<Scalar> SquashingDataAbstract; | ||
| 30 | typedef typename MathBase::VectorXs VectorXs; | ||
| 31 | |||
| 32 |
3/6✓ Branch 2 taken 283 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 283 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 283 times.
✗ Branch 9 not taken.
|
283 | SquashingModelAbstractTpl(const std::size_t ns) : ns_(ns) { |
| 33 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 283 times.
|
283 | if (ns_ == 0) { |
| 34 | ✗ | throw_pretty("Invalid argument: " << "ns cannot be zero"); | |
| 35 | } | ||
| 36 | 283 | }; | |
| 37 | 570 | virtual ~SquashingModelAbstractTpl() {}; | |
| 38 | |||
| 39 | virtual void calc(const boost::shared_ptr<SquashingDataAbstract>& data, | ||
| 40 | const Eigen::Ref<const VectorXs>& s) = 0; | ||
| 41 | virtual void calcDiff(const boost::shared_ptr<SquashingDataAbstract>& data, | ||
| 42 | const Eigen::Ref<const VectorXs>& s) = 0; | ||
| 43 | 8095 | virtual boost::shared_ptr<SquashingDataAbstract> createData() { | |
| 44 | return boost::allocate_shared<SquashingDataAbstract>( | ||
| 45 |
1/2✓ Branch 2 taken 8095 times.
✗ Branch 3 not taken.
|
16190 | Eigen::aligned_allocator<SquashingDataAbstract>(), this); |
| 46 | } | ||
| 47 | |||
| 48 | 24287 | std::size_t get_ns() const { return ns_; }; | |
| 49 | ✗ | const VectorXs& get_s_lb() const { return s_lb_; }; | |
| 50 | ✗ | const VectorXs& get_s_ub() const { return s_ub_; }; | |
| 51 | |||
| 52 | ✗ | void set_s_lb(const VectorXs& s_lb) { s_lb_ = s_lb; }; | |
| 53 | ✗ | void set_s_ub(const VectorXs& s_ub) { s_ub_ = s_ub; }; | |
| 54 | |||
| 55 | protected: | ||
| 56 | std::size_t ns_; | ||
| 57 | VectorXs u_ub_; // Squashing function upper bound | ||
| 58 | VectorXs u_lb_; // Squashing function lower bound | ||
| 59 | VectorXs | ||
| 60 | s_ub_; // Bound for the s variable (to apply using the Quadratic barrier) | ||
| 61 | VectorXs | ||
| 62 | s_lb_; // Bound for the s variable (to apply using the Quadratic barrier) | ||
| 63 | }; | ||
| 64 | |||
| 65 | template <typename _Scalar> | ||
| 66 | struct SquashingDataAbstractTpl { | ||
| 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 68 | |||
| 69 | typedef _Scalar Scalar; | ||
| 70 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 71 | typedef typename MathBase::VectorXs VectorXs; | ||
| 72 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 73 | |||
| 74 | template <template <typename Scalar> class Model> | ||
| 75 | 8095 | explicit SquashingDataAbstractTpl(Model<Scalar>* const model) | |
| 76 |
2/4✓ Branch 2 taken 8095 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 8095 times.
✗ Branch 8 not taken.
|
8095 | : u(model->get_ns()), du_ds(model->get_ns(), model->get_ns()) { |
| 77 |
1/2✓ Branch 1 taken 8095 times.
✗ Branch 2 not taken.
|
8095 | u.setZero(); |
| 78 |
1/2✓ Branch 1 taken 8095 times.
✗ Branch 2 not taken.
|
8095 | du_ds.setZero(); |
| 79 | 8095 | } | |
| 80 | 16194 | virtual ~SquashingDataAbstractTpl() {} | |
| 81 | |||
| 82 | VectorXs u; | ||
| 83 | MatrixXs du_ds; | ||
| 84 | }; | ||
| 85 | |||
| 86 | } // namespace crocoddyl | ||
| 87 | |||
| 88 | #endif // CROCODDYL_CORE_SQUASHING_BASE_HPP_ | ||
| 89 |