10 #include "crocoddyl/core/utils/exception.hpp" 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) {}
18 template <
typename Scalar>
19 StateNumDiffTpl<Scalar>::~StateNumDiffTpl() {}
21 template <
typename Scalar>
22 typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::zero()
const {
23 return state_->zero();
26 template <
typename Scalar>
27 typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::rand()
const {
28 return state_->rand();
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_) +
")");
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_) +
")");
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_) +
")");
46 state_->diff(x0, x1, dxout);
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_) +
")");
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_) +
")");
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_) +
")");
64 state_->integrate(x, dx, xout);
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_) +
")");
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_) +
")");
80 VectorXs tmp_x_ = VectorXs::Zero(nx_);
81 VectorXs dx_ = VectorXs::Zero(ndx_);
82 VectorXs dx0_ = VectorXs::Zero(ndx_);
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_) +
93 for (std::size_t i = 0; i < ndx_; ++i) {
94 dx_(i) = disturbance_;
96 integrate(x0, dx_, tmp_x_);
98 diff(tmp_x_, x1, Jfirst.col(i));
101 Jfirst.col(i) -= dx0_;
104 Jfirst /= disturbance_;
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_) +
")");
114 for (std::size_t i = 0; i < ndx_; ++i) {
115 dx_(i) = disturbance_;
117 integrate(x1, dx_, tmp_x_);
119 diff(x0, tmp_x_, Jsecond.col(i));
122 Jsecond.col(i) -= dx0_;
125 Jsecond /= disturbance_;
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_) +
")");
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_) +
")");
142 VectorXs tmp_x_ = VectorXs::Zero(nx_);
143 VectorXs dx_ = VectorXs::Zero(ndx_);
144 VectorXs x0_ = VectorXs::Zero(nx_);
147 integrate(x, dx, x0_);
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_) +
156 for (std::size_t i = 0; i < ndx_; ++i) {
157 dx_(i) = disturbance_;
159 integrate(x, dx_, tmp_x_);
161 integrate(tmp_x_, dx, tmp_x_);
164 diff(x0_, tmp_x_, Jfirst.col(i));
167 Jfirst /= disturbance_;
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_) +
")");
176 for (std::size_t i = 0; i < ndx_; ++i) {
177 dx_(i) = disturbance_;
179 integrate(x, dx + dx_, tmp_x_);
182 diff(x0_, tmp_x_, Jsecond.col(i));
185 Jsecond /= disturbance_;
189 template <
typename Scalar>
190 const Scalar& StateNumDiffTpl<Scalar>::get_disturbance()
const {
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");
200 disturbance_ = disturbance;
Definition: action-base.hxx:11