GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: bindings/python/crocoddyl/core/state-base.hpp Lines: 6 147 4.1 %
Date: 2024-02-13 11:12:33 Branches: 0 554 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("Invalid argument: "
49
                   << "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("Invalid argument: "
54
                   << "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("Invalid argument: "
72
                   << "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("Invalid argument: "
77
                   << "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("Invalid argument: "
126
                   << "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("Invalid argument: "
131
                   << "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("Invalid argument: "
239
                   << "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("Invalid argument: "
244
                   << "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("Invalid argument: "
296
                   << "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("Invalid argument: "
301
                   << "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_