crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
weighted-quadratic-barrier.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, University of Edinburgh, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
11 
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>
16 namespace crocoddyl {
17 
18 template <typename _Scalar>
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  explicit ActivationModelWeightedQuadraticBarrierTpl(const ActivationBounds& bounds, const VectorXs& weights)
33  : Base(bounds.lb.size()), bounds_(bounds), weights_(weights){};
35 
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_) + ")");
40  }
41  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
42 
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();
47  data->a_value =
48  Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
49  };
50 
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_) + ")");
55  }
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 
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.)));
65  }
66 
67  data->Arr.diagonal().array() *= weights_.array();
68  };
69 
70  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
71  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
72  };
73 
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()) + ")");
81  }
82  weights_ = weights;
83  };
84 
85  protected:
86  using Base::nr_;
87 
88  private:
89  ActivationBounds bounds_;
90  VectorXs weights_;
91 };
92 
93 } // namespace crocoddyl
94 
95 #endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_