crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
weighted-quadratic-barrier.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-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 <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"
16
17namespace crocoddyl {
18
19template <typename _Scalar>
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23
24 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
32
33 explicit ActivationModelWeightedQuadraticBarrierTpl(const ActivationBounds& bounds, const VectorXs& weights)
34 : Base(bounds.lb.size()), bounds_(bounds), weights_(weights){};
36
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_) + ")");
41 }
42 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
43
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();
48 data->a_value =
49 Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
50 };
51
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_) + ")");
56 }
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();
60
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.)));
66 }
67
68 data->Arr.diagonal().array() *= weights_.array();
69 };
70
71 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
72 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
73 };
74
75 const ActivationBounds& get_bounds() const { return bounds_; };
76 const VectorXs& get_weights() const { return weights_; };
77 void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; };
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()) + ")");
82 }
83 weights_ = weights;
84 };
85
91 virtual void print(std::ostream& os) const { os << "ActivationModelWeightedQuadraticBarrier {nr=" << nr_ << "}"; }
92
93 protected:
94 using Base::nr_;
95
96 private:
97 ActivationBounds bounds_;
98 VectorXs weights_;
99};
100
101} // namespace crocoddyl
102
103#endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the quadratic barrier model.