state.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh, New York University,
5 // Max Planck Gesellschaft
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #include "crocoddyl/core/utils/exception.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 StateNumDiffTpl<Scalar>::StateNumDiffTpl(boost::shared_ptr<Base> state)
16  : Base(state->get_nx(), state->get_ndx()), state_(state), disturbance_(1e-6) {}
17 
18 template <typename Scalar>
19 StateNumDiffTpl<Scalar>::~StateNumDiffTpl() {}
20 
21 template <typename Scalar>
22 typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::zero() const {
23  return state_->zero();
24 }
25 
26 template <typename Scalar>
27 typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::rand() const {
28  return state_->rand();
29 }
30 
31 template <typename Scalar>
32 void StateNumDiffTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
33  Eigen::Ref<VectorXs> dxout) const {
34  if (static_cast<std::size_t>(x0.size()) != nx_) {
35  throw_pretty("Invalid argument: "
36  << "x0 has wrong dimension (it should be " + std::to_string(nx_) + ")");
37  }
38  if (static_cast<std::size_t>(x1.size()) != nx_) {
39  throw_pretty("Invalid argument: "
40  << "x1 has wrong dimension (it should be " + std::to_string(nx_) + ")");
41  }
42  if (static_cast<std::size_t>(dxout.size()) != ndx_) {
43  throw_pretty("Invalid argument: "
44  << "dxout has wrong dimension (it should be " + std::to_string(ndx_) + ")");
45  }
46  state_->diff(x0, x1, dxout);
47 }
48 
49 template <typename Scalar>
50 void StateNumDiffTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
51  Eigen::Ref<VectorXs> xout) const {
52  if (static_cast<std::size_t>(x.size()) != nx_) {
53  throw_pretty("Invalid argument: "
54  << "x has wrong dimension (it should be " + std::to_string(nx_) + ")");
55  }
56  if (static_cast<std::size_t>(dx.size()) != ndx_) {
57  throw_pretty("Invalid argument: "
58  << "dx has wrong dimension (it should be " + std::to_string(ndx_) + ")");
59  }
60  if (static_cast<std::size_t>(xout.size()) != nx_) {
61  throw_pretty("Invalid argument: "
62  << "xout has wrong dimension (it should be " + std::to_string(nx_) + ")");
63  }
64  state_->integrate(x, dx, xout);
65 }
66 
67 template <typename Scalar>
68 void StateNumDiffTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
69  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
70  Jcomponent firstsecond) const {
71  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
72  if (static_cast<std::size_t>(x0.size()) != nx_) {
73  throw_pretty("Invalid argument: "
74  << "x0 has wrong dimension (it should be " + std::to_string(nx_) + ")");
75  }
76  if (static_cast<std::size_t>(x1.size()) != nx_) {
77  throw_pretty("Invalid argument: "
78  << "x1 has wrong dimension (it should be " + std::to_string(nx_) + ")");
79  }
80  VectorXs tmp_x_ = VectorXs::Zero(nx_);
81  VectorXs dx_ = VectorXs::Zero(ndx_);
82  VectorXs dx0_ = VectorXs::Zero(ndx_);
83 
84  dx_.setZero();
85  diff(x0, x1, dx0_);
86  if (firstsecond == first || firstsecond == both) {
87  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
88  throw_pretty("Invalid argument: "
89  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
90  ")");
91  }
92  Jfirst.setZero();
93  for (std::size_t i = 0; i < ndx_; ++i) {
94  dx_(i) = disturbance_;
95  // tmp_x = int(x0, dx)
96  integrate(x0, dx_, tmp_x_);
97  // Jfirst[:,k] = diff(tmp_x, x1) = diff(int(x0 + dx), x1)
98  diff(tmp_x_, x1, Jfirst.col(i));
99  // Jfirst[:,k] = Jfirst[:,k] - tmp_dx_, or
100  // Jfirst[:,k] = Jfirst[:,k] - diff(x0, x1)
101  Jfirst.col(i) -= dx0_;
102  dx_(i) = 0.0;
103  }
104  Jfirst /= disturbance_;
105  }
106  if (firstsecond == second || firstsecond == both) {
107  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
108  throw_pretty("Invalid argument: "
109  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
110  std::to_string(ndx_) + ")");
111  }
112 
113  Jsecond.setZero();
114  for (std::size_t i = 0; i < ndx_; ++i) {
115  dx_(i) = disturbance_;
116  // tmp_x = int(x1 + dx)
117  integrate(x1, dx_, tmp_x_);
118  // Jsecond[:,k] = diff(x0, tmp_x) = diff(x0, int(x1 + dx))
119  diff(x0, tmp_x_, Jsecond.col(i));
120  // Jsecond[:,k] = J[:,k] - tmp_dx_
121  // Jsecond[:,k] = Jsecond[:,k] - diff(x0, x1)
122  Jsecond.col(i) -= dx0_;
123  dx_(i) = 0.0;
124  }
125  Jsecond /= disturbance_;
126  }
127 }
128 
129 template <typename Scalar>
130 void StateNumDiffTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
131  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
132  Jcomponent firstsecond) const {
133  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
134  if (static_cast<std::size_t>(x.size()) != nx_) {
135  throw_pretty("Invalid argument: "
136  << "x has wrong dimension (it should be " + std::to_string(nx_) + ")");
137  }
138  if (static_cast<std::size_t>(dx.size()) != ndx_) {
139  throw_pretty("Invalid argument: "
140  << "dx has wrong dimension (it should be " + std::to_string(ndx_) + ")");
141  }
142  VectorXs tmp_x_ = VectorXs::Zero(nx_);
143  VectorXs dx_ = VectorXs::Zero(ndx_);
144  VectorXs x0_ = VectorXs::Zero(nx_);
145 
146  // x0_ = integrate(x, dx)
147  integrate(x, dx, x0_);
148 
149  if (firstsecond == first || firstsecond == both) {
150  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
151  throw_pretty("Invalid argument: "
152  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
153  ")");
154  }
155  Jfirst.setZero();
156  for (std::size_t i = 0; i < ndx_; ++i) {
157  dx_(i) = disturbance_;
158  // tmp_x_ = integrate(x, dx_) = integrate(x, disturbance_vector)
159  integrate(x, dx_, tmp_x_);
160  // tmp_x_ = integrate(tmp_x_, dx) = integrate(integrate(x, dx_), dx)
161  integrate(tmp_x_, dx, tmp_x_);
162  // Jfirst[:,i] = diff(x0_, tmp_x_)
163  // Jfirst[:,i] = diff( integrate(x, dx), integrate(integrate(x, dx_), dx))
164  diff(x0_, tmp_x_, Jfirst.col(i));
165  dx_(i) = 0.0;
166  }
167  Jfirst /= disturbance_;
168  }
169  if (firstsecond == second || firstsecond == both) {
170  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
171  throw_pretty("Invalid argument: "
172  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
173  std::to_string(ndx_) + ")");
174  }
175  Jsecond.setZero();
176  for (std::size_t i = 0; i < ndx_; ++i) {
177  dx_(i) = disturbance_;
178  // tmp_x_ = integrate(x, dx + dx_) = integrate(x, dx + disturbance_vector)
179  integrate(x, dx + dx_, tmp_x_);
180  // Jsecond[:,i] = diff(x0_, tmp_x_)
181  // Jsecond[:,i] = diff( integrate(x, dx), integrate(x, dx_ + dx) )
182  diff(x0_, tmp_x_, Jsecond.col(i));
183  dx_(i) = 0.0;
184  }
185  Jsecond /= disturbance_;
186  }
187 }
188 
189 template <typename Scalar>
190 const Scalar& StateNumDiffTpl<Scalar>::get_disturbance() const {
191  return disturbance_;
192 }
193 
194 template <typename Scalar>
195 void StateNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
196  if (disturbance < 0.) {
197  throw_pretty("Invalid argument: "
198  << "Disturbance value is positive");
199  }
200  disturbance_ = disturbance;
201 }
202 
203 } // namespace crocoddyl
Definition: action-base.hxx:11