crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
smooth-1norm.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
10#define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
11
12#include <iostream>
13#include <stdexcept>
14
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/activation-base.hpp"
17#include "crocoddyl/core/utils/exception.hpp"
18
19namespace crocoddyl {
20
33template <typename _Scalar>
35 public:
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37
38 typedef _Scalar Scalar;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
45
54 explicit ActivationModelSmooth1NormTpl(const std::size_t nr, const Scalar eps = Scalar(1.)) : Base(nr), eps_(eps) {
55 if (eps < Scalar(0.)) {
56 throw_pretty("Invalid argument: "
57 << "eps should be a positive value");
58 }
59 if (eps == Scalar(0.)) {
60 std::cerr
61 << "Warning: eps=0 leads to derivatives discontinuities in the origin, it becomes the absolute function"
62 << std::endl;
63 }
64 };
66
73 virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
74 if (static_cast<std::size_t>(r.size()) != nr_) {
75 throw_pretty("Invalid argument: "
76 << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
77 }
78 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
79
80 d->a = (r.array().cwiseAbs2().array() + eps_).array().cwiseSqrt();
81 data->a_value = d->a.sum();
82 };
83
90 virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
91 if (static_cast<std::size_t>(r.size()) != nr_) {
92 throw_pretty("Invalid argument: "
93 << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
94 }
95
96 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
97 data->Ar = r.cwiseProduct(d->a.cwiseInverse());
98 data->Arr.diagonal() = d->a.cwiseProduct(d->a).cwiseProduct(d->a).cwiseInverse();
99 };
100
106 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
107 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
108 };
109
115 virtual void print(std::ostream& os) const {
116 os << "ActivationModelSmooth1Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
117 }
118
119 protected:
120 using Base::nr_;
121 Scalar eps_;
122};
123
124template <typename _Scalar>
126 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
127
128 typedef _Scalar Scalar;
131 typedef typename MathBase::VectorXs VectorXs;
132 typedef typename MathBase::MatrixXs MatrixXs;
133
134 template <typename Activation>
135 explicit ActivationDataSmooth1NormTpl(Activation* const activation)
136 : Base(activation), a(VectorXs::Zero(activation->get_nr())) {}
137
138 VectorXs a;
139 using Base::Arr;
140};
141
142} // namespace crocoddyl
143
144#endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
virtual boost::shared_ptr< ActivationDataAbstract > createData()
Create the smooth-abs activation data.
virtual void calc(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the smooth-abs 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-abs function.
ActivationModelSmooth1NormTpl(const std::size_t nr, const Scalar eps=Scalar(1.))
Initialize the smooth-abs activation model.