diff-action-base.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, The University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 
11 namespace crocoddyl {
12 
13 template <typename Scalar>
14 DifferentialActionModelAbstractTpl<Scalar>::DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state,
15  const std::size_t& nu,
16  const std::size_t& nr)
17  : nu_(nu),
18  nr_(nr),
19  state_(state),
20  unone_(VectorXs::Zero(nu)),
21  u_lb_(VectorXs::Constant(nu, -std::numeric_limits<Scalar>::infinity())),
22  u_ub_(VectorXs::Constant(nu, std::numeric_limits<Scalar>::infinity())),
23  has_control_limits_(false) {}
24 
25 template <typename Scalar>
26 DifferentialActionModelAbstractTpl<Scalar>::~DifferentialActionModelAbstractTpl() {}
27 
28 template <typename Scalar>
29 void DifferentialActionModelAbstractTpl<Scalar>::calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
30  const Eigen::Ref<const VectorXs>& x) {
31  calc(data, x, unone_);
32 }
33 
34 template <typename Scalar>
35 void DifferentialActionModelAbstractTpl<Scalar>::calcDiff(
36  const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) {
37  calcDiff(data, x, unone_);
38 }
39 
40 template <typename Scalar>
41 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
42 DifferentialActionModelAbstractTpl<Scalar>::createData() {
43  return boost::make_shared<DifferentialActionDataAbstract>(this);
44 }
45 
46 template <typename Scalar>
47 const std::size_t& DifferentialActionModelAbstractTpl<Scalar>::get_nu() const {
48  return nu_;
49 }
50 
51 template <typename Scalar>
52 const std::size_t& DifferentialActionModelAbstractTpl<Scalar>::get_nr() const {
53  return nr_;
54 }
55 
56 template <typename Scalar>
57 const boost::shared_ptr<StateAbstractTpl<Scalar> >& DifferentialActionModelAbstractTpl<Scalar>::get_state() const {
58  return state_;
59 }
60 
61 template <typename Scalar>
62 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelAbstractTpl<Scalar>::get_u_lb() const {
63  return u_lb_;
64 }
65 
66 template <typename Scalar>
67 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelAbstractTpl<Scalar>::get_u_ub() const {
68  return u_ub_;
69 }
70 
71 template <typename Scalar>
72 bool const& DifferentialActionModelAbstractTpl<Scalar>::get_has_control_limits() const {
73  return has_control_limits_;
74 }
75 
76 template <typename Scalar>
77 void DifferentialActionModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
78  if (static_cast<std::size_t>(u_lb.size()) != nu_) {
79  throw_pretty("Invalid argument: "
80  << "lower bound has wrong dimension (it should be " + std::to_string(nu_) + ")");
81  }
82  u_lb_ = u_lb;
83  update_has_control_limits();
84 }
85 
86 template <typename Scalar>
87 void DifferentialActionModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
88  if (static_cast<std::size_t>(u_ub.size()) != nu_) {
89  throw_pretty("Invalid argument: "
90  << "upper bound has wrong dimension (it should be " + std::to_string(nu_) + ")");
91  }
92  u_ub_ = u_ub;
93  update_has_control_limits();
94 }
95 
96 template <typename Scalar>
97 void DifferentialActionModelAbstractTpl<Scalar>::update_has_control_limits() {
98  has_control_limits_ = isfinite(u_lb_.array()).any() && isfinite(u_ub_.array()).any();
99 }
100 
101 } // namespace crocoddyl
Definition: action-base.hxx:11