Crocoddyl
smooth-2norm.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
11 
12 #include <stdexcept>
13 
14 #include "crocoddyl/core/activation-base.hpp"
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 
18 namespace crocoddyl {
19 
34 template <typename _Scalar>
36  : public ActivationModelAbstractTpl<_Scalar> {
37  public:
38  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 
40  typedef _Scalar Scalar;
44  typedef typename MathBase::VectorXs VectorXs;
45  typedef typename MathBase::MatrixXs MatrixXs;
46 
55  explicit ActivationModelSmooth2NormTpl(const std::size_t nr,
56  const Scalar eps = Scalar(1.))
57  : Base(nr), eps_(eps) {
58  if (eps < Scalar(0.)) {
59  throw_pretty("Invalid argument: "
60  << "eps should be a positive value");
61  }
62  };
63  virtual ~ActivationModelSmooth2NormTpl(){};
64 
71  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data,
72  const Eigen::Ref<const VectorXs>& r) {
73  if (static_cast<std::size_t>(r.size()) != nr_) {
74  throw_pretty("Invalid argument: "
75  << "r has wrong dimension (it should be " +
76  std::to_string(nr_) + ")");
77  }
78  using std::sqrt;
79  data->a_value = sqrt(r.squaredNorm() + eps_);
80  };
81 
88  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
89  const Eigen::Ref<const VectorXs>& r) {
90  if (static_cast<std::size_t>(r.size()) != nr_) {
91  throw_pretty("Invalid argument: "
92  << "r has wrong dimension (it should be " +
93  std::to_string(nr_) + ")");
94  }
95 
96  data->Ar = r / data->a_value;
97  using std::pow;
98  data->Arr.diagonal().array() = Scalar(1) / pow(data->a_value, 3);
99  };
100 
106  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
107  return boost::allocate_shared<ActivationDataAbstract>(
108  Eigen::aligned_allocator<ActivationDataAbstract>(), this);
109  };
110 
111  protected:
117  virtual void print(std::ostream& os) const {
118  os << "ActivationModelSmooth2Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
119  }
120 
121  using Base::nr_;
122  Scalar eps_;
123 };
124 
125 } // namespace crocoddyl
126 
127 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
virtual boost::shared_ptr< ActivationDataAbstract > createData()
Create the smooth-2Norm activation data.
ActivationModelSmooth2NormTpl(const std::size_t nr, const Scalar eps=Scalar(1.))
Initialize the smooth-2Norm activation model.
virtual void calc(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the smooth-2Norm function.
virtual void print(std::ostream &os) const
Print relevant information of the smooth-1norm model.
Scalar eps_
< Dimension of the residual vector
virtual void calcDiff(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the derivatives of the smooth-2Norm function.