Crocoddyl
 
Loading...
Searching...
No Matches
2norm-barrier.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, LAAS-CNRS, Airbus, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_
11#define CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_
12
13#include "crocoddyl/core/activation-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
38template <typename _Scalar>
40 : public ActivationModelAbstractTpl<_Scalar> {
41 public:
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44
45 typedef _Scalar Scalar;
50 typedef typename MathBase::VectorXs VectorXs;
51
62 explicit ActivationModel2NormBarrierTpl(const std::size_t nr,
63 const Scalar alpha = Scalar(0.1),
64 const bool true_hessian = false)
65 : Base(nr), alpha_(alpha), true_hessian_(true_hessian) {
66 if (alpha < Scalar(0.)) {
67 throw_pretty("Invalid argument: " << "alpha should be a positive value");
68 }
69 };
70 virtual ~ActivationModel2NormBarrierTpl() = default;
71
78 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
79 const Eigen::Ref<const VectorXs>& r) override {
80 if (static_cast<std::size_t>(r.size()) != nr_) {
81 throw_pretty(
82 "Invalid argument: " << "r has wrong dimension (it should be " +
83 std::to_string(nr_) + ")");
84 }
85 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
86
87 d->d = r.norm();
88 if (d->d < alpha_) {
89 data->a_value = Scalar(0.5) * (d->d - alpha_) * (d->d - alpha_);
90 } else {
91 data->a_value = Scalar(0.0);
92 }
93 };
94
101 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
102 const Eigen::Ref<const VectorXs>& r) override {
103 if (static_cast<std::size_t>(r.size()) != nr_) {
104 throw_pretty(
105 "Invalid argument: " << "r has wrong dimension (it should be " +
106 std::to_string(nr_) + ")");
107 }
108 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
109
110 if (d->d < alpha_) {
111 data->Ar = (d->d - alpha_) / d->d * r;
112 if (true_hessian_) {
113 data->Arr.diagonal() =
114 alpha_ * r.array().square() / pow(d->d, Scalar(3)); // True Hessian
115 data->Arr.diagonal().array() += (d->d - alpha_) / d->d;
116 } else {
117 data->Arr.diagonal() =
118 r.array().square() /
119 pow(d->d, Scalar(2)); // GN Hessian approximation
120 }
121 } else {
122 data->Ar.setZero();
123 data->Arr.setZero();
124 }
125 };
126
132 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
133 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
134 };
135
136 template <typename NewScalar>
139 ReturnType res(nr_, scalar_cast<NewScalar>(alpha_), true_hessian_);
140 return res;
141 }
142
146 const Scalar& get_alpha() const { return alpha_; };
147 void set_alpha(const Scalar& alpha) { alpha_ = alpha; };
148
154 virtual void print(std::ostream& os) const override {
155 os << "ActivationModel2NormBarrier {nr=" << nr_ << ", alpha=" << alpha_
156 << ", Hessian=" << true_hessian_ << "}";
157 }
158
159 protected:
160 using Base::nr_;
161 Scalar alpha_;
164};
165
166template <typename _Scalar>
168 : public ActivationDataAbstractTpl<_Scalar> {
169 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
170
171 typedef _Scalar Scalar;
173 typedef typename MathBase::VectorXs VectorXs;
174 typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
176
177 template <typename Activation>
178 explicit ActivationData2NormBarrierTpl(Activation* const activation)
179 : Base(activation), d(Scalar(0)) {}
180 virtual ~ActivationData2NormBarrierTpl() = default;
181
182 Scalar d;
183
184 using Base::a_value;
185 using Base::Ar;
186 using Base::Arr;
187};
188
189} // namespace crocoddyl
190
191CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
193CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
195
196#endif // CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_
virtual void calcDiff(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r) override
Compute the derivatives of the 2norm-barrier function.
const Scalar & get_alpha() const
Get and set the threshold factor.
Scalar alpha_
< Dimension of the residual vector
ActivationModel2NormBarrierTpl(const std::size_t nr, const Scalar alpha=Scalar(0.1), const bool true_hessian=false)
Initialize the 2-norm barrier activation model.
virtual std::shared_ptr< ActivationDataAbstract > createData() override
Create the 2norm-barrier activation data.
virtual void calc(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r) override
Compute the 2-norm barrier function.
virtual void print(std::ostream &os) const override
Print relevant information of the 2-norm barrier model.