action-base.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, 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 ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(boost::shared_ptr<StateAbstractTpl<Scalar> > state,
15  const std::size_t& nu, const std::size_t& nr)
16  : nu_(nu),
17  nr_(nr),
18  state_(state),
19  unone_(MathBase::VectorXs::Zero(nu)),
20  u_lb_(MathBase::VectorXs::Constant(nu, -std::numeric_limits<Scalar>::infinity())),
21  u_ub_(MathBase::VectorXs::Constant(nu, std::numeric_limits<Scalar>::infinity())),
22  has_control_limits_(false) {}
23 
24 template <typename Scalar>
25 ActionModelAbstractTpl<Scalar>::~ActionModelAbstractTpl() {}
26 
27 template <typename Scalar>
28 void ActionModelAbstractTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
29  const Eigen::Ref<const VectorXs>& x) {
30  calc(data, x, unone_);
31 }
32 
33 template <typename Scalar>
34 void ActionModelAbstractTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
35  const Eigen::Ref<const VectorXs>& x) {
36  calcDiff(data, x, unone_);
37 }
38 
39 template <typename Scalar>
40 void ActionModelAbstractTpl<Scalar>::quasiStatic(const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
41  Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
42  const std::size_t& maxiter, const Scalar& tol) {
43  if (static_cast<std::size_t>(u.size()) != nu_) {
44  throw_pretty("Invalid argument: "
45  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
46  }
47  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
48  throw_pretty("Invalid argument: "
49  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
50  }
51 
52  const std::size_t& ndx = state_->get_ndx();
53  VectorXs dx = VectorXs::Zero(ndx);
54  if (nu_ == 0) {
55  // TODO(cmastalli): create a method for autonomous systems
56  } else {
57  VectorXs du = VectorXs::Zero(nu_);
58  for (std::size_t i = 0; i < maxiter; ++i) {
59  calc(data, x, u);
60  calcDiff(data, x, u);
61  state_->diff(x, data->xnext, dx);
62  du = -pseudoInverse(data->Fu) * data->Fx * dx;
63  u += du;
64  if (du.norm() <= tol) {
65  break;
66  }
67  }
68  }
69 }
70 
71 template <typename Scalar>
72 typename MathBaseTpl<Scalar>::VectorXs ActionModelAbstractTpl<Scalar>::quasiStatic_x(
73  const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x, const std::size_t& maxiter,
74  const Scalar& tol) {
75  VectorXs u(nu_);
76  u.setZero();
77  quasiStatic(data, u, x, maxiter, tol);
78  return u;
79 }
80 
81 template <typename Scalar>
82 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelAbstractTpl<Scalar>::createData() {
83  return boost::make_shared<ActionDataAbstractTpl<Scalar> >(this);
84 }
85 
86 template <typename Scalar>
87 const std::size_t& ActionModelAbstractTpl<Scalar>::get_nu() const {
88  return nu_;
89 }
90 
91 template <typename Scalar>
92 const std::size_t& ActionModelAbstractTpl<Scalar>::get_nr() const {
93  return nr_;
94 }
95 
96 template <typename Scalar>
97 const boost::shared_ptr<StateAbstractTpl<Scalar> >& ActionModelAbstractTpl<Scalar>::get_state() const {
98  return state_;
99 }
100 
101 template <typename Scalar>
102 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelAbstractTpl<Scalar>::get_u_lb() const {
103  return u_lb_;
104 }
105 
106 template <typename Scalar>
107 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelAbstractTpl<Scalar>::get_u_ub() const {
108  return u_ub_;
109 }
110 
111 template <typename Scalar>
112 bool const& ActionModelAbstractTpl<Scalar>::get_has_control_limits() const {
113  return has_control_limits_;
114 }
115 
116 template <typename Scalar>
117 void ActionModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
118  if (static_cast<std::size_t>(u_lb.size()) != nu_) {
119  throw_pretty("Invalid argument: "
120  << "lower bound has wrong dimension (it should be " + std::to_string(nu_) + ")");
121  }
122  u_lb_ = u_lb;
123  update_has_control_limits();
124 }
125 
126 template <typename Scalar>
127 void ActionModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
128  if (static_cast<std::size_t>(u_ub.size()) != nu_) {
129  throw_pretty("Invalid argument: "
130  << "upper bound has wrong dimension (it should be " + std::to_string(nu_) + ")");
131  }
132  u_ub_ = u_ub;
133  update_has_control_limits();
134 }
135 
136 template <typename Scalar>
137 void ActionModelAbstractTpl<Scalar>::update_has_control_limits() {
138  has_control_limits_ = isfinite(u_lb_.array()).any() && isfinite(u_ub_.array()).any();
139 }
140 
141 } // namespace crocoddyl
Definition: action-base.hxx:11