Crocoddyl
 
Loading...
Searching...
No Matches
quadratic.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
11#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
12
13#include "crocoddyl/core/activation-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 CROCODDYL_DERIVED_CAST(ActivationModelBase, ActivationModelQuadTpl)
23
24 typedef _Scalar Scalar;
28 typedef typename MathBase::VectorXs VectorXs;
29 typedef typename MathBase::MatrixXs MatrixXs;
30
31 explicit ActivationModelQuadTpl(const std::size_t nr) : Base(nr) {};
32 virtual ~ActivationModelQuadTpl() = default;
33
34 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
35 const Eigen::Ref<const VectorXs>& r) override {
36 if (static_cast<std::size_t>(r.size()) != nr_) {
37 throw_pretty(
38 "Invalid argument: " << "r has wrong dimension (it should be " +
39 std::to_string(nr_) + ")");
40 }
41 data->a_value = Scalar(0.5) * r.dot(r);
42 };
43
44 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
45 const Eigen::Ref<const VectorXs>& r) override {
46 if (static_cast<std::size_t>(r.size()) != nr_) {
47 throw_pretty(
48 "Invalid argument: " << "r has wrong dimension (it should be " +
49 std::to_string(nr_) + ")");
50 }
51
52 data->Ar = r;
53 // The Hessian has constant values which were set in createData.
54 assert_pretty(MatrixXs(data->Arr).isApprox(MatrixXs::Identity(nr_, nr_)),
55 "Arr has wrong value");
56 };
57
58 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
59 std::shared_ptr<ActivationDataAbstract> data =
60 std::allocate_shared<ActivationDataAbstract>(
61 Eigen::aligned_allocator<ActivationDataAbstract>(), this);
62 data->Arr.diagonal().setOnes();
63 return data;
64 };
65
66 template <typename NewScalar>
68 typedef ActivationModelQuadTpl<NewScalar> ReturnType;
69 ReturnType res(nr_);
70 return res;
71 }
72
78 virtual void print(std::ostream& os) const override {
79 os << "ActivationModelQuad {nr=" << nr_ << "}";
80 }
81
82 protected:
83 using Base::nr_;
84};
85
86} // namespace crocoddyl
87
88CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActivationModelQuadTpl)
89
90#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
virtual void print(std::ostream &os) const override
Print relevant information of the quadratic model.
Definition quadratic.hpp:78