crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
squashing-base.hpp
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.
8
9#ifndef CROCODDYL_CORE_SQUASHING_BASE_HPP_
10#define CROCODDYL_CORE_SQUASHING_BASE_HPP_
11
12#include <stdexcept>
13#include <boost/shared_ptr.hpp>
14#include <boost/make_shared.hpp>
15
16#include "crocoddyl/core/fwd.hpp"
17#include "crocoddyl/core/mathbase.hpp"
18#include "crocoddyl/core/utils/exception.hpp"
19
20namespace crocoddyl {
21
22template <typename _Scalar>
24 public:
25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26
27 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31
32 SquashingModelAbstractTpl(const std::size_t ns) : ns_(ns) {
33 if (ns_ == 0) {
34 throw_pretty("Invalid argument: "
35 << "ns cannot be zero");
36 }
37 };
39
40 virtual void calc(const boost::shared_ptr<SquashingDataAbstract>& data, const Eigen::Ref<const VectorXs>& s) = 0;
41 virtual void calcDiff(const boost::shared_ptr<SquashingDataAbstract>& data, const Eigen::Ref<const VectorXs>& s) = 0;
42 virtual boost::shared_ptr<SquashingDataAbstract> createData() {
43 return boost::allocate_shared<SquashingDataAbstract>(Eigen::aligned_allocator<SquashingDataAbstract>(), this);
44 }
45
46 std::size_t get_ns() const { return ns_; };
47 const VectorXs& get_s_lb() const { return s_lb_; };
48 const VectorXs& get_s_ub() const { return s_ub_; };
49
50 void set_s_lb(const VectorXs& s_lb) { s_lb_ = s_lb; };
51 void set_s_ub(const VectorXs& s_ub) { s_ub_ = s_ub; };
52
53 protected:
54 std::size_t ns_;
55 VectorXs u_ub_; // Squashing function upper bound
56 VectorXs u_lb_; // Squashing function lower bound
57 VectorXs s_ub_; // Bound for the s variable (to apply using the Quadratic barrier)
58 VectorXs s_lb_; // Bound for the s variable (to apply using the Quadratic barrier)
59};
60
61template <typename _Scalar>
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
67 typedef typename MathBase::VectorXs VectorXs;
68 typedef typename MathBase::MatrixXs MatrixXs;
69
70 template <template <typename Scalar> class Model>
71 explicit SquashingDataAbstractTpl(Model<Scalar>* const model)
72 : u(model->get_ns()), du_ds(model->get_ns(), model->get_ns()) {
73 u.setZero();
74 du_ds.setZero();
75 }
76 virtual ~SquashingDataAbstractTpl() {}
77
78 VectorXs u;
79 MatrixXs du_ds;
80};
81
82} // namespace crocoddyl
83
84#endif // CROCODDYL_CORE_SQUASHING_BASE_HPP_