state-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 #include "crocoddyl/core/mathbase.hpp"
9 
10 namespace crocoddyl {
11 template <typename Scalar>
12 StateAbstractTpl<Scalar>::StateAbstractTpl(const std::size_t& nx, const std::size_t& ndx)
13  : nx_(nx),
14  ndx_(ndx),
15  lb_(VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
16  ub_(VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
17  has_limits_(false) {
18  nv_ = ndx / 2;
19  nq_ = nx_ - nv_;
20 }
21 
22 template <typename Scalar>
23 StateAbstractTpl<Scalar>::StateAbstractTpl()
24  : nx_(0),
25  ndx_(0),
26  lb_(MathBase::VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
27  ub_(MathBase::VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
28  has_limits_(false) {}
29 
30 template <typename Scalar>
31 StateAbstractTpl<Scalar>::~StateAbstractTpl() {}
32 
33 template <typename Scalar>
34 typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::diff_dx(const Eigen::Ref<const VectorXs>& x0,
35  const Eigen::Ref<const VectorXs>& x1) {
36  VectorXs dxout = VectorXs::Zero(ndx_);
37  diff(x0, x1, dxout);
38  return dxout;
39 }
40 
41 template <typename Scalar>
42 typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::integrate_x(const Eigen::Ref<const VectorXs>& x,
43  const Eigen::Ref<const VectorXs>& dx) {
44  VectorXs xout = VectorXs::Zero(nx_);
45  integrate(x, dx, xout);
46  return xout;
47 }
48 
49 template <typename Scalar>
50 std::vector<typename MathBaseTpl<Scalar>::MatrixXs> StateAbstractTpl<Scalar>::Jdiff_Js(
51  const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1, Jcomponent firstsecond) {
52  MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
53  std::vector<MatrixXs> Jacs;
54  Jdiff(x0, x1, Jfirst, Jsecond, firstsecond);
55  switch (firstsecond) {
56  case both:
57  Jacs.push_back(Jfirst);
58  Jacs.push_back(Jsecond);
59  break;
60  case first:
61  Jacs.push_back(Jfirst);
62  break;
63  case second:
64  Jacs.push_back(Jsecond);
65  break;
66  default:
67  Jacs.push_back(Jfirst);
68  Jacs.push_back(Jsecond);
69  break;
70  }
71  return Jacs;
72 }
73 
74 template <typename Scalar>
75 std::vector<typename MathBaseTpl<Scalar>::MatrixXs> StateAbstractTpl<Scalar>::Jintegrate_Js(
76  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx, Jcomponent firstsecond) {
77  MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
78  std::vector<MatrixXs> Jacs;
79  Jintegrate(x, dx, Jfirst, Jsecond, firstsecond);
80  switch (firstsecond) {
81  case both:
82  Jacs.push_back(Jfirst);
83  Jacs.push_back(Jsecond);
84  break;
85  case first:
86  Jacs.push_back(Jfirst);
87  break;
88  case second:
89  Jacs.push_back(Jsecond);
90  break;
91  default:
92  Jacs.push_back(Jfirst);
93  Jacs.push_back(Jsecond);
94  break;
95  }
96  return Jacs;
97 }
98 
99 template <typename Scalar>
100 const std::size_t& StateAbstractTpl<Scalar>::get_nx() const {
101  return nx_;
102 }
103 
104 template <typename Scalar>
105 const std::size_t& StateAbstractTpl<Scalar>::get_ndx() const {
106  return ndx_;
107 }
108 
109 template <typename Scalar>
110 const std::size_t& StateAbstractTpl<Scalar>::get_nq() const {
111  return nq_;
112 }
113 
114 template <typename Scalar>
115 const std::size_t& StateAbstractTpl<Scalar>::get_nv() const {
116  return nv_;
117 }
118 
119 template <typename Scalar>
120 const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_lb() const {
121  return lb_;
122 }
123 
124 template <typename Scalar>
125 const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_ub() const {
126  return ub_;
127 }
128 
129 template <typename Scalar>
130 bool const& StateAbstractTpl<Scalar>::get_has_limits() const {
131  return has_limits_;
132 }
133 
134 template <typename Scalar>
135 void StateAbstractTpl<Scalar>::set_lb(const VectorXs& lb) {
136  if (static_cast<std::size_t>(lb.size()) != nx_) {
137  throw_pretty("Invalid argument: "
138  << "lower bound has wrong dimension (it should be " + std::to_string(nx_) + ")");
139  }
140  lb_ = lb;
141  update_has_limits();
142 }
143 
144 template <typename Scalar>
145 void StateAbstractTpl<Scalar>::set_ub(const VectorXs& ub) {
146  if (static_cast<std::size_t>(ub.size()) != nx_) {
147  throw_pretty("Invalid argument: "
148  << "upper bound has wrong dimension (it should be " + std::to_string(nx_) + ")");
149  }
150  ub_ = ub;
151  update_has_limits();
152 }
153 
154 template <typename Scalar>
155 void StateAbstractTpl<Scalar>::update_has_limits() {
156  has_limits_ = isfinite(lb_.array()).any() && isfinite(ub_.array()).any();
157 }
158 
159 } // namespace crocoddyl
Definition: action-base.hxx:11