Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/activations/quadratic-flat-exp.hpp	Lines:	27	34	79.4 %
Date:	2024-02-13 11:12:33	Branches:	16	74	21.6 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_

#include "crocoddyl/core/activation-base.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

/*
 * @brief Quadratic-flat-exp activation
 *
 * This activation function describes a quadratic exponential activation
 * depending on the square norm of a residual vector, i.e. \f[ \begin{equation}
 * 1 - exp(\|\mathbf{r}\|^2 / \alpha) \end{equation} \f] where \f$\alpha\f$
 * defines the width of the quadratic basin, \f$r\f$ is the scalar residual,
 * \f$nr\f$ is the dimension of the residual vector. Far
 * away from zero, the quadFlat activation is nearly flat.
 *
 * The computation of the function and it derivatives are carried out in
 * `calc()` and `caldDiff()`, respectively.
 *
 * \sa `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class ActivationModelQuadFlatExpTpl
    : public ActivationModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActivationModelAbstractTpl<Scalar> Base;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef ActivationDataQuadFlatExpTpl<Scalar> Data;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /*
   * @brief Initialize the quadratic-flat-exp activation model
   *
   * The default `alpha` value is defined as 1.
   *
   * @param[in] nr     Dimension of the residual vector
   * @param[in] alpha  Width of quadratic basin (default: 1.)
   */

  explicit ActivationModelQuadFlatExpTpl(const std::size_t &nr,
                                         const Scalar &alpha = Scalar(1.))
      : Base(nr), alpha_(alpha) {
    if (alpha < Scalar(0.)) {
      throw_pretty("Invalid argument: "
                   << "alpha should be a positive value");
    }
  };
  virtual ~ActivationModelQuadFlatExpTpl(){};

  /*
   * @brief Compute the quadratic-flat-exp function
   *
   * @param[in] data  Quadratic-flat activation data
   * @param[in] r     Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
   */
  virtual void calc(const boost::shared_ptr<ActivationDataAbstract> &data,
                    const Eigen::Ref<const VectorXs> &r) {
    if (static_cast<std::size_t>(r.size()) != nr_) {
      throw_pretty("Invalid argument: "
                   << "r has wrong dimension (it should be " +
                          std::to_string(nr_) + ")");
    }
    boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);

    d->a0 = exp(-r.squaredNorm() / alpha_);
    data->a_value = Scalar(1.0) - d->a0;
  };

  /*
   * @brief Compute the derivatives of the quadratic-flat-exp function
   *
   * @param[in] data  Quadratic-flat activation data
   * @param[in] r     Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
   */
  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract> &data,
                        const Eigen::Ref<const VectorXs> &r) {
    if (static_cast<std::size_t>(r.size()) != nr_) {
      throw_pretty("Invalid argument: "
                   << "r has wrong dimension (it should be " +
                          std::to_string(nr_) + ")");
    }
    boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);

    d->a1 = Scalar(2.0) / alpha_ * d->a0;
    data->Ar = d->a1 * r;
    data->Arr.diagonal() = -Scalar(2.0) * d->a1 * r.array().square() / alpha_;
    data->Arr.diagonal().array() += d->a1;
  };

  /**
   * @brief Create the quadratic-flat-exp activation data
   *
   * @return the activation data
   */
  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
    boost::shared_ptr<Data> data =
        boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
    return data;
  };

  Scalar get_alpha() const { return alpha_; };
  void set_alpha(const Scalar alpha) { alpha_ = alpha; };

  /**
   * @brief Print relevant information of the quadratic flat-exp model
   *
   * @param[out] os  Output stream object
   */
  virtual void print(std::ostream &os) const {
    os << "ActivationModelQuadFlatExp {nr=" << nr_ << ", a=" << alpha_ << "}";
  }

 protected:
  using Base::nr_;  //!< Dimension of the residual vector

 private:
  Scalar alpha_;  //!< Width of quadratic basin
};

/*
 * @brief Data structure of the quadratic-flat-exp activation
 *
 * @param[in] a0  computed in calc to avoid recomputation
 * @param[in] a1  computed in calcDiff to avoid recomputation
 */
template <typename _Scalar>
struct ActivationDataQuadFlatExpTpl
    : public ActivationDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActivationDataAbstractTpl<Scalar> Base;

  template <typename Activation>
  explicit ActivationDataQuadFlatExpTpl(Activation *const activation)
      : Base(activation), a0(0), a1(0) {}

  Scalar a0;
  Scalar a1;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2020, LAAS-CNRS, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
10			#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
11
12			#include "crocoddyl/core/activation-base.hpp"
13			#include "crocoddyl/core/fwd.hpp"
14			#include "crocoddyl/core/utils/exception.hpp"
15
16			namespace crocoddyl {
17
18			/*
19			* @brief Quadratic-flat-exp activation
20			*
21			* This activation function describes a quadratic exponential activation
22			* depending on the square norm of a residual vector, i.e. \f[ \begin{equation}
23			* 1 - exp(\\|\mathbf{r}\\|^2 / \alpha) \end{equation} \f] where \f$\alpha\f$
24			* defines the width of the quadratic basin, \f$r\f$ is the scalar residual,
25			* \f$nr\f$ is the dimension of the residual vector. Far
26			* away from zero, the quadFlat activation is nearly flat.
27			*
28			* The computation of the function and it derivatives are carried out in
29			* `calc()` and `caldDiff()`, respectively.
30			*
31			* \sa `calc()`, `calcDiff()`, `createData()`
32			*/
33			template <typename _Scalar>
34			class ActivationModelQuadFlatExpTpl
35			: public ActivationModelAbstractTpl<_Scalar> {
36			public:
37			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38
39			typedef _Scalar Scalar;
40			typedef MathBaseTpl<Scalar> MathBase;
41			typedef ActivationModelAbstractTpl<Scalar> Base;
42			typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
43			typedef ActivationDataQuadFlatExpTpl<Scalar> Data;
44			typedef typename MathBase::VectorXs VectorXs;
45			typedef typename MathBase::MatrixXs MatrixXs;
46
47			/*
48			* @brief Initialize the quadratic-flat-exp activation model
49			*
50			* The default `alpha` value is defined as 1.
51			*
52			* @param[in] nr Dimension of the residual vector
53			* @param[in] alpha Width of quadratic basin (default: 1.)
54			*/
55
56		223	explicit ActivationModelQuadFlatExpTpl(const std::size_t &nr,
57			const Scalar &alpha = Scalar(1.))
58		223	: Base(nr), alpha_(alpha) {
59	✗✓	223	if (alpha < Scalar(0.)) {
60			throw_pretty("Invalid argument: "
61			<< "alpha should be a positive value");
62			}
63		223	};
64		450	virtual ~ActivationModelQuadFlatExpTpl(){};
65
66			/*
67			* @brief Compute the quadratic-flat-exp function
68			*
69			* @param[in] data Quadratic-flat activation data
70			* @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
71			*/
72		9675	virtual void calc(const boost::shared_ptr<ActivationDataAbstract> &data,
73			const Eigen::Ref<const VectorXs> &r) {
74	✓✗✗✓	9675	if (static_cast<std::size_t>(r.size()) != nr_) {
75			throw_pretty("Invalid argument: "
76			<< "r has wrong dimension (it should be " +
77			std::to_string(nr_) + ")");
78			}
79		9675	boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
80
81	✓✗	9675	d->a0 = exp(-r.squaredNorm() / alpha_);
82		9675	data->a_value = Scalar(1.0) - d->a0;
83		9675	};
84
85			/*
86			* @brief Compute the derivatives of the quadratic-flat-exp function
87			*
88			* @param[in] data Quadratic-flat activation data
89			* @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
90			*/
91		304	virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract> &data,
92			const Eigen::Ref<const VectorXs> &r) {
93	✓✗✗✓	304	if (static_cast<std::size_t>(r.size()) != nr_) {
94			throw_pretty("Invalid argument: "
95			<< "r has wrong dimension (it should be " +
96			std::to_string(nr_) + ")");
97			}
98		304	boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
99
100		304	d->a1 = Scalar(2.0) / alpha_ * d->a0;
101	✓✗✓✗	304	data->Ar = d->a1 * r;
102	✓✗✓✗ ✓✗✓✗ ✓✗	304	data->Arr.diagonal() = -Scalar(2.0) * d->a1 * r.array().square() / alpha_;
103	✓✗✓✗	304	data->Arr.diagonal().array() += d->a1;
104		304	};
105
106			/**
107			* @brief Create the quadratic-flat-exp activation data
108			*
109			* @return the activation data
110			*/
111		7837	virtual boost::shared_ptr<ActivationDataAbstract> createData() {
112	✓✗	7837	boost::shared_ptr<Data> data =
113			boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
114		15674	return data;
115			};
116
117			Scalar get_alpha() const { return alpha_; };
118			void set_alpha(const Scalar alpha) { alpha_ = alpha; };
119
120			/**
121			* @brief Print relevant information of the quadratic flat-exp model
122			*
123			* @param[out] os Output stream object
124			*/
125		37	virtual void print(std::ostream &os) const {
126		37	os << "ActivationModelQuadFlatExp {nr=" << nr_ << ", a=" << alpha_ << "}";
127		37	}
128
129			protected:
130			using Base::nr_; //!< Dimension of the residual vector
131
132			private:
133			Scalar alpha_; //!< Width of quadratic basin
134			};
135
136			/*
137			* @brief Data structure of the quadratic-flat-exp activation
138			*
139			* @param[in] a0 computed in calc to avoid recomputation
140			* @param[in] a1 computed in calcDiff to avoid recomputation
141			*/
142			template <typename _Scalar>
143			struct ActivationDataQuadFlatExpTpl
144			: public ActivationDataAbstractTpl<_Scalar> {
145			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146
147			typedef _Scalar Scalar;
148			typedef MathBaseTpl<Scalar> MathBase;
149			typedef ActivationDataAbstractTpl<Scalar> Base;
150
151			template <typename Activation>
152		7837	explicit ActivationDataQuadFlatExpTpl(Activation *const activation)
153		7837	: Base(activation), a0(0), a1(0) {}
154
155			Scalar a0;
156			Scalar a1;
157			};
158
159			} // namespace crocoddyl
160
161			#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_