Crocoddyl
 
Loading...
Searching...
No Matches
weighted-quadratic-barrier.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
11#define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
12
13#include <pinocchio/utils/static-if.hpp>
14
15#include "crocoddyl/core/activations/quadratic-barrier.hpp"
16#include "crocoddyl/core/fwd.hpp"
17
18namespace crocoddyl {
19
20template <typename _Scalar>
22 : public ActivationModelAbstractTpl<_Scalar> {
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 CROCODDYL_DERIVED_CAST(ActivationModelBase,
27
28 typedef _Scalar Scalar;
34 typedef typename MathBase::VectorXs VectorXs;
35 typedef typename MathBase::MatrixXs MatrixXs;
36
38 const ActivationBounds& bounds, const VectorXs& weights)
39 : Base(bounds.lb.size()), bounds_(bounds), weights_(weights) {};
41
42 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
43 const Eigen::Ref<const VectorXs>& r) override {
44 if (static_cast<std::size_t>(r.size()) != nr_) {
45 throw_pretty(
46 "Invalid argument: " << "r has wrong dimension (it should be " +
47 std::to_string(nr_) + ")");
48 }
49 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
50
51 d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
52 d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
53 d->rlb_min_.array() *= weights_.array();
54 d->rub_max_.array() *= weights_.array();
55 data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() +
56 Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
57 };
58
59 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
60 const Eigen::Ref<const VectorXs>& r) override {
61 if (static_cast<std::size_t>(r.size()) != nr_) {
62 throw_pretty(
63 "Invalid argument: " << "r has wrong dimension (it should be " +
64 std::to_string(nr_) + ")");
65 }
66 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
67 data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
68 data->Ar.array() *= weights_.array();
69
70 using pinocchio::internal::if_then_else;
71 for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
72 data->Arr.diagonal()[i] = if_then_else(
73 pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
74 if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i],
75 Scalar(0.), Scalar(1.), Scalar(0.)));
76 }
77
78 data->Arr.diagonal().array() *= weights_.array();
79 };
80
81 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
82 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
83 };
84
85 template <typename NewScalar>
88 ReturnType res(bounds_.template cast<NewScalar>(),
89 weights_.template cast<NewScalar>());
90 return res;
91 }
92
93 const ActivationBounds& get_bounds() const { return bounds_; };
94 const VectorXs& get_weights() const { return weights_; };
95 void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; };
96 void set_weights(const VectorXs& weights) {
97 if (weights.size() != weights_.size()) {
98 throw_pretty("Invalid argument: "
99 << "weight vector has wrong dimension (it should be " +
100 std::to_string(weights_.size()) + ")");
101 }
102 weights_ = weights;
103 };
104
110 virtual void print(std::ostream& os) const override {
111 os << "ActivationModelWeightedQuadraticBarrier {nr=" << nr_ << "}";
112 }
113
114 protected:
115 using Base::nr_;
116
117 private:
118 ActivationBounds bounds_;
119 VectorXs weights_;
120};
121
122} // namespace crocoddyl
123
124CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
126
127#endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
virtual void print(std::ostream &os) const override
Print relevant information of the quadratic barrier model.