crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
quadratic.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2019, LAAS-CNRS
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 
17 namespace crocoddyl {
18 
19 template <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.diagonal().asDiagonal()).isApprox(MatrixXs::Identity(nr_, nr_)),
51  "Arr has wrong value");
52  };
53 
54  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
55  boost::shared_ptr<ActivationDataAbstract> data =
56  boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(), this);
57  data->Arr.diagonal().fill((Scalar)1.);
58  return data;
59  };
60 
61  protected:
62  using Base::nr_;
63 };
64 
65 } // namespace crocoddyl
66 
67 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_