GCC Code Coverage Report

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