GCC Code Coverage Report

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