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" 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();
59 data->Arr.diagonal() =
60 (((r - bounds_.lb).array() <= Scalar(0.)) + ((r - bounds_.ub).array() >= 0.)).matrix().template cast<Scalar>();
61 data->Arr.diagonal().array() *= weights_.array();
64 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
65 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
68 const ActivationBounds& get_bounds()
const {
return bounds_; };
69 const VectorXs& get_weights()
const {
return weights_; };
70 void set_bounds(
const ActivationBounds& bounds) { bounds_ = bounds; };
71 void set_weights(
const VectorXs& weights) {
72 if (weights.size() != weights_.size()) {
73 throw_pretty(
"Invalid argument: " 74 <<
"weight vector has wrong dimension (it should be " + std::to_string(weights_.size()) +
")");
83 ActivationBounds bounds_;
89 #endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_