GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/multibody/actions/free-fwddyn.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 35 35 100.0%
Branches: 62 124 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/multibody/actions/free-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 DifferentialActionModelFreeFwdDynamicsVisitor
20 : public bp::def_visitor<
21 DifferentialActionModelFreeFwdDynamicsVisitor<Model>> {
22 typedef typename Model::DifferentialActionDataAbstract Data;
23 typedef typename Model::VectorXs VectorXs;
24 template <class PyClass>
25 40 void visit(PyClass& cl) const {
26 40 cl.def("calc",
27 static_cast<void (Model::*)(
28 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
29 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
30 bp::args("self", "data", "x", "u"),
31 "Compute the next state and cost value.\n\n"
32 "It describes the time-continuous evolution of the multibody system "
33 "without any contact. Additionally it computes the cost value "
34 "associated to this state and control pair.\n"
35 ":param data: free forward-dynamics action data\n"
36 ":param x: time-continuous state vector\n"
37 ":param u: time-continuous control input")
38
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc",
39 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
40 const Eigen::Ref<const VectorXs>&)>(
41 &Model::calc),
42 bp::args("self", "data", "x"))
43
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
44 "calcDiff",
45 static_cast<void (Model::*)(
46 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
47 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
48 bp::args("self", "data", "x", "u"),
49 "Compute the derivatives of the differential multibody system "
50 "(free of contact) and its cost functions.\n\n"
51 "It computes the partial derivatives of the differential multibody "
52 "system and the cost function. It assumes that calc has been run "
53 "first. This function builds a quadratic approximation of the "
54 "action model (i.e. dynamical system and cost function).\n"
55 ":param data: free forward-dynamics action data\n"
56 ":param x: time-continuous state vector\n"
57 ":param u: time-continuous control input\n")
58
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff",
59 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
60 const Eigen::Ref<const VectorXs>&)>(
61 &Model::calcDiff),
62 bp::args("self", "data", "x"))
63
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"),
64 "Create the free forward dynamics differential action data.")
65
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",
66 bp::make_function(&Model::get_pinocchio,
67 40 bp::return_internal_reference<>()),
68 "multibody model (i.e. pinocchio model)")
69
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
70 "actuation",
71 bp::make_function(&Model::get_actuation,
72 40 bp::return_value_policy<bp::return_by_value>()),
73 "actuation model")
74
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
75 "costs",
76 bp::make_function(&Model::get_costs,
77 40 bp::return_value_policy<bp::return_by_value>()),
78 "total cost model")
79
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
80 "constraints",
81 bp::make_function(&Model::get_constraints,
82 40 bp::return_value_policy<bp::return_by_value>()),
83 "constraint model manager")
84
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",
85 bp::make_function(&Model::get_armature,
86 40 bp::return_internal_reference<>()),
87 bp::make_function(&Model::set_armature),
88 "set an armature mechanism in the joints");
89 40 }
90 };
91
92 template <typename Data>
93 struct DifferentialActionDataFreeFwdDynamicsVisitor
94 : public bp::def_visitor<
95 DifferentialActionDataFreeFwdDynamicsVisitor<Data>> {
96 template <class PyClass>
97 40 void visit(PyClass& cl) const {
98
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 cl.add_property(
99 "pinocchio",
100 40 bp::make_getter(&Data::pinocchio, bp::return_internal_reference<>()),
101 "pinocchio data")
102
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("multibody",
103 bp::make_getter(&Data::multibody,
104 40 bp::return_internal_reference<>()),
105 "multibody data")
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_getter(&Data::costs,
109 40 bp::return_value_policy<bp::return_by_value>()),
110 "total cost data")
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_getter(&Data::constraints,
114 40 bp::return_value_policy<bp::return_by_value>()),
115 "constraint data")
116
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
117 "Minv",
118 40 bp::make_getter(&Data::Minv, bp::return_internal_reference<>()),
119 "inverse of the joint-space inertia matrix")
120
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
121 "u_drift",
122 40 bp::make_getter(&Data::u_drift, bp::return_internal_reference<>()),
123 "force-bias vector that accounts for control, Coriolis and "
124 "gravitational effects");
125 40 }
126 };
127
128 #define CROCODDYL_ACTION_MODEL_FREE_FWDDYN_PYTHON_BINDINGS(Scalar) \
129 typedef DifferentialActionModelFreeFwdDynamicsTpl<Scalar> Model; \
130 typedef DifferentialActionModelAbstractTpl<Scalar> ModelBase; \
131 typedef typename Model::StateMultibody State; \
132 typedef typename Model::ActuationModelAbstract Actuation; \
133 typedef typename Model::CostModelSum Costs; \
134 typedef typename Model::ConstraintModelManager Constraints; \
135 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
136 bp::class_<Model, bp::bases<ModelBase>>( \
137 "DifferentialActionModelFreeFwdDynamics", \
138 "Differential action model for free forward dynamics in multibody " \
139 "systems.\n\n" \
140 "This class implements a the dynamics using Articulate Body Algorithm " \
141 "(ABA), or a custom implementation in case of system with armatures. " \
142 "If you want to include the armature, you need to use set_armature(). " \
143 "On the other hand, the stack of cost functions are implemented in " \
144 "CostModelSum().", \
145 bp::init<std::shared_ptr<State>, std::shared_ptr<Actuation>, \
146 std::shared_ptr<Costs>, \
147 bp::optional<std::shared_ptr<Constraints>>>( \
148 bp::args("self", "state", "actuation", "costs", "constraints"), \
149 "Initialize the free forward-dynamics action model.\n\n" \
150 ":param state: multibody state\n" \
151 ":param actuation: abstract actuation model\n" \
152 ":param costs: stack of cost functions\n" \
153 ":param constraints: stack of constraint functions")) \
154 .def(DifferentialActionModelFreeFwdDynamicsVisitor<Model>()) \
155 .def(CastVisitor<Model>()) \
156 .def(PrintableVisitor<Model>()) \
157 .def(CopyableVisitor<Model>());
158
159 #define CROCODDYL_ACTION_DATA_FREE_FWDDYN_PYTHON_BINDINGS(Scalar) \
160 typedef DifferentialActionDataFreeFwdDynamicsTpl<Scalar> Data; \
161 typedef DifferentialActionDataAbstractTpl<Scalar> DataBase; \
162 typedef DifferentialActionModelFreeFwdDynamicsTpl<Scalar> Model; \
163 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
164 bp::class_<Data, bp::bases<DataBase>>( \
165 "DifferentialActionDataFreeFwdDynamics", \
166 "Action data for the free forward dynamics system.", \
167 bp::init<Model*>(bp::args("self", "model"), \
168 "Create free forward-dynamics action data.\n\n" \
169 ":param model: free forward-dynamics action model")) \
170 .def(DifferentialActionDataFreeFwdDynamicsVisitor<Data>()) \
171 .def(CopyableVisitor<Data>());
172
173 10 void exposeDifferentialActionFreeFwdDynamics() {
174
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_ACTION_MODEL_FREE_FWDDYN_PYTHON_BINDINGS)
175
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_FREE_FWDDYN_PYTHON_BINDINGS)
176 10 }
177
178 } // namespace python
179 } // namespace crocoddyl
180