9 #ifndef CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_
10 #define CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 #include "crocoddyl/core/actuation/squashing-base.hpp"
21 template <
typename _Scalar>
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
41 smooth_ = Scalar(0.1);
43 d_ = (u_ub_ - u_lb_) * smooth_;
44 a_ = d_.array() * d_.array();
49 virtual void calc(
const boost::shared_ptr<SquashingDataAbstract>& data,
const Eigen::Ref<const VectorXs>& s) {
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());
57 virtual void calcDiff(
const boost::shared_ptr<SquashingDataAbstract>& data,
const Eigen::Ref<const VectorXs>& s) {
58 data->du_ds.diagonal() =
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());
64 const Scalar get_smooth()
const {
return smooth_; };
65 void set_smooth(
const Scalar smooth) {
67 throw_pretty(
"Invalid argument: "
68 <<
"Smooth value has to be positive");
72 d_ = (u_ub_ - u_lb_) * smooth_;
73 a_ = d_.array() * d_.array();
79 const VectorXs& get_d()
const {
return d_; };
97 #endif // CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_