crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
quadratic.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
10#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
11
12#include <stdexcept>
13#include "crocoddyl/core/fwd.hpp"
14#include "crocoddyl/core/activation-base.hpp"
15#include "crocoddyl/core/utils/exception.hpp"
16
17namespace crocoddyl {
18
19template <typename _Scalar>
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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(){};
33
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_) + ")");
38 }
39 data->a_value = (Scalar(0.5) * r.transpose() * r)[0];
40 };
41
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_) + ")");
46 }
47
48 data->Ar = r;
49 // The Hessian has constant values which were set in createData.
50 assert_pretty(MatrixXs(data->Arr).isApprox(MatrixXs::Identity(nr_, nr_)), "Arr has wrong value");
51 };
52
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();
57 return data;
58 };
59
65 virtual void print(std::ostream& os) const { os << "ActivationModelQuad {nr=" << nr_ << "}"; }
66
67 protected:
68 using Base::nr_;
69};
70
71} // namespace crocoddyl
72
73#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the quadratic model.
Definition: quadratic.hpp:65