Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/state.hxx	Lines:	81	107	75.7 %
Date:	2024-02-13 11:12:33	Branches:	137	526	26.0 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, New York
// University,
//                          Max Planck Gesellschaft, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename Scalar>
StateNumDiffTpl<Scalar>::StateNumDiffTpl(boost::shared_ptr<Base> state)
    : Base(state->get_nx(), state->get_ndx()),
      state_(state),
      e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}

template <typename Scalar>
StateNumDiffTpl<Scalar>::~StateNumDiffTpl() {}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::zero() const {
  return state_->zero();
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateNumDiffTpl<Scalar>::rand() const {
  return state_->rand();
}

template <typename Scalar>
void StateNumDiffTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
                                   const Eigen::Ref<const VectorXs>& x1,
                                   Eigen::Ref<VectorXs> dxout) const {
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(x1.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x1 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dxout.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dxout has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }
  state_->diff(x0, x1, dxout);
}

template <typename Scalar>
void StateNumDiffTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
                                        const Eigen::Ref<const VectorXs>& dx,
                                        Eigen::Ref<VectorXs> xout) const {
  if (static_cast<std::size_t>(x.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dx.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dx has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }
  if (static_cast<std::size_t>(xout.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "xout has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  state_->integrate(x, dx, xout);
}

template <typename Scalar>
void StateNumDiffTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0,
                                    const Eigen::Ref<const VectorXs>& x1,
                                    Eigen::Ref<MatrixXs> Jfirst,
                                    Eigen::Ref<MatrixXs> Jsecond,
                                    Jcomponent firstsecond) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(x1.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x1 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  VectorXs tmp_x_ = VectorXs::Zero(nx_);
  VectorXs dx_ = VectorXs::Zero(ndx_);
  VectorXs dx0_ = VectorXs::Zero(ndx_);

  dx_.setZero();
  diff(x0, x1, dx0_);
  if (firstsecond == first || firstsecond == both) {
    const Scalar x0h_jac = e_jac_ * std::max(1., x0.norm());
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    Jfirst.setZero();
    for (std::size_t i = 0; i < ndx_; ++i) {
      dx_(i) = x0h_jac;
      integrate(x0, dx_, tmp_x_);
      diff(tmp_x_, x1, Jfirst.col(i));
      Jfirst.col(i) -= dx0_;
      dx_(i) = 0.0;
    }
    Jfirst /= x0h_jac;
  }
  if (firstsecond == second || firstsecond == both) {
    const Scalar x1h_jac = e_jac_ * std::max(1., x1.norm());
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }

    Jsecond.setZero();
    for (std::size_t i = 0; i < ndx_; ++i) {
      dx_(i) = x1h_jac;
      integrate(x1, dx_, tmp_x_);
      diff(x0, tmp_x_, Jsecond.col(i));
      Jsecond.col(i) -= dx0_;
      dx_(i) = 0.0;
    }
    Jsecond /= x1h_jac;
  }
}

template <typename Scalar>
void StateNumDiffTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x,
                                         const Eigen::Ref<const VectorXs>& dx,
                                         Eigen::Ref<MatrixXs> Jfirst,
                                         Eigen::Ref<MatrixXs> Jsecond,
                                         const Jcomponent firstsecond,
                                         const AssignmentOp) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));
  if (static_cast<std::size_t>(x.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dx.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dx has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }
  VectorXs tmp_x_ = VectorXs::Zero(nx_);
  VectorXs dx_ = VectorXs::Zero(ndx_);
  VectorXs x0_ = VectorXs::Zero(nx_);

  // x0_ = integrate(x, dx)
  integrate(x, dx, x0_);

  if (firstsecond == first || firstsecond == both) {
    const Scalar xh_jac = e_jac_ * std::max(1., x.norm());
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    Jfirst.setZero();
    for (std::size_t i = 0; i < ndx_; ++i) {
      dx_(i) = xh_jac;
      integrate(x, dx_, tmp_x_);
      integrate(tmp_x_, dx, tmp_x_);
      diff(x0_, tmp_x_, Jfirst.col(i));
      dx_(i) = 0.;
    }
    Jfirst /= xh_jac;
  }
  if (firstsecond == second || firstsecond == both) {
    const Scalar dxh_jac = e_jac_ * std::max(1., dx.norm());
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    Jsecond.setZero();
    for (std::size_t i = 0; i < ndx_; ++i) {
      dx_(i) = dxh_jac;
      integrate(x, dx + dx_, tmp_x_);
      diff(x0_, tmp_x_, Jsecond.col(i));
      dx_(i) = 0.;
    }
    Jsecond /= dxh_jac;
  }
}

template <typename Scalar>
void StateNumDiffTpl<Scalar>::JintegrateTransport(
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&,
    Eigen::Ref<MatrixXs>, const Jcomponent) const {}

template <typename Scalar>
const Scalar StateNumDiffTpl<Scalar>::get_disturbance() const {
  return e_jac_;
}

template <typename Scalar>
void StateNumDiffTpl<Scalar>::set_disturbance(Scalar disturbance) {
  if (disturbance < 0.) {
    throw_pretty("Invalid argument: "
                 << "Disturbance constant is positive");
  }
  e_jac_ = disturbance;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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