crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
smooth-abs.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2019, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_
11 
12 #include <stdexcept>
13 
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/activation-base.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
22  public:
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
30  typedef typename MathBase::VectorXs VectorXs;
31  typedef typename MathBase::MatrixXs MatrixXs;
32 
33  explicit ActivationModelSmoothAbsTpl(const std::size_t& nr) : Base(nr){};
34  virtual ~ActivationModelSmoothAbsTpl(){};
35 
36  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
37  if (static_cast<std::size_t>(r.size()) != nr_) {
38  throw_pretty("Invalid argument: "
39  << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
40  }
41  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
42 
43  d->a = (r.array().cwiseAbs2().array() + Scalar(1)).array().cwiseSqrt();
44  data->a_value = d->a.sum();
45  };
46 
47  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
48  if (static_cast<std::size_t>(r.size()) != nr_) {
49  throw_pretty("Invalid argument: "
50  << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
51  }
52 
53  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
54  data->Ar = r.cwiseProduct(d->a.cwiseInverse());
55  data->Arr.diagonal() = d->a.cwiseProduct(d->a).cwiseProduct(d->a).cwiseInverse();
56  };
57 
58  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
59  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
60  };
61 
62  protected:
63  using Base::nr_;
64 };
65 
66 template <typename _Scalar>
68  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 
70  typedef _Scalar Scalar;
73  typedef typename MathBase::VectorXs VectorXs;
74  typedef typename MathBase::MatrixXs MatrixXs;
75 
76  template <typename Activation>
77  explicit ActivationDataSmoothAbsTpl(Activation* const activation)
78  : Base(activation), a(VectorXs::Zero(activation->get_nr())) {
79  Arr.diagonal().array() = Scalar(2);
80  }
81 
82  VectorXs a;
83  using Base::Arr;
84 };
85 
86 } // namespace crocoddyl
87 
88 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_