Crocoddyl
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of
5 // Oxford Copyright note valid unless otherwise stated in individual files. All
6 // rights reserved.
8
11
12 #include <math.h>
13
14 #include <pinocchio/utils/static-if.hpp>
15 #include <stdexcept>
16
17 #include "crocoddyl/core/activation-base.hpp"
18 #include "crocoddyl/core/fwd.hpp"
19 #include "crocoddyl/core/utils/exception.hpp"
20
21 namespace crocoddyl {
22
23 template <typename _Scalar>
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26
27  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31
32  ActivationBoundsTpl(const VectorXs& lower, const VectorXs& upper,
33  const Scalar b = (Scalar)1.)
34  : lb(lower), ub(upper), beta(b) {
35  if (lb.size() != ub.size()) {
36  throw_pretty("Invalid argument: "
37  << "The lower and upper bounds don't have the same "
38  "dimension (lb,ub dimensions equal to " +
39  std::to_string(lb.size()) + "," +
40  std::to_string(ub.size()) + ", respectively)");
41  }
42  if (beta < Scalar(0) || beta > Scalar(1.)) {
43  throw_pretty("Invalid argument: "
44  << "The range of beta is between 0 and 1");
45  }
46  using std::isfinite;
47  for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
48  if (isfinite(lb(i)) && isfinite(ub(i))) {
49  if (lb(i) - ub(i) > 0) {
50  throw_pretty("Invalid argument: "
51  << "The lower and upper bounds are badly defined; ub "
52  "has to be bigger / equals to lb");
53  }
54  }
55  // Assign the maximum value for infinity/nan values
56  if (!isfinite(lb(i))) {
57  lb(i) = -std::numeric_limits<Scalar>::max();
58  }
59  if (!isfinite(ub(i))) {
60  ub(i) = std::numeric_limits<Scalar>::max();
61  }
62  }
63
64  if (beta >= Scalar(0) && beta <= Scalar(1.)) {
65  for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
66  // do not use beta when one of the bounds is inf
67  if (lb(i) != (-std::numeric_limits<Scalar>::max()) &&
68  ub(i) != (std::numeric_limits<Scalar>::max())) {
69  Scalar m = Scalar(0.5) * (lb(i) + ub(i));
70  Scalar d = Scalar(0.5) * (ub(i) - lb(i));
71  lb(i) = m - beta * d;
72  ub(i) = m + beta * d;
73  }
74  }
75  } else {
76  beta = Scalar(1.);
77  }
78  }
80  : lb(other.lb), ub(other.ub), beta(other.beta) {}
81  ActivationBoundsTpl() : beta(Scalar(1.)) {}
82
83  ActivationBoundsTpl& operator=(const ActivationBoundsTpl& other) {
84  if (this != &other) {
85  lb = other.lb;
86  ub = other.ub;
87  beta = other.beta;
88  }
89  return *this;
90  }
91
92  VectorXs lb;
93  VectorXs ub;
94  Scalar beta;
95 };
96
97 template <typename _Scalar>
99  : public ActivationModelAbstractTpl<_Scalar> {
100  public:
101  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102
103  typedef _Scalar Scalar;
109  typedef typename MathBase::VectorXs VectorXs;
110  typedef typename MathBase::MatrixXs MatrixXs;
111
113  : Base(bounds.lb.size()), bounds_(bounds) {};
115
116  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data,
117  const Eigen::Ref<const VectorXs>& r) {
118  if (static_cast<std::size_t>(r.size()) != nr_) {
119  throw_pretty("Invalid argument: "
120  << "r has wrong dimension (it should be " +
121  std::to_string(nr_) + ")");
122  }
123
124  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
125
126  d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
127  d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
128  data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() +
129  Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
130  };
131
132  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
133  const Eigen::Ref<const VectorXs>& r) {
134  if (static_cast<std::size_t>(r.size()) != nr_) {
135  throw_pretty("Invalid argument: "
136  << "r has wrong dimension (it should be " +
137  std::to_string(nr_) + ")");
138  }
139
140  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
141  data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
142
143  using pinocchio::internal::if_then_else;
144  for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
145  data->Arr.diagonal()[i] = if_then_else(
146  pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
147  if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i],
148  Scalar(0.), Scalar(1.), Scalar(0.)));
149  }
150  };
151
152  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
153  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
154  };
155
156  const ActivationBounds& get_bounds() const { return bounds_; };
157  void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; };
158
164  virtual void print(std::ostream& os) const {
165  os << "ActivationModelQuadraticBarrier {nr=" << nr_ << "}";
166  }
167
168  protected:
169  using Base::nr_;
170
171  private:
172  ActivationBounds bounds_;
173 };
174
175 template <typename _Scalar>
177  : public ActivationDataAbstractTpl<_Scalar> {
178  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
179
180  typedef _Scalar Scalar;
182  typedef typename MathBase::ArrayXs ArrayXs;
184
185  template <typename Activation>
187  : Base(activation),
188  rlb_min_(activation->get_nr()),
189  rub_max_(activation->get_nr()) {
190  rlb_min_.setZero();
191  rub_max_.setZero();
192  }
193
194  ArrayXs rlb_min_;
195  ArrayXs rub_max_;
196  using Base::a_value;
197  using Base::Ar;
198  using Base::Arr;
199 };
200
201 } // namespace crocoddyl
202