GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/core/integrator/rk.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 59 59 100.0%
Branches: 86 172 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, University of Trento,
5 // LAAS-CNRS, IRI: CSIC-UPC, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/core/integrator/rk.hpp"
11
12 #include "python/crocoddyl/core/core.hpp"
13 #include "python/crocoddyl/core/integ-action-base.hpp"
14
15 namespace crocoddyl {
16 namespace python {
17
18 template <typename Model>
19 struct IntegratedActionModelRKVisitor
20 : public bp::def_visitor<IntegratedActionModelRKVisitor<Model>> {
21 typedef typename Model::Scalar Scalar;
22 typedef typename Model::ActionDataAbstract Data;
23 typedef
24 typename Model::DifferentialActionModelAbstract DifferentialActionModel;
25 typedef typename Model::ControlParametrizationModelAbstract
26 ControlParametrizationModel;
27 typedef typename Model::VectorXs VectorXs;
28 template <class PyClass>
29 40 void visit(PyClass& cl) const {
30
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 40 cl.def(bp::init<std::shared_ptr<DifferentialActionModel>,
31 std::shared_ptr<ControlParametrizationModel>, RKType,
32 bp::optional<Scalar, bool>>(
33 bp::args("self", "diffModel", "control", "rktype", "stepTime",
34 "withCostResidual"),
35 "Initialize the RK integrator.\n\n"
36 ":param diffModel: differential action model\n"
37 ":param control: the control parametrization\n"
38 ":param rktype: type of RK integrator (options are two, three, "
39 "and four)\n"
40 ":param stepTime: step time (default 1e-3)\n"
41 ":param withCostResidual: includes the cost residuals and "
42 "derivatives (default True)."))
43
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def(
44 "calc",
45 static_cast<void (Model::*)(
46 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
47 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
48 bp::args("self", "data", "x", "u"),
49 "Compute the time-discrete evolution of a differential action "
50 "model.\n\n"
51 "It describes the time-discrete evolution of action model.\n"
52 ":param data: action data\n"
53 ":param x: state point (dim. state.nx)\n"
54 ":param u: control input (dim. nu)")
55
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc",
56 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
57 const Eigen::Ref<const VectorXs>&)>(
58 &Model::calc),
59 bp::args("self", "data", "x"))
60
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
61 "calcDiff",
62 static_cast<void (Model::*)(
63 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
64 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
65 bp::args("self", "data", "x", "u"),
66 "Computes the derivatives of the integrated action model wrt state "
67 "and control. \n\n"
68 "This function builds a quadratic approximation of the action "
69 "model (i.e. dynamical system and cost function). It assumes that "
70 "calc has been run first.\n"
71 ":param data: action data\n"
72 ":param x: state point (dim. state.nx)\n"
73 ":param u: control input (dim. nu)")
74
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff",
75 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
76 const Eigen::Ref<const VectorXs>&)>(
77 &Model::calcDiff),
78 bp::args("self", "data", "x"))
79
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("createData", &Model::createData, bp::args("self"),
80 "Create the RK integrator data.")
81
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 .add_property(
82 "ni",
83 bp::make_function(&Model::get_ni,
84 40 bp::return_value_policy<bp::return_by_value>()),
85 "number of nodes to be integrated");
86 40 }
87 };
88
89 template <typename Data>
90 struct IntegratedActionDataRKVisitor
91 : public bp::def_visitor<IntegratedActionDataRKVisitor<Data>> {
92 template <class PyClass>
93 40 void visit(PyClass& cl) const {
94
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 cl.add_property(
95 "differential",
96 bp::make_getter(&Data::differential,
97 40 bp::return_value_policy<bp::return_by_value>()),
98 "list of differential action data")
99
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
100 "control",
101 bp::make_getter(&Data::control,
102 40 bp::return_value_policy<bp::return_by_value>()),
103 "list of control parametrization data")
104
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
105 "integral",
106 bp::make_getter(&Data::integral,
107 40 bp::return_value_policy<bp::return_by_value>()),
108 "list of RK terms related to the cost")
109
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
110 40 "dx", bp::make_getter(&Data::dx, bp::return_internal_reference<>()),
111 "state rate.")
112
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
113 40 "ki", bp::make_getter(&Data::ki, bp::return_internal_reference<>()),
114 "list of RK terms related to system dynamics")
115
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
116 40 "y", bp::make_getter(&Data::y, bp::return_internal_reference<>()),
117 "list of states where f is evaluated in the RK integration")
118
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
119 40 "ws", bp::make_getter(&Data::ws, bp::return_internal_reference<>()),
120 "control inputs evaluated in the RK integration")
121
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
122 "dki_dx",
123 40 bp::make_getter(&Data::dki_dx, bp::return_internal_reference<>()),
124 "list of partial derivatives of RK nodes with respect to the state "
125 "of the RK integration. dki/dx")
126
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
127 "dki_du",
128 40 bp::make_getter(&Data::dki_du, bp::return_internal_reference<>()),
129 "list of partial derivatives of RK nodes with respect to the "
130 "control parameters of the RK integration. dki/du")
131
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
132 "dyi_dx",
133 40 bp::make_getter(&Data::dyi_dx, bp::return_internal_reference<>()),
134 "list of partial derivatives of RK dynamics with respect to the "
135 "state of the RK integrator. dyi/dx")
136
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
137 "dyi_du",
138 40 bp::make_getter(&Data::dyi_du, bp::return_internal_reference<>()),
139 "list of partial derivatives of RK dynamics with respect to the "
140 "control parameters of the RK integrator. dyi/du")
141
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
142 "dli_dx",
143 40 bp::make_getter(&Data::dli_dx, bp::return_internal_reference<>()),
144 "list of partial derivatives of the cost with respect to the state "
145 "of the RK integration. dli_dx")
146
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
147 "dli_du",
148 40 bp::make_getter(&Data::dli_du, bp::return_internal_reference<>()),
149 "list of partial derivatives of the cost with respect to the "
150 "control input of the RK integration. dli_du")
151
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
152 "ddli_ddx",
153 40 bp::make_getter(&Data::ddli_ddx, bp::return_internal_reference<>()),
154 "list of second partial derivatives of the cost with respect to "
155 "the state of the RK integration. ddli_ddx")
156
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
157 "ddli_ddw",
158 40 bp::make_getter(&Data::ddli_ddw, bp::return_internal_reference<>()),
159 "list of second partial derivatives of the cost with respect to "
160 "the control of the differential action model w. ddli_ddw")
161
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
162 "ddli_ddu",
163 40 bp::make_getter(&Data::ddli_ddu, bp::return_internal_reference<>()),
164 "list of second partial derivatives of the cost with respect to "
165 "the control input of the RK integration. ddli_ddu")
166
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
167 "ddli_dxdw",
168 bp::make_getter(&Data::ddli_dxdw,
169 40 bp::return_internal_reference<>()),
170 "list of second partial derivatives of the cost with respect to "
171 "the state and control of the differential action model. ddli_dxdw")
172
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
173 "ddli_dxdu",
174 bp::make_getter(&Data::ddli_dxdu,
175 40 bp::return_internal_reference<>()),
176 "list of second partial derivatives of the cost with respect to "
177 "the state and control input of the RK integration. ddli_dxdu")
178
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
179 "ddli_dwdu",
180 bp::make_getter(&Data::ddli_dwdu,
181 40 bp::return_internal_reference<>()),
182 "list of second partial derivatives of the cost with respect to "
183 "the control of the differential action model and the control "
184 "inputs of the RK integration. ddli_dwdu");
185 40 }
186 };
187
188 #define CROCODDYL_INTACTION_MODEL_RK_PYTHON_BINDINGS(Scalar) \
189 typedef IntegratedActionModelRKTpl<Scalar> Model; \
190 typedef IntegratedActionModelAbstractTpl<Scalar> ModelBase; \
191 typedef ActionModelAbstractTpl<Scalar> ActionBase; \
192 typedef DifferentialActionModelAbstractTpl<Scalar> DiffActionBase; \
193 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
194 bp::class_<Model, bp::bases<ModelBase, ActionBase>>( \
195 "IntegratedActionModelRK", \
196 "RK integrator for differential action models.\n\n" \
197 "This class implements different RK integrator schemes. The available " \
198 "integrators are: RK2, RK3, and RK4.", \
199 bp::init<std::shared_ptr<DiffActionBase>, RKType, \
200 bp::optional<Scalar, bool>>( \
201 bp::args("self", "diffModel", "rktype", "stepTime", \
202 "withCostResidual"), \
203 "Initialize the RK integrator.\n\n" \
204 ":param diffModel: differential action model\n" \
205 ":param rktype: type of RK integrator (options are two, three, and " \
206 "four)\n" \
207 ":param stepTime: step time (default 1e-3)\n" \
208 ":param withCostResidual: includes the cost residuals and " \
209 "derivatives (default True).")) \
210 .def(IntegratedActionModelRKVisitor<Model>()) \
211 .def(CastVisitor<Model>()) \
212 .def(PrintableVisitor<Model>()) \
213 .def(CopyableVisitor<Model>());
214
215 #define CROCODDYL_INTACTION_DATA_RK_PYTHON_BINDINGS(Scalar) \
216 typedef IntegratedActionDataRKTpl<Scalar> Data; \
217 typedef IntegratedActionDataAbstractTpl<Scalar> DataBase; \
218 typedef IntegratedActionModelRKTpl<Scalar> Model; \
219 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
220 bp::class_<Data, bp::bases<DataBase>>( \
221 "IntegratedActionDataRK", "RK integrator data.", \
222 bp::init<Model*>(bp::args("self", "model"), \
223 "Create RK integrator data.\n\n" \
224 ":param model: RK integrator model")) \
225 .def(IntegratedActionDataRKVisitor<Data>()) \
226 .def(CopyableVisitor<Data>());
227
228 10 void exposeIntegratedActionRK() {
229 20 bp::enum_<RKType>("RKType")
230
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 .value("two", two)
231
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 .value("three", three)
232
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 .value("four", four)
233
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 .export_values();
234
235
17/34
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 10 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 10 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 10 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 10 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_INTACTION_MODEL_RK_PYTHON_BINDINGS)
236
13/26
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 29 taken 10 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 10 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 10 times.
✗ Branch 45 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_INTACTION_DATA_RK_PYTHON_BINDINGS)
237 10 }
238
239 } // namespace python
240 } // namespace crocoddyl
241