Crocoddyl
 
Loading...
Searching...
No Matches
squashing-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, University of Edinburgh, IRI: CSIC-UPC,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_SQUASHING_BASE_HPP_
11#define CROCODDYL_CORE_SQUASHING_BASE_HPP_
12
13#include "crocoddyl/core/fwd.hpp"
14
15namespace crocoddyl {
16
18 public:
19 virtual ~SquashingModelBase() = default;
20
22};
23
24template <typename _Scalar>
26 public:
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28
29 typedef _Scalar Scalar;
32 typedef typename MathBase::VectorXs VectorXs;
33
34 SquashingModelAbstractTpl(const std::size_t ns) : ns_(ns) {
35 if (ns_ == 0) {
36 throw_pretty("Invalid argument: " << "ns cannot be zero");
37 }
38 };
39 virtual ~SquashingModelAbstractTpl() = default;
40
41 virtual void calc(const std::shared_ptr<SquashingDataAbstract>& data,
42 const Eigen::Ref<const VectorXs>& s) = 0;
43 virtual void calcDiff(const std::shared_ptr<SquashingDataAbstract>& data,
44 const Eigen::Ref<const VectorXs>& s) = 0;
45 virtual std::shared_ptr<SquashingDataAbstract> createData() {
46 return std::allocate_shared<SquashingDataAbstract>(
47 Eigen::aligned_allocator<SquashingDataAbstract>(), this);
48 }
49
53 template <class Scalar>
54 friend std::ostream& operator<<(
55 std::ostream& os, const SquashingModelAbstractTpl<Scalar>& model);
56
62 virtual void print(std::ostream& os) const {
63 os << boost::core::demangle(typeid(*this).name());
64 }
65
66 std::size_t get_ns() const { return ns_; };
67 const VectorXs& get_s_lb() const { return s_lb_; };
68 const VectorXs& get_s_ub() const { return s_ub_; };
69
70 void set_s_lb(const VectorXs& s_lb) { s_lb_ = s_lb; };
71 void set_s_ub(const VectorXs& s_ub) { s_ub_ = s_ub; };
72
73 protected:
74 std::size_t ns_;
75 VectorXs u_ub_; // Squashing function upper bound
76 VectorXs u_lb_; // Squashing function lower bound
77 VectorXs
78 s_ub_; // Bound for the s variable (to apply using the Quadratic barrier)
79 VectorXs
80 s_lb_; // Bound for the s variable (to apply using the Quadratic barrier)
81 SquashingModelAbstractTpl() : ns_(0) {};
82};
83
84template <typename _Scalar>
86 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
87
88 typedef _Scalar Scalar;
90 typedef typename MathBase::VectorXs VectorXs;
91 typedef typename MathBase::MatrixXs MatrixXs;
92
93 template <template <typename Scalar> class Model>
94 explicit SquashingDataAbstractTpl(Model<Scalar>* const model)
95 : u(model->get_ns()), du_ds(model->get_ns(), model->get_ns()) {
96 u.setZero();
97 du_ds.setZero();
98 }
100 virtual ~SquashingDataAbstractTpl() = default;
101
102 VectorXs u;
103 MatrixXs du_ds;
104};
105
106template <class Scalar>
107std::ostream& operator<<(std::ostream& os,
109 model.print(os);
110 return os;
111}
112
113} // namespace crocoddyl
114
115CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SquashingModelAbstractTpl)
116CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::SquashingDataAbstractTpl)
117
118#endif // CROCODDYL_CORE_SQUASHING_BASE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the squashing model.
friend std::ostream & operator<<(std::ostream &os, const SquashingModelAbstractTpl< Scalar > &model)
Print information on the actuation model.