GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/multibody/actions/impulse-fwddyn.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 38 38 100.0%
Branches: 71 142 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
11
12 #include "python/crocoddyl/core/action-base.hpp"
13 #include "python/crocoddyl/multibody/multibody.hpp"
14
15 namespace crocoddyl {
16 namespace python {
17
18 template <typename Model>
19 struct ActionModelImpulseFwdDynamicsVisitor
20 : public bp::def_visitor<ActionModelImpulseFwdDynamicsVisitor<Model>> {
21 typedef typename Model::Scalar Scalar;
22 typedef typename Model::ActionDataAbstract Data;
23 typedef typename Model::StateMultibody State;
24 typedef typename Model::ImpulseModelMultiple Impulses;
25 typedef typename Model::CostModelSum Costs;
26 typedef typename Model::ConstraintModelManager Constraints;
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<State>, std::shared_ptr<Impulses>,
31 std::shared_ptr<Costs>, std::shared_ptr<Constraints>,
32 bp::optional<Scalar, Scalar, bool>>(
33 bp::args("self", "state", "impulses", "costs", "constraints",
34 "r_coeff", "inv_damping", "enable_force"),
35 "Initialize the constrained forward-dynamics action model.\n\n"
36 "The damping factor is needed when the contact Jacobian is not "
37 "full-rank. Otherwise, a good damping factor could be 1e-12. In "
38 "addition, if you have cost based on forces, you need to enable "
39 "the computation of the force Jacobians (i.e., "
40 "enable_force=True).\n"
41 ":param state: multibody state\n"
42 ":param impulses: multiple impulse model\n"
43 ":param costs: stack of cost functions\n"
44 ":param constraints: stack of constraint functions\n"
45 ":param r_coeff: restitution coefficient (default 0.)\n"
46 ":param inv_damping: Damping factor for cholesky decomposition "
47 "of JMinvJt (default 0.)\n"
48 ":param enable_force: Enable the computation of force Jacobians "
49 "(default False)"))
50
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def(
51 "calc",
52 static_cast<void (Model::*)(
53 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
54 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
55 bp::args("self", "data", "x", "u"),
56 "Compute the next state and cost value.\n\n"
57 "It describes the time-continuous evolution of the multibody "
58 "system with impulse. The impulses are modelled as holonomic "
59 "constraints. Additionally it computes the cost value associated "
60 "to this state and control pair.\n"
61 ":param data: impulse forward-dynamics action data\n"
62 ":param x: time-continuous state vector\n"
63 ":param u: time-continuous control input")
64
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc",
65 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
66 const Eigen::Ref<const VectorXs>&)>(
67 &Model::calc),
68 bp::args("self", "data", "x"))
69
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
70 "calcDiff",
71 static_cast<void (Model::*)(
72 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
73 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
74 bp::args("self", "data", "x", "u"),
75 "Compute the derivatives of the differential multibody system and "
76 "its cost functions.\n\n"
77 "It computes the partial derivatives of the differential multibody "
78 "system and the cost function. It assumes that calc has been run "
79 "first. This function builds a quadratic approximation of the "
80 "action model (i.e. dynamical system and cost function).\n"
81 ":param data: impulse forward-dynamics action data\n"
82 ":param x: time-continuous state vector\n"
83 ":param u: time-continuous control input\n"
84 "")
85
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff",
86 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
87 const Eigen::Ref<const VectorXs>&)>(
88 &Model::calcDiff),
89 bp::args("self", "data", "x"))
90
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"),
91 "Create the impulse forward dynamics differential action data.")
92
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("pinocchio",
93 bp::make_function(&Model::get_pinocchio,
94 40 bp::return_internal_reference<>()),
95 "multibody model (i.e. pinocchio model)")
96
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
97 "impulses",
98 bp::make_function(&Model::get_impulses,
99 40 bp::return_value_policy<bp::return_by_value>()),
100 "multiple contact model")
101
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
102 "costs",
103 bp::make_function(&Model::get_costs,
104 40 bp::return_value_policy<bp::return_by_value>()),
105 "total cost model")
106
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
107 "constraints",
108 bp::make_function(&Model::get_constraints,
109 40 bp::return_value_policy<bp::return_by_value>()),
110 "constraint model manager")
111
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 .add_property("armature",
112 bp::make_function(&Model::get_armature,
113 40 bp::return_internal_reference<>()),
114 bp::make_function(&Model::set_armature),
115 "set an armature mechanism in the joints")
116
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 .add_property("r_coeff",
117 bp::make_function(&Model::get_restitution_coefficient),
118 bp::make_function(&Model::set_restitution_coefficient),
119 "Restitution coefficient that describes elastic impacts")
120
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("JMinvJt_damping",
121 bp::make_function(&Model::get_damping_factor),
122 bp::make_function(&Model::set_damping_factor),
123 "Damping factor for cholesky decomposition of JMinvJt");
124 40 }
125 };
126
127 template <typename Data>
128 struct ActionDataImpulseFwdDynamicsVisitor
129 : public bp::def_visitor<ActionDataImpulseFwdDynamicsVisitor<Data>> {
130 template <class PyClass>
131 40 void visit(PyClass& cl) const {
132
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 cl.add_property(
133 "pinocchio",
134 40 bp::make_getter(&Data::pinocchio, bp::return_internal_reference<>()),
135 "pinocchio data")
136
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("multibody",
137 bp::make_getter(&Data::multibody,
138 40 bp::return_internal_reference<>()),
139 "multibody 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(
141 "costs",
142 bp::make_getter(&Data::costs,
143 40 bp::return_value_policy<bp::return_by_value>()),
144 "total cost data")
145
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
146 "constraints",
147 bp::make_getter(&Data::constraints,
148 40 bp::return_value_policy<bp::return_by_value>()),
149 "constraint data")
150
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
151 "Kinv",
152 40 bp::make_getter(&Data::Kinv, bp::return_internal_reference<>()),
153 "inverse of the KKT matrix")
154
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
155 "df_dx",
156 40 bp::make_getter(&Data::df_dx, bp::return_internal_reference<>()),
157 "Jacobian of the contact impulse");
158 40 }
159 };
160
161 #define CROCODDYL_ACTION_MODEL_IMPULSE_FWDDYN_PYTHON_BINDINGS(Scalar) \
162 typedef ActionModelImpulseFwdDynamicsTpl<Scalar> Model; \
163 typedef ActionModelAbstractTpl<Scalar> ModelBase; \
164 typedef typename Model::StateMultibody State; \
165 typedef typename Model::ImpulseModelMultiple Impulses; \
166 typedef typename Model::CostModelSum Costs; \
167 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
168 bp::class_<Model, bp::bases<ModelBase>>( \
169 "ActionModelImpulseFwdDynamics", \
170 "Action model for impulse forward dynamics in multibody systems.\n\n" \
171 "The impulse is modelled as holonomic constraits in the contact frame. " \
172 "There is also a custom implementation in case of system with " \
173 "armatures. If you want to include the armature, you need to use " \
174 "set_armature(). On the other hand, the stack of cost functions are " \
175 "implemented in CostModelSum().", \
176 bp::init<std::shared_ptr<State>, std::shared_ptr<Impulses>, \
177 std::shared_ptr<Costs>, bp::optional<Scalar, Scalar, bool>>( \
178 bp::args("self", "state", " impulses", "costs", "r_coeff", \
179 "inv_damping", "enable_force"), \
180 "Initialize the impulse forward-dynamics action model.\n\n" \
181 "The damping factor is needed when the contact Jacobian is not " \
182 "full-rank. Otherwise, a good damping factor could be 1e-12. In " \
183 "addition, if you have cost based on forces, you need to enable " \
184 "the computation of the force Jacobians (i.e., " \
185 "enable_force=True).\n" \
186 ":param state: multibody state\n" \
187 ":param impulses: multiple impulse model\n" \
188 ":param costs: stack of cost functions\n" \
189 ":param r_coeff: restitution coefficient (default 0.)\n" \
190 ":param inv_damping: Damping factor for cholesky decomposition of " \
191 "JMinvJt (default 0.)\n" \
192 ":param enable_force: Enable the computation of force Jacobians " \
193 "(default False)")) \
194 .def(ActionModelImpulseFwdDynamicsVisitor<Model>()) \
195 .def(CastVisitor<Model>()) \
196 .def(PrintableVisitor<Model>()) \
197 .def(CopyableVisitor<Model>());
198
199 #define CROCODDYL_ACTION_DATA_IMPULSE_FWDDYN_PYTHON_BINDINGS(Scalar) \
200 typedef ActionDataImpulseFwdDynamicsTpl<Scalar> Data; \
201 typedef ActionDataAbstractTpl<Scalar> DataBase; \
202 typedef ActionModelImpulseFwdDynamicsTpl<Scalar> Model; \
203 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
204 bp::class_<Data, bp::bases<DataBase>>( \
205 "ActionDataImpulseFwdDynamics", \
206 "Action data for the impulse forward dynamics system.", \
207 bp::init<Model*>(bp::args("self", "model"), \
208 "Create impulse forward-dynamics action data.\n\n" \
209 ":param model: impulse forward-dynamics action model")) \
210 .def(ActionDataImpulseFwdDynamicsVisitor<Data>()) \
211 .def(CopyableVisitor<Data>());
212
213 10 void exposeActionImpulseFwdDynamics() {
214
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(
215 CROCODDYL_ACTION_MODEL_IMPULSE_FWDDYN_PYTHON_BINDINGS)
216
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_ACTION_DATA_IMPULSE_FWDDYN_PYTHON_BINDINGS)
217 10 }
218
219 } // namespace python
220 } // namespace crocoddyl
221