crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
smooth-sat.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_SMOOTH_SAT_HPP_
10#define CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_
11
12#include <stdexcept>
13#include <math.h>
14
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/utils/exception.hpp"
17#include "crocoddyl/core/actuation/squashing-base.hpp"
18
19namespace crocoddyl {
20
21template <typename _Scalar>
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25
26 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31
32 SquashingModelSmoothSatTpl(const Eigen::Ref<const VectorXs>& u_lb, const Eigen::Ref<const VectorXs>& u_ub,
33 const std::size_t ns)
34 : Base(ns) {
35 u_lb_ = u_lb;
36 u_ub_ = u_ub;
37
38 s_lb_ = u_lb_;
39 s_ub_ = u_ub_;
40
41 smooth_ = Scalar(0.1);
42
43 d_ = (u_ub_ - u_lb_) * smooth_;
44 a_ = d_.array() * d_.array();
45 }
46
48
49 virtual void calc(const boost::shared_ptr<SquashingDataAbstract>& data, const Eigen::Ref<const VectorXs>& s) {
50 // Squashing function used: "Smooth abs":
51 // s(u) = 0.5*(lb + ub + sqrt(smooth + (u - lb)^2) - sqrt(smooth + (u - ub)^2))
52 data->u =
53 Scalar(0.5) * (Eigen::sqrt(Eigen::pow((s - u_lb_).array(), 2) + a_.array()) -
54 Eigen::sqrt(Eigen::pow((s - u_ub_).array(), 2) + a_.array()) + u_lb_.array() + u_ub_.array());
55 }
56
57 virtual void calcDiff(const boost::shared_ptr<SquashingDataAbstract>& data, const Eigen::Ref<const VectorXs>& s) {
58 data->du_ds.diagonal() =
59 Scalar(0.5) *
60 (Eigen::pow(a_.array() + Eigen::pow((s - u_lb_).array(), 2), Scalar(-0.5)).array() * (s - u_lb_).array() -
61 Eigen::pow(a_.array() + Eigen::pow((s - u_ub_).array(), 2), Scalar(-0.5)).array() * (s - u_ub_).array());
62 }
63
64 const Scalar get_smooth() const { return smooth_; };
65 void set_smooth(const Scalar smooth) {
66 if (smooth < 0.) {
67 throw_pretty("Invalid argument: "
68 << "Smooth value has to be positive");
69 }
70 smooth_ = smooth;
71
72 d_ = (u_ub_ - u_lb_) * smooth_;
73 a_ = d_.array() * d_.array();
74
75 s_lb_ = u_lb_;
76 s_ub_ = u_ub_;
77 }
78
79 const VectorXs& get_d() const { return d_; };
80
81 private:
82 VectorXs a_;
83 VectorXs d_;
84
85 Scalar smooth_;
86
87 protected:
88 using Base::s_lb_;
89 using Base::s_ub_;
90
91 using Base::u_lb_;
92 using Base::u_ub_;
93};
94
95} // namespace crocoddyl
96
97#endif // CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_