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" 33 template <
typename _Scalar>
34 class ActivationModel2NormBarrierTpl :
public ActivationModelAbstractTpl<_Scalar> {
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 typedef _Scalar Scalar;
39 typedef MathBaseTpl<Scalar> MathBase;
40 typedef ActivationModelAbstractTpl<Scalar> Base;
41 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
42 typedef ActivationData2NormBarrierTpl<Scalar> Data;
43 typedef typename MathBase::VectorXs VectorXs;
55 explicit ActivationModel2NormBarrierTpl(
const std::size_t nr,
const Scalar alpha = Scalar(0.1),
56 const bool true_hessian =
false)
57 : Base(nr), alpha_(alpha), true_hessian_(true_hessian) {
58 if (alpha < Scalar(0.)) {
59 throw_pretty(
"Invalid argument: " 60 <<
"alpha should be a positive value");
63 virtual ~ActivationModel2NormBarrierTpl(){};
71 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
72 if (static_cast<std::size_t>(r.size()) != nr_) {
73 throw_pretty(
"Invalid argument: " 74 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
76 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
80 data->a_value = Scalar(0.5) * (d->d - alpha_) * (d->d - alpha_);
82 data->a_value = Scalar(0.0);
92 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
93 if (static_cast<std::size_t>(r.size()) != nr_) {
94 throw_pretty(
"Invalid argument: " 95 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
97 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
100 data->Ar = (d->d - alpha_) / d->d * r;
102 data->Arr.diagonal() = alpha_ * r.array().square() / std::pow(d->d, 3);
103 data->Arr.diagonal().array() += (d->d - alpha_) / d->d;
105 data->Arr.diagonal() = r.array().square() / std::pow(d->d, 2);
118 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
119 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
126 const Scalar& get_alpha()
const {
return alpha_; };
127 void set_alpha(
const Scalar& alpha) { alpha_ = alpha; };
134 virtual void print(std::ostream& os)
const {
135 os <<
"ActivationModel2NormBarrier {nr=" << nr_ <<
", alpha=" << alpha_ <<
", hessian=" << true_hessian_ <<
"}";
144 template <
typename _Scalar>
145 struct ActivationData2NormBarrierTpl :
public ActivationDataAbstractTpl<_Scalar> {
146 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148 typedef _Scalar Scalar;
149 typedef ActivationDataAbstractTpl<Scalar> Base;
151 template <
typename Activation>
152 explicit ActivationData2NormBarrierTpl(Activation*
const activation) : Base(activation), d(Scalar(0)) {}
163 #endif // CROCODDYL_CORE_ACTIVATIONS_2NORM_BARRIER_HPP_