GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/core/state-base.hpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 6 147 4.1%
Branches: 0 546 0.0%
Line Branch Exec Source
1
2 ///////////////////////////////////////////////////////////////////////////////
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef BINDINGS_PYTHON_CROCODDYL_CORE_STATE_BASE_HPP_
11 #define BINDINGS_PYTHON_CROCODDYL_CORE_STATE_BASE_HPP_
12
13 #include <string>
14
15 #include "crocoddyl/core/state-base.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 #include "python/crocoddyl/core/core.hpp"
18
19 namespace crocoddyl {
20 namespace python {
21
22 class StateAbstract_wrap : public StateAbstract,
23 public bp::wrapper<StateAbstract> {
24 public:
25 using StateAbstract::lb_;
26 using StateAbstract::ndx_;
27 using StateAbstract::nq_;
28 using StateAbstract::nv_;
29 using StateAbstract::nx_;
30 using StateAbstract::ub_;
31
32 4 StateAbstract_wrap(int nx, int ndx)
33 4 : StateAbstract(nx, ndx), bp::wrapper<StateAbstract>() {
34 4 enableMultithreading() = false;
35 4 }
36
37 ✗ Eigen::VectorXd zero() const {
38 ✗ return bp::call<Eigen::VectorXd>(this->get_override("zero").ptr());
39 }
40
41 ✗ Eigen::VectorXd rand() const {
42 ✗ return bp::call<Eigen::VectorXd>(this->get_override("rand").ptr());
43 }
44
45 ✗ Eigen::VectorXd diff_wrap(const Eigen::Ref<const Eigen::VectorXd>& x0,
46 const Eigen::Ref<const Eigen::VectorXd>& x1) const {
47 ✗ if (static_cast<std::size_t>(x0.size()) != nx_) {
48 ✗ throw_pretty(
49 "Invalid argument: " << "x0 has wrong dimension (it should be " +
50 std::to_string(nx_) + ")");
51 }
52 ✗ if (static_cast<std::size_t>(x1.size()) != nx_) {
53 ✗ throw_pretty(
54 "Invalid argument: " << "x1 has wrong dimension (it should be " +
55 std::to_string(nx_) + ")");
56 }
57 ✗ return bp::call<Eigen::VectorXd>(this->get_override("diff").ptr(),
58 ✗ (Eigen::VectorXd)x0, (Eigen::VectorXd)x1);
59 }
60
61 ✗ void diff(const Eigen::Ref<const Eigen::VectorXd>& x0,
62 const Eigen::Ref<const Eigen::VectorXd>& x1,
63 Eigen::Ref<Eigen::VectorXd> dxout) const {
64 ✗ dxout = diff_wrap(x0, x1);
65 }
66
67 ✗ Eigen::VectorXd integrate_wrap(
68 const Eigen::Ref<const Eigen::VectorXd>& x,
69 const Eigen::Ref<const Eigen::VectorXd>& dx) const {
70 ✗ if (static_cast<std::size_t>(x.size()) != nx_) {
71 ✗ throw_pretty(
72 "Invalid argument: " << "x has wrong dimension (it should be " +
73 std::to_string(nx_) + ")");
74 }
75 ✗ if (static_cast<std::size_t>(dx.size()) != ndx_) {
76 ✗ throw_pretty(
77 "Invalid argument: " << "dx has wrong dimension (it should be " +
78 std::to_string(ndx_) + ")");
79 }
80 ✗ return bp::call<Eigen::VectorXd>(this->get_override("integrate").ptr(),
81 ✗ (Eigen::VectorXd)x, (Eigen::VectorXd)dx);
82 }
83
84 ✗ void integrate(const Eigen::Ref<const Eigen::VectorXd>& x,
85 const Eigen::Ref<const Eigen::VectorXd>& dx,
86 Eigen::Ref<Eigen::VectorXd> x1out) const {
87 ✗ x1out = integrate_wrap(x, dx);
88 }
89
90 ✗ void Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0,
91 const Eigen::Ref<const Eigen::VectorXd>& x1,
92 Eigen::Ref<Eigen::MatrixXd> Jfirst,
93 Eigen::Ref<Eigen::MatrixXd> Jsecond,
94 const Jcomponent firstsecond) const {
95 ✗ bp::list res = Jdiff_wrap(x0, x1, firstsecond);
96 ✗ switch (firstsecond) {
97 ✗ case first: {
98 ✗ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
99 ✗ break;
100 }
101 ✗ case second: {
102 ✗ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
103 ✗ break;
104 }
105 ✗ case both: {
106 ✗ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
107 ✗ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[1])();
108 ✗ break;
109 }
110 ✗ default: {
111 ✗ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
112 ✗ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[1])();
113 ✗ break;
114 }
115 }
116 }
117
118 ✗ bp::list Jdiff_wrap(const Eigen::Ref<const Eigen::VectorXd>& x0,
119 const Eigen::Ref<const Eigen::VectorXd>& x1,
120 const Jcomponent firstsecond) const {
121 ✗ assert_pretty(
122 is_a_Jcomponent(firstsecond),
123 ("firstsecond must be one of the Jcomponent {both, first, second}"));
124 ✗ if (static_cast<std::size_t>(x0.size()) != nx_) {
125 ✗ throw_pretty(
126 "Invalid argument: " << "x0 has wrong dimension (it should be " +
127 std::to_string(nx_) + ")");
128 }
129 ✗ if (static_cast<std::size_t>(x1.size()) != nx_) {
130 ✗ throw_pretty(
131 "Invalid argument: " << "x1 has wrong dimension (it should be " +
132 std::to_string(nx_) + ")");
133 }
134
135 ✗ bp::list Jacs;
136 ✗ switch (firstsecond) {
137 ✗ case first: {
138 Eigen::MatrixXd J = bp::call<Eigen::MatrixXd>(
139 ✗ this->get_override("Jdiff").ptr(), (Eigen::VectorXd)x0,
140 ✗ (Eigen::VectorXd)x1, firstsecond);
141 ✗ Jacs.append(J);
142 ✗ break;
143 }
144 ✗ case second: {
145 Eigen::MatrixXd J = bp::call<Eigen::MatrixXd>(
146 ✗ this->get_override("Jdiff").ptr(), (Eigen::VectorXd)x0,
147 ✗ (Eigen::VectorXd)x1, firstsecond);
148 ✗ Jacs.append(J);
149 ✗ break;
150 }
151 ✗ case both: {
152 ✗ Jacs = bp::call<bp::list>(this->get_override("Jdiff").ptr(),
153 ✗ (Eigen::VectorXd)x0, (Eigen::VectorXd)x1,
154 ✗ firstsecond);
155 ✗ break;
156 }
157 ✗ default: {
158 ✗ Jacs = bp::call<bp::list>(this->get_override("Jdiff").ptr(),
159 ✗ (Eigen::VectorXd)x0, (Eigen::VectorXd)x1,
160 ✗ firstsecond);
161 ✗ break;
162 }
163 }
164 ✗ return Jacs;
165 }
166
167 ✗ void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x,
168 const Eigen::Ref<const Eigen::VectorXd>& dx,
169 Eigen::Ref<Eigen::MatrixXd> Jfirst,
170 Eigen::Ref<Eigen::MatrixXd> Jsecond,
171 const Jcomponent firstsecond, const AssignmentOp op) const {
172 ✗ bp::list res = Jintegrate_wrap(x, dx, firstsecond);
173 ✗ if (firstsecond == first || firstsecond == both) {
174 ✗ if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
175 ✗ static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
176 ✗ throw_pretty("Invalid argument: "
177 << "Jfirst has wrong dimension (it should be " +
178 std::to_string(ndx_) + "," + std::to_string(ndx_) +
179 ")");
180 }
181 ✗ switch (op) {
182 ✗ case setto: {
183 ✗ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
184 ✗ break;
185 }
186 ✗ case addto: {
187 ✗ Jfirst.derived() += bp::extract<Eigen::MatrixXd>(res[0])();
188 ✗ break;
189 }
190 ✗ case rmfrom: {
191 ✗ Jfirst.derived() -= bp::extract<Eigen::MatrixXd>(res[0])();
192 ✗ break;
193 }
194 ✗ default: {
195 ✗ throw_pretty(
196 "Invalid argument: allowed operators: setto, addto, rmfrom");
197 break;
198 }
199 }
200 }
201 ✗ if (firstsecond == second || firstsecond == both) {
202 ✗ if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
203 ✗ static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
204 ✗ throw_pretty("Invalid argument: "
205 << "Jsecond has wrong dimension (it should be " +
206 std::to_string(ndx_) + "," + std::to_string(ndx_) +
207 ")");
208 }
209 ✗ switch (op) {
210 ✗ case setto: {
211 ✗ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
212 ✗ break;
213 }
214 ✗ case addto: {
215 ✗ Jsecond.derived() += bp::extract<Eigen::MatrixXd>(res[0])();
216 ✗ break;
217 }
218 ✗ case rmfrom: {
219 ✗ Jsecond.derived() -= bp::extract<Eigen::MatrixXd>(res[0])();
220 ✗ break;
221 }
222 ✗ default: {
223 ✗ throw_pretty(
224 "Invalid argument: allowed operators: setto, addto, rmfrom");
225 break;
226 }
227 }
228 }
229 }
230
231 ✗ bp::list Jintegrate_wrap(const Eigen::Ref<const Eigen::VectorXd>& x,
232 const Eigen::Ref<const Eigen::VectorXd>& dx,
233 const Jcomponent firstsecond) const {
234 ✗ assert_pretty(
235 is_a_Jcomponent(firstsecond),
236 ("firstsecond must be one of the Jcomponent {both, first, second}"));
237 ✗ if (static_cast<std::size_t>(x.size()) != nx_) {
238 ✗ throw_pretty(
239 "Invalid argument: " << "x has wrong dimension (it should be " +
240 std::to_string(nx_) + ")");
241 }
242 ✗ if (static_cast<std::size_t>(dx.size()) != ndx_) {
243 ✗ throw_pretty(
244 "Invalid argument: " << "dx has wrong dimension (it should be " +
245 std::to_string(ndx_) + ")");
246 }
247
248 ✗ bp::list Jacs;
249 ✗ switch (firstsecond) {
250 ✗ case first: {
251 Eigen::MatrixXd J = bp::call<Eigen::MatrixXd>(
252 ✗ this->get_override("Jintegrate").ptr(), (Eigen::VectorXd)x,
253 ✗ (Eigen::VectorXd)dx, firstsecond);
254 ✗ Jacs.append(J);
255 ✗ break;
256 }
257 ✗ case second: {
258 Eigen::MatrixXd J = bp::call<Eigen::MatrixXd>(
259 ✗ this->get_override("Jintegrate").ptr(), (Eigen::VectorXd)x,
260 ✗ (Eigen::VectorXd)dx, firstsecond);
261 ✗ Jacs.append(J);
262 ✗ break;
263 }
264 ✗ case both: {
265 ✗ Jacs = bp::call<bp::list>(this->get_override("Jintegrate").ptr(),
266 ✗ (Eigen::VectorXd)x, (Eigen::VectorXd)dx,
267 ✗ firstsecond);
268 ✗ break;
269 }
270 ✗ default: {
271 ✗ Jacs = bp::call<bp::list>(this->get_override("Jintegrate").ptr(),
272 ✗ (Eigen::VectorXd)x, (Eigen::VectorXd)dx,
273 ✗ firstsecond);
274 ✗ break;
275 }
276 }
277 ✗ return Jacs;
278 }
279
280 ✗ void JintegrateTransport(const Eigen::Ref<const Eigen::VectorXd>& x,
281 const Eigen::Ref<const Eigen::VectorXd>& dx,
282 Eigen::Ref<Eigen::MatrixXd> Jin,
283 const Jcomponent firstsecond) const {
284 ✗ Jin = JintegrateTransport_wrap(x, dx, Jin, firstsecond);
285 }
286
287 ✗ Eigen::MatrixXd JintegrateTransport_wrap(
288 const Eigen::Ref<const Eigen::VectorXd>& x,
289 const Eigen::Ref<const Eigen::VectorXd>& dx,
290 Eigen::Ref<Eigen::MatrixXd> Jin, const Jcomponent firstsecond) const {
291 ✗ assert_pretty(
292 is_a_Jcomponent(firstsecond),
293 ("firstsecond must be one of the Jcomponent {both, first, second}"));
294 ✗ if (static_cast<std::size_t>(x.size()) != nx_) {
295 ✗ throw_pretty(
296 "Invalid argument: " << "x has wrong dimension (it should be " +
297 std::to_string(nx_) + ")");
298 }
299 ✗ if (static_cast<std::size_t>(dx.size()) != ndx_) {
300 ✗ throw_pretty(
301 "Invalid argument: " << "dx has wrong dimension (it should be " +
302 std::to_string(ndx_) + ")");
303 }
304 return bp::call<Eigen::MatrixXd>(
305 ✗ this->get_override("JintegrateTransport").ptr(), (Eigen::VectorXd)x,
306 ✗ (Eigen::VectorXd)dx, (Eigen::MatrixXd)Jin, firstsecond);
307 }
308 };
309
310 72 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Jdiffs, StateAbstract::Jdiff_Js, 2, 3)
311 48 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Jintegrates,
312 StateAbstract::Jintegrate_Js, 2, 3)
313
314 } // namespace python
315 } // namespace crocoddyl
316
317 #endif // BINDINGS_PYTHON_CROCODDYL_CORE_STATE_BASE_HPP_
318