9 #ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_ 10 #define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_ 12 #include "crocoddyl/core/fwd.hpp" 13 #include "crocoddyl/core/utils/exception.hpp" 14 #include "crocoddyl/core/activations/quadratic-barrier.hpp" 15 #include <pinocchio/utils/static-if.hpp> 18 template <
typename _Scalar>
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 typedef _Scalar Scalar;
29 typedef typename MathBase::VectorXs VectorXs;
30 typedef typename MathBase::MatrixXs MatrixXs;
33 : Base(bounds.lb.size()), bounds_(bounds), weights_(weights){};
36 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
37 if (static_cast<std::size_t>(r.size()) != nr_) {
38 throw_pretty(
"Invalid argument: " 39 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
41 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
43 d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
44 d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
45 d->rlb_min_.array() *= weights_.array();
46 d->rub_max_.array() *= weights_.array();
48 Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
51 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
52 if (static_cast<std::size_t>(r.size()) != nr_) {
53 throw_pretty(
"Invalid argument: " 54 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
56 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
57 data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
58 data->Ar.array() *= weights_.array();
60 using pinocchio::internal::if_then_else;
61 for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
62 data->Arr.diagonal()[i] = if_then_else(
63 pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
64 if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i], Scalar(0.), Scalar(1.), Scalar(0.)));
67 data->Arr.diagonal().array() *= weights_.array();
70 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
71 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
74 const ActivationBounds& get_bounds()
const {
return bounds_; };
75 const VectorXs& get_weights()
const {
return weights_; };
76 void set_bounds(
const ActivationBounds& bounds) { bounds_ = bounds; };
77 void set_weights(
const VectorXs& weights) {
78 if (weights.size() != weights_.size()) {
79 throw_pretty(
"Invalid argument: " 80 <<
"weight vector has wrong dimension (it should be " + std::to_string(weights_.size()) +
")");
89 ActivationBounds bounds_;
95 #endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_