9 #ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
12 #include <pinocchio/utils/static-if.hpp>
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
15 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
19 template <
typename _Scalar>
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
34 :
Base(bounds.lb.size()), bounds_(bounds), weights_(weights){};
37 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
38 if (
static_cast<std::size_t
>(r.size()) != nr_) {
39 throw_pretty(
"Invalid argument: "
40 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
42 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
44 d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
45 d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
46 d->rlb_min_.array() *= weights_.array();
47 d->rub_max_.array() *= weights_.array();
49 Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
52 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
53 if (
static_cast<std::size_t
>(r.size()) != nr_) {
54 throw_pretty(
"Invalid argument: "
55 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
57 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
58 data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
59 data->Ar.array() *= weights_.array();
61 using pinocchio::internal::if_then_else;
62 for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
63 data->Arr.diagonal()[i] = if_then_else(
64 pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
65 if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i], Scalar(0.), Scalar(1.), Scalar(0.)));
68 data->Arr.diagonal().array() *= weights_.array();
71 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
72 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
76 const VectorXs& get_weights()
const {
return weights_; };
78 void set_weights(
const VectorXs& weights) {
79 if (weights.size() != weights_.size()) {
80 throw_pretty(
"Invalid argument: "
81 <<
"weight vector has wrong dimension (it should be " + std::to_string(weights_.size()) +
")");
91 virtual void print(std::ostream& os)
const { os <<
"ActivationModelWeightedQuadraticBarrier {nr=" << nr_ <<
"}"; }
103 #endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_