crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
smooth-2norm.hpp
1
2// 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/fwd.hpp"
15#include "crocoddyl/core/activation-base.hpp"
16#include "crocoddyl/core/utils/exception.hpp"
17
18namespace crocoddyl {
19
32template <typename _Scalar>
34 public:
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36
37 typedef _Scalar Scalar;
41 typedef typename MathBase::VectorXs VectorXs;
42 typedef typename MathBase::MatrixXs MatrixXs;
43
52 explicit ActivationModelSmooth2NormTpl(const std::size_t nr, const Scalar eps = Scalar(1.)) : Base(nr), eps_(eps) {
53 if (eps < Scalar(0.)) {
54 throw_pretty("Invalid argument: "
55 << "eps should be a positive value");
56 }
57 };
59
66 virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
67 if (static_cast<std::size_t>(r.size()) != nr_) {
68 throw_pretty("Invalid argument: "
69 << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
70 }
71 using std::sqrt;
72 data->a_value = sqrt(r.squaredNorm() + eps_);
73 };
74
81 virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
82 if (static_cast<std::size_t>(r.size()) != nr_) {
83 throw_pretty("Invalid argument: "
84 << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
85 }
86
87 data->Ar = r / data->a_value;
88 using std::pow;
89 data->Arr.diagonal().array() = Scalar(1) / pow(data->a_value, 3);
90 };
91
97 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
98 return boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(), this);
99 };
100
101 protected:
107 virtual void print(std::ostream& os) const {
108 os << "ActivationModelSmooth2Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
109 }
110
111 using Base::nr_;
112 Scalar eps_;
113};
114
115} // namespace crocoddyl
116
117#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.