GCC Code Coverage Report

Line	Branch	Exec	Source
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.
8			///////////////////////////////////////////////////////////////////////////////
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
18			namespace crocoddyl {
19
20			template <typename _Scalar>
21			class ActivationModelWeightedQuadraticBarrierTpl
22			: public ActivationModelAbstractTpl<_Scalar> {
23			public:
24			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25		✗	CROCODDYL_DERIVED_CAST(ActivationModelBase,
26			ActivationModelWeightedQuadraticBarrierTpl)
27
28			typedef _Scalar Scalar;
29			typedef MathBaseTpl<Scalar> MathBase;
30			typedef ActivationModelAbstractTpl<Scalar> Base;
31			typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
32			typedef ActivationDataQuadraticBarrierTpl<Scalar> Data;
33			typedef ActivationBoundsTpl<Scalar> ActivationBounds;
34			typedef typename MathBase::VectorXs VectorXs;
35			typedef typename MathBase::MatrixXs MatrixXs;
36
37		✗	explicit ActivationModelWeightedQuadraticBarrierTpl(
38			const ActivationBounds& bounds, const VectorXs& weights)
39		✗	: Base(bounds.lb.size()), bounds_(bounds), weights_(weights) {};
40		✗	virtual ~ActivationModelWeightedQuadraticBarrierTpl() = default;
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>
86		✗	ActivationModelWeightedQuadraticBarrierTpl<NewScalar> cast() const {
87			typedef ActivationModelWeightedQuadraticBarrierTpl<NewScalar> ReturnType;
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
105			/**
106			* @brief Print relevant information of the quadratic barrier model
107			*
108			* @param[out] os Output stream object
109			*/
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
124			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
125			crocoddyl::ActivationModelWeightedQuadraticBarrierTpl)
126
127			#endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
128