9 #ifndef CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_
13 #include <pinocchio/utils/static-if.hpp>
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/activation-base.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
38 template <
typename _Scalar>
41 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 typedef _Scalar Scalar;
48 typedef typename MathBase::VectorXs VectorXs;
61 const bool true_hessian =
false)
63 if (alpha < Scalar(0.)) {
64 throw_pretty(
"Invalid argument: "
65 <<
"alpha should be a positive value");
76 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
77 if (
static_cast<std::size_t
>(r.size()) != nr_) {
78 throw_pretty(
"Invalid argument: "
79 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
81 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
85 data->a_value = Scalar(0.5) * (d->d -
alpha_) * (d->d -
alpha_);
87 data->a_value = Scalar(0.0);
97 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
98 if (
static_cast<std::size_t
>(r.size()) != nr_) {
99 throw_pretty(
"Invalid argument: "
100 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
102 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
105 data->Ar = (d->d -
alpha_) / d->d * r;
107 data->Arr.diagonal() =
alpha_ * r.array().square() / std::pow(d->d, 3);
108 data->Arr.diagonal().array() += (d->d -
alpha_) / d->d;
110 data->Arr.diagonal() = r.array().square() / std::pow(d->d, 2);
123 virtual boost::shared_ptr<ActivationDataAbstract>
createData() {
124 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
131 void set_alpha(
const Scalar& alpha) {
alpha_ = alpha; };
138 virtual void print(std::ostream& os)
const {
139 os <<
"ActivationModel2NormBarrier {nr=" << nr_ <<
", alpha=" <<
alpha_ <<
", hessian=" <<
true_hessian_ <<
"}";
148 template <
typename _Scalar>
150 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
152 typedef _Scalar Scalar;
155 template <
typename Activation>
167 #endif // CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_