GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/multibody/actions/free-invdyn.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 35 35 100.0%
Branches: 94 188 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/multibody/actions/free-invdyn.hpp"
10
11 #include "python/crocoddyl/core/diff-action-base.hpp"
12 #include "python/crocoddyl/multibody/multibody.hpp"
13
14 namespace crocoddyl {
15 namespace python {
16
17 template <typename Model>
18 struct DifferentialActionModelFreeInvDynamicsVisitor
19 : public bp::def_visitor<
20 DifferentialActionModelFreeInvDynamicsVisitor<Model>> {
21 typedef typename Model::DifferentialActionDataAbstract Data;
22 typedef typename Model::VectorXs VectorXs;
23 template <class PyClass>
24 40 void visit(PyClass& cl) const {
25 40 cl.def("calc",
26 static_cast<void (Model::*)(
27 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
28 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
29 bp::args("self", "data", "x", "u"),
30 "Compute the next state and cost value.\n\n"
31 ":param data: free inverse-dynamics data\n"
32 ":param x: state point (dim. state.nx)\n"
33 ":param u: control input (dim. nu)")
34
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc",
35 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
36 const Eigen::Ref<const VectorXs>&)>(
37 &Model::calc),
38 bp::args("self", "data", "x"))
39
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
40 "calcDiff",
41 static_cast<void (Model::*)(
42 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
43 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
44 bp::args("self", "data", "x", "u"),
45 "Compute the derivatives of the differential multibody system "
46 "(free of contact) and its cost functions.\n\n"
47 "It computes the partial derivatives of the differential multibody "
48 "system and the cost function. It assumes that calc has been run "
49 "first. This function builds a quadratic approximation of the\n"
50 "action model (i.e. dynamical system and cost function).\n"
51 ":param data: free inverse-dynamics data\n"
52 ":param x: state point (dim. state.nx)\n"
53 ":param u: control input (dim. nu)")
54
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff",
55 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
56 const Eigen::Ref<const VectorXs>&)>(
57 &Model::calcDiff),
58 bp::args("self", "data", "x"))
59
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"),
60 "Create the free inverse-dynamics differential action data.")
61
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(
62 "actuation",
63 bp::make_function(&Model::get_actuation,
64 40 bp::return_value_policy<bp::return_by_value>()),
65 "actuation model")
66
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
67 "costs",
68 bp::make_function(&Model::get_costs,
69 40 bp::return_value_policy<bp::return_by_value>()),
70 "total cost model")
71
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
72 "constraints",
73 bp::make_function(&Model::get_constraints,
74 40 bp::return_value_policy<bp::return_by_value>()),
75 "constraint model manager");
76 40 }
77 };
78
79 template <typename Model>
80 struct DifferentialActionModelFreeInvDynamicsResidualModelActuationVisitor
81 : public bp::def_visitor<
82 DifferentialActionModelFreeInvDynamicsResidualModelActuationVisitor<
83 Model>> {
84 typedef typename Model::ResidualDataAbstract Data;
85 typedef typename Model::VectorXs VectorXs;
86 template <class PyClass>
87 40 void visit(PyClass& cl) const {
88 40 cl.def("calc",
89 static_cast<void (Model::*)(
90 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
91 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
92 bp::args("self", "data", "x", "u"),
93 "Compute the actuation residual.\n\n"
94 ":param data: residual data\n"
95 ":param x: state point (dim. state.nx)\n"
96 ":param u: control input (dim. nu)")
97
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc",
98 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
99 const Eigen::Ref<const VectorXs>&)>(
100 &Model::calc),
101 bp::args("self", "data", "x"))
102
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
103 "calcDiff",
104 static_cast<void (Model::*)(
105 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
106 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
107 bp::args("self", "data", "x", "u"),
108 "Compute the Jacobians of the actuation residual.\n\n"
109 "It assumes that calc has been run first.\n"
110 ":param data: action data\n"
111 ":param x: state point (dim. state.nx)\n"
112 ":param u: control input (dim. nu)\n")
113
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff",
114 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
115 const Eigen::Ref<const VectorXs>&)>(
116 &Model::calcDiff),
117 bp::args("self", "data", "x"))
118
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.
 80 .def("createData", &Model::createData,
119 40 bp::with_custodian_and_ward_postcall<0, 2>(),
120 bp::args("self", "data"),
121 "Create the actuation residual data.\n\n"
122 "Each residual model has its own data that needs to be allocated. "
123 "This function\n"
124 "returns the allocated data for the actuation residual.\n"
125 ":param data: shared data\n"
126 ":return residual data.");
127 40 }
128 };
129
130 template <typename Data>
131 struct DifferentialActionDataFreeInvDynamicsVisitor
132 : public bp::def_visitor<
133 DifferentialActionDataFreeInvDynamicsVisitor<Data>> {
134 template <class PyClass>
135 40 void visit(PyClass& cl) const {
136
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 cl.add_property(
137 "pinocchio",
138 40 bp::make_getter(&Data::pinocchio, bp::return_internal_reference<>()),
139 "pinocchio data")
140
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("multibody",
141 bp::make_getter(&Data::multibody,
142 40 bp::return_internal_reference<>()),
143 "multibody data")
144
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
145 "costs",
146 bp::make_getter(&Data::costs,
147 40 bp::return_value_policy<bp::return_by_value>()),
148 "total cost data")
149
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
150 "constraints",
151 bp::make_getter(&Data::constraints,
152 40 bp::return_value_policy<bp::return_by_value>()),
153 "constraint data");
154 40 }
155 };
156
157 #define CROCODDYL_ACTION_MODEL_FREE_INVDYN_PYTHON_BINDINGS(Scalar) \
158 typedef DifferentialActionModelFreeInvDynamicsTpl<Scalar> Model; \
159 typedef DifferentialActionModelAbstractTpl<Scalar> ModelBase; \
160 typedef typename Model::StateMultibody State; \
161 typedef typename Model::ActuationModelAbstract Actuation; \
162 typedef typename Model::CostModelSum Costs; \
163 typedef typename Model::ConstraintModelManager Constraints; \
164 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
165 bp::scope model_outer = \
166 bp::class_<Model, bp::bases<ModelBase>>( \
167 "DifferentialActionModelFreeInvDynamics", \
168 "Differential action model for free inverse dynamics in multibody " \
169 "systems.\n\n" \
170 "This class implements the dynamics using Recursive Newton Euler " \
171 "Algorithm (RNEA) as an equality constraint. The stack of cost and " \
172 "constraint functions are implemented in ConstraintModelManager() " \
173 "and CostModelSum(), respectively.", \
174 bp::init<std::shared_ptr<State>, std::shared_ptr<Actuation>, \
175 std::shared_ptr<Costs>, \
176 bp::optional<std::shared_ptr<Constraints>>>( \
177 bp::args("self", "state", "actuation", "costs", "constraints"), \
178 "Initialize the free inverse-dynamics action model.\n\n" \
179 "It describes the kinematic evolution of the multibody system " \
180 "and computes the needed torques using inverse dynamics.\n" \
181 ":param state: multibody state\n" \
182 ":param actuation: abstract actuation model\n" \
183 ":param costs: stack of cost functions\n" \
184 ":param constraints: stack of constraint functions")) \
185 .def(DifferentialActionModelFreeInvDynamicsVisitor<Model>()) \
186 .def(CastVisitor<Model>()) \
187 .def(PrintableVisitor<Model>()) \
188 .def(CopyableVisitor<Model>()); \
189 typedef typename Model::ResidualModelActuation ResidualModelActuation; \
190 typedef typename ResidualModelActuation::Base ResidualModelBase; \
191 bp::register_ptr_to_python<std::shared_ptr<ResidualModelActuation>>(); \
192 bp::class_<ResidualModelActuation, bp::bases<ResidualModelBase>>( \
193 "ResidualModelActuation", \
194 "This residual function enforces the torques of under-actuated joints " \
195 "(e.g., floating-base joints) to be zero. We compute these torques and " \
196 "their derivatives using RNEA inside " \
197 "DifferentialActionModelFreeInvDynamics.", \
198 bp::init<std::shared_ptr<State>, std::size_t>( \
199 bp::args("self", "state", "nu"), \
200 "Initialize the actuation residual model.\n\n" \
201 ":param state: state description\n" \
202 ":param nu: dimension of the joint torques")) \
203 .def( \
204 DifferentialActionModelFreeInvDynamicsResidualModelActuationVisitor< \
205 ResidualModelActuation>()) \
206 .def(CopyableVisitor<ResidualModelActuation>());
207
208 #define CROCODDYL_ACTION_DATA_FREE_INVDYN_PYTHON_BINDINGS(Scalar) \
209 typedef DifferentialActionDataFreeInvDynamicsTpl<Scalar> Data; \
210 typedef DifferentialActionDataAbstractTpl<Scalar> DataBase; \
211 typedef DifferentialActionModelFreeInvDynamicsTpl<Scalar> Model; \
212 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
213 bp::scope data_outer = \
214 bp::class_<Data, bp::bases<DataBase>>( \
215 "DifferentialActionDataFreeInvDynamics", \
216 "Action data for the free inverse-dynamics system.", \
217 bp::init<Model*>( \
218 bp::args("self", "model"), \
219 "Create free inverse-dynamics action data.\n\n" \
220 ":param model: free inverse-dynamics action model")) \
221 .def(DifferentialActionDataFreeInvDynamicsVisitor<Data>()) \
222 .def(CopyableVisitor<Data>()); \
223 typedef typename Model::ResidualModelActuation ResidualModelActuation; \
224 typedef typename Data::ResidualDataActuation ResidualDataActuation; \
225 typedef typename ResidualDataActuation::Base ResidualDataBase; \
226 typedef \
227 typename ResidualModelActuation::DataCollectorAbstract DataCollector; \
228 bp::register_ptr_to_python<std::shared_ptr<ResidualDataActuation>>(); \
229 bp::class_<ResidualDataActuation, bp::bases<ResidualDataBase>>( \
230 "ResidualDataActuation", "Data for actuation residual.\n\n", \
231 bp::init<ResidualModelActuation*, DataCollector*>( \
232 bp::args("self", "model", "data"), \
233 "Create actuation residual data.\n\n" \
234 ":param model: actuation residual model\n" \
235 ":param data: shared data")[bp::with_custodian_and_ward< \
236 1, 2, bp::with_custodian_and_ward<1, 3>>()]) \
237 .def(CopyableVisitor<ResidualDataActuation>());
238
239 10 void exposeDifferentialActionFreeInvDynamics() {
240
33/66
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 30 taken 10 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 10 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 10 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 10 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 10 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 10 times.
✗ Branch 46 not taken.
✓ Branch 52 taken 10 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 10 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 10 times.
✗ Branch 59 not taken.
✓ Branch 63 taken 10 times.
✗ Branch 64 not taken.
✓ Branch 66 taken 10 times.
✗ Branch 67 not taken.
✓ Branch 69 taken 10 times.
✗ Branch 70 not taken.
✓ Branch 72 taken 10 times.
✗ Branch 73 not taken.
✓ Branch 75 taken 10 times.
✗ Branch 76 not taken.
✓ Branch 78 taken 10 times.
✗ Branch 79 not taken.
✓ Branch 81 taken 10 times.
✗ Branch 82 not taken.
✓ Branch 84 taken 10 times.
✗ Branch 85 not taken.
✓ Branch 87 taken 10 times.
✗ Branch 88 not taken.
✓ Branch 92 taken 10 times.
✗ Branch 93 not taken.
✓ Branch 95 taken 10 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 10 times.
✗ Branch 99 not taken.
✓ Branch 101 taken 10 times.
✗ Branch 102 not taken.
✓ Branch 104 taken 10 times.
✗ Branch 105 not taken.
✓ Branch 107 taken 10 times.
✗ Branch 108 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_ACTION_MODEL_FREE_INVDYN_PYTHON_BINDINGS)
241
29/58
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 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 33 taken 10 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 10 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 10 times.
✗ Branch 40 not taken.
✓ Branch 46 taken 10 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 10 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 10 times.
✗ Branch 53 not taken.
✓ Branch 57 taken 10 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 10 times.
✗ Branch 61 not taken.
✓ Branch 63 taken 10 times.
✗ Branch 64 not taken.
✓ Branch 66 taken 10 times.
✗ Branch 67 not taken.
✓ Branch 69 taken 10 times.
✗ Branch 70 not taken.
✓ Branch 72 taken 10 times.
✗ Branch 73 not taken.
✓ Branch 75 taken 10 times.
✗ Branch 76 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 10 times.
✗ Branch 87 not taken.
✓ Branch 89 taken 10 times.
✗ Branch 90 not taken.
✓ Branch 92 taken 10 times.
✗ Branch 93 not taken.
✓ Branch 95 taken 10 times.
✗ Branch 96 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_ACTION_DATA_FREE_INVDYN_PYTHON_BINDINGS)
242 10 }
243
244 } // namespace python
245 } // namespace crocoddyl
246