1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021, LAAS-CNRS, Airbus, University of Edinburgh |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#ifndef CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_ |
10 |
|
|
#define CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_ |
11 |
|
|
|
12 |
|
|
#include <pinocchio/utils/static-if.hpp> |
13 |
|
|
#include <stdexcept> |
14 |
|
|
|
15 |
|
|
#include "crocoddyl/core/activation-base.hpp" |
16 |
|
|
#include "crocoddyl/core/fwd.hpp" |
17 |
|
|
#include "crocoddyl/core/utils/exception.hpp" |
18 |
|
|
|
19 |
|
|
namespace crocoddyl { |
20 |
|
|
|
21 |
|
|
/** |
22 |
|
|
* @brief 2-norm barrier activation |
23 |
|
|
* |
24 |
|
|
* This activation function describes a quadratic barrier of the 2-norm of a |
25 |
|
|
* residual vector, i.e., |
26 |
|
|
* \f[ |
27 |
|
|
* \Bigg\{\begin{aligned} |
28 |
|
|
* &\frac{1}{2} (d - \alpha)^2, &\textrm{if} \,\,\, d < \alpha \\ |
29 |
|
|
* &0, &\textrm{otherwise}, |
30 |
|
|
* \end{aligned} |
31 |
|
|
* \f] |
32 |
|
|
* where \f$d = \|r\|\f$ is the norm of the residual, \f$\alpha\f$ the threshold |
33 |
|
|
* distance from which the barrier is active, \f$nr\f$ is the dimension of the |
34 |
|
|
* residual vector. |
35 |
|
|
* |
36 |
|
|
* The computation of the function and it derivatives are carried out in |
37 |
|
|
* `calc()` and `calcDiff()`, respectively. |
38 |
|
|
* |
39 |
|
|
* \sa `ActivationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` |
40 |
|
|
*/ |
41 |
|
|
template <typename _Scalar> |
42 |
|
|
class ActivationModel2NormBarrierTpl |
43 |
|
|
: public ActivationModelAbstractTpl<_Scalar> { |
44 |
|
|
public: |
45 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
46 |
|
|
|
47 |
|
|
typedef _Scalar Scalar; |
48 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
49 |
|
|
typedef ActivationModelAbstractTpl<Scalar> Base; |
50 |
|
|
typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract; |
51 |
|
|
typedef ActivationData2NormBarrierTpl<Scalar> Data; |
52 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
53 |
|
|
|
54 |
|
|
/** |
55 |
|
|
* @brief Initialize the 2-norm barrier activation model |
56 |
|
|
* |
57 |
|
|
* The default `alpha` value is defined as 0.1. |
58 |
|
|
* |
59 |
|
|
* @param[in] nr Dimension of the residual vector |
60 |
|
|
* @param[in] alpha Threshold factor (default 0.1) |
61 |
|
|
* @param[in] true_hessian Boolean indicating whether to use the Gauss-Newton |
62 |
|
|
* approximation or true Hessian in computing the derivatives (default: false) |
63 |
|
|
*/ |
64 |
|
185 |
explicit ActivationModel2NormBarrierTpl(const std::size_t nr, |
65 |
|
|
const Scalar alpha = Scalar(0.1), |
66 |
|
|
const bool true_hessian = false) |
67 |
|
185 |
: Base(nr), alpha_(alpha), true_hessian_(true_hessian) { |
68 |
✗✓ |
185 |
if (alpha < Scalar(0.)) { |
69 |
|
|
throw_pretty("Invalid argument: " |
70 |
|
|
<< "alpha should be a positive value"); |
71 |
|
|
} |
72 |
|
185 |
}; |
73 |
|
374 |
virtual ~ActivationModel2NormBarrierTpl(){}; |
74 |
|
|
|
75 |
|
|
/** |
76 |
|
|
* @brief Compute the 2-norm barrier function |
77 |
|
|
* |
78 |
|
|
* @param[in] data 2-norm barrier activation data |
79 |
|
|
* @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$ |
80 |
|
|
*/ |
81 |
|
3795 |
virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, |
82 |
|
|
const Eigen::Ref<const VectorXs>& r) { |
83 |
✓✗✗✓
|
3795 |
if (static_cast<std::size_t>(r.size()) != nr_) { |
84 |
|
|
throw_pretty("Invalid argument: " |
85 |
|
|
<< "r has wrong dimension (it should be " + |
86 |
|
|
std::to_string(nr_) + ")"); |
87 |
|
|
} |
88 |
|
7590 |
boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data); |
89 |
|
|
|
90 |
✓✗ |
3795 |
d->d = r.norm(); |
91 |
✓✓ |
3795 |
if (d->d < alpha_) { |
92 |
|
158 |
data->a_value = Scalar(0.5) * (d->d - alpha_) * (d->d - alpha_); |
93 |
|
|
} else { |
94 |
|
3637 |
data->a_value = Scalar(0.0); |
95 |
|
|
} |
96 |
|
3795 |
}; |
97 |
|
|
|
98 |
|
|
/** |
99 |
|
|
* @brief Compute the derivatives of the 2norm-barrier function |
100 |
|
|
* |
101 |
|
|
* @param[in] data 2-norm barrier activation data |
102 |
|
|
* @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$ |
103 |
|
|
*/ |
104 |
|
212 |
virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, |
105 |
|
|
const Eigen::Ref<const VectorXs>& r) { |
106 |
✓✗✗✓
|
212 |
if (static_cast<std::size_t>(r.size()) != nr_) { |
107 |
|
|
throw_pretty("Invalid argument: " |
108 |
|
|
<< "r has wrong dimension (it should be " + |
109 |
|
|
std::to_string(nr_) + ")"); |
110 |
|
|
} |
111 |
|
424 |
boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data); |
112 |
|
|
|
113 |
✓✓ |
212 |
if (d->d < alpha_) { |
114 |
✓✗✓✗
|
6 |
data->Ar = (d->d - alpha_) / d->d * r; |
115 |
✗✓ |
6 |
if (true_hessian_) { |
116 |
|
|
data->Arr.diagonal() = |
117 |
|
|
alpha_ * r.array().square() / std::pow(d->d, 3); // True Hessian |
118 |
|
|
data->Arr.diagonal().array() += (d->d - alpha_) / d->d; |
119 |
|
|
} else { |
120 |
✓✗✓✗ ✓✗ |
6 |
data->Arr.diagonal() = |
121 |
✓✗ |
12 |
r.array().square() / std::pow(d->d, 2); // GN Hessian approximation |
122 |
|
|
} |
123 |
|
|
} else { |
124 |
✓✗ |
206 |
data->Ar.setZero(); |
125 |
✓✗ |
206 |
data->Arr.setZero(); |
126 |
|
|
} |
127 |
|
212 |
}; |
128 |
|
|
|
129 |
|
|
/** |
130 |
|
|
* @brief Create the 2norm-barrier activation data |
131 |
|
|
* |
132 |
|
|
* @return the activation data |
133 |
|
|
*/ |
134 |
|
4679 |
virtual boost::shared_ptr<ActivationDataAbstract> createData() { |
135 |
✓✗ |
4679 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); |
136 |
|
|
}; |
137 |
|
|
|
138 |
|
|
/** |
139 |
|
|
* @brief Get and set the threshold factor |
140 |
|
|
*/ |
141 |
|
|
const Scalar& get_alpha() const { return alpha_; }; |
142 |
|
|
void set_alpha(const Scalar& alpha) { alpha_ = alpha; }; |
143 |
|
|
|
144 |
|
|
/** |
145 |
|
|
* @brief Print relevant information of the 2-norm barrier model |
146 |
|
|
* |
147 |
|
|
* @param[out] os Output stream object |
148 |
|
|
*/ |
149 |
|
37 |
virtual void print(std::ostream& os) const { |
150 |
|
37 |
os << "ActivationModel2NormBarrier {nr=" << nr_ << ", alpha=" << alpha_ |
151 |
|
37 |
<< ", Hessian=" << true_hessian_ << "}"; |
152 |
|
37 |
} |
153 |
|
|
|
154 |
|
|
protected: |
155 |
|
|
using Base::nr_; //!< Dimension of the residual vector |
156 |
|
|
Scalar alpha_; //!< Threshold factor |
157 |
|
|
bool true_hessian_; //!< Use true Hessian in calcDiff if true, Gauss-Newton |
158 |
|
|
//!< approximation if false |
159 |
|
|
}; |
160 |
|
|
|
161 |
|
|
template <typename _Scalar> |
162 |
|
|
struct ActivationData2NormBarrierTpl |
163 |
|
|
: public ActivationDataAbstractTpl<_Scalar> { |
164 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
165 |
|
|
|
166 |
|
|
typedef _Scalar Scalar; |
167 |
|
|
typedef ActivationDataAbstractTpl<Scalar> Base; |
168 |
|
|
|
169 |
|
|
template <typename Activation> |
170 |
|
4679 |
explicit ActivationData2NormBarrierTpl(Activation* const activation) |
171 |
|
4679 |
: Base(activation), d(Scalar(0)) {} |
172 |
|
|
|
173 |
|
|
Scalar d; //!< Norm of the residual |
174 |
|
|
|
175 |
|
|
using Base::a_value; |
176 |
|
|
using Base::Ar; |
177 |
|
|
using Base::Arr; |
178 |
|
|
}; |
179 |
|
|
|
180 |
|
|
} // namespace crocoddyl |
181 |
|
|
|
182 |
|
|
#endif // CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_ |