9 #ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/activation-base.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
19 template <
typename _Scalar>
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 typedef _Scalar Scalar;
28 typedef typename MathBase::VectorXs VectorXs;
29 typedef typename MathBase::MatrixXs MatrixXs;
34 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
35 if (
static_cast<std::size_t
>(r.size()) != nr_) {
36 throw_pretty(
"Invalid argument: "
37 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
39 data->a_value = (Scalar(0.5) * r.transpose() * r)[0];
42 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
43 if (
static_cast<std::size_t
>(r.size()) != nr_) {
44 throw_pretty(
"Invalid argument: "
45 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
50 assert_pretty(MatrixXs(data->Arr).isApprox(MatrixXs::Identity(nr_, nr_)),
"Arr has wrong value");
53 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
54 boost::shared_ptr<ActivationDataAbstract> data =
55 boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(),
this);
56 data->Arr.diagonal().setOnes();
65 virtual void print(std::ostream& os)
const { os <<
"ActivationModelQuad {nr=" << nr_ <<
"}"; }
73 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_