crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
quadratic-barrier.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_BARRIER_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_BARRIER_HPP_
11 
12 #include <stdexcept>
13 #include <math.h>
14 
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 #include "crocoddyl/core/activation-base.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
27  typedef typename MathBase::VectorXs VectorXs;
28  typedef typename MathBase::MatrixXs MatrixXs;
29 
30  ActivationBoundsTpl(const VectorXs& lower, const VectorXs& upper, const Scalar& b = (Scalar)1.)
31  : lb(lower), ub(upper), beta(b) {
32  if (lb.size() != ub.size()) {
33  throw_pretty("Invalid argument: "
34  << "The lower and upper bounds don't have the same dimension (lb,ub dimensions equal to " +
35  std::to_string(lb.size()) + "," + std::to_string(ub.size()) + ", respectively)");
36  }
37  if (beta < 0 || beta > 1.) {
38  throw_pretty("Invalid argument: "
39  << "The range of beta is between 0 and 1");
40  }
41  for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
42  if (std::isfinite(lb(i)) && std::isfinite(ub(i))) {
43  if (lb(i) - ub(i) > 0) {
44  throw_pretty("Invalid argument: "
45  << "The lower and upper bounds are badly defined; ub has to be bigger / equals to lb");
46  }
47  }
48  }
49 
50  if (beta >= 0 && beta <= 1.) {
51  VectorXs m = (Scalar)0.5 * (lower + upper);
52  VectorXs d = (Scalar)0.5 * (upper - lower);
53  lb = m - beta * d;
54  ub = m + beta * d;
55  } else {
56  beta = 1.;
57  }
58  }
59  ActivationBoundsTpl(const ActivationBoundsTpl& bounds) : lb(bounds.lb), ub(bounds.ub), beta(bounds.beta) {}
60  ActivationBoundsTpl() : beta(1.) {}
61 
62  VectorXs lb;
63  VectorXs ub;
64  Scalar beta;
65 };
66 
67 template <typename _Scalar>
69  public:
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 
72  typedef _Scalar Scalar;
78  typedef typename MathBase::VectorXs VectorXs;
79  typedef typename MathBase::MatrixXs MatrixXs;
80 
81  explicit ActivationModelQuadraticBarrierTpl(const ActivationBounds& bounds)
82  : Base(bounds.lb.size()), bounds_(bounds){};
84 
85  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
86  if (static_cast<std::size_t>(r.size()) != nr_) {
87  throw_pretty("Invalid argument: "
88  << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
89  }
90 
91  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
92 
93  d->rlb_min_ = (r - bounds_.lb).array().min(0.);
94  d->rub_max_ = (r - bounds_.ub).array().max(0.);
95  data->a_value =
96  Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
97  };
98 
99  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) {
100  if (static_cast<std::size_t>(r.size()) != nr_) {
101  throw_pretty("Invalid argument: "
102  << "r has wrong dimension (it should be " + std::to_string(nr_) + ")");
103  }
104 
105  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
106  data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
107  data->Arr.diagonal() =
108  (((r - bounds_.lb).array() <= 0.) + ((r - bounds_.ub).array() >= 0.)).matrix().template cast<Scalar>();
109  };
110 
111  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
112  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
113  };
114 
115  const ActivationBounds& get_bounds() const { return bounds_; };
116  void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; };
117 
118  protected:
119  using Base::nr_;
120 
121  private:
122  ActivationBounds bounds_;
123 };
124 
125 template <typename _Scalar>
127  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
128 
129  typedef _Scalar Scalar;
131  typedef typename MathBase::ArrayXs ArrayXs;
133 
134  template <typename Activation>
135  explicit ActivationDataQuadraticBarrierTpl(Activation* const activation)
136  : Base(activation), rlb_min_(activation->get_nr()), rub_max_(activation->get_nr()) {
137  rlb_min_.setZero();
138  rub_max_.setZero();
139  }
140 
141  ArrayXs rlb_min_;
142  ArrayXs rub_max_;
143  using Base::a_value;
144  using Base::Ar;
145  using Base::Arr;
146 };
147 
148 } // namespace crocoddyl
149 
150 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_BARRIER_HPP_