GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/multibody/contacts/contact-2d.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 33 34 97.1%
Branches: 62 124 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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/contacts/contact-2d.hpp"
11
12 #include "python/crocoddyl/multibody/multibody.hpp"
13
14 namespace crocoddyl {
15 namespace python {
16
17 template <typename Model>
18 struct ContactModel2DVisitor
19 : public bp::def_visitor<ContactModel2DVisitor<Model>> {
20 typedef typename Model::StateMultibody State;
21 typedef typename Model::Vector2s Vector2s;
22 template <class PyClass>
23 40 void visit(PyClass& cl) const {
24
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>, pinocchio::FrameIndex, Vector2s,
25 bp::optional<Vector2s>>(
26 bp::args("self", "state", "id", "xref", "gains"),
27 "Initialize the contact model.\n\n"
28 ":param state: state of the multibody system\n"
29 ":param id: reference frame id of the contact\n"
30 ":param xref: contact position used for the Baumgarte "
31 "stabilization\n"
32 ":param gains: gains of the contact model (default "
33 "np.matrix([0.,0.]))"))
34
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def("calc", &Model::calc, bp::args("self", "data", "x"),
35 "Compute the 2D contact Jacobian and drift.\n\n"
36 "The rigid contact model throught acceleration-base holonomic "
37 "constraint of the contact frame placement.\n"
38 ":param data: contact data\n"
39 ":param x: state point (dim. state.nx)")
40
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff", &Model::calcDiff, bp::args("self", "data", "x"),
41 "Compute the derivatives of the 2D contact holonomic "
42 "constraint.\n\n"
43 "The rigid contact model throught acceleration-base holonomic "
44 "constraint of the contact frame placement. It assumes that calc "
45 "has been run first.\n"
46 ":param data: cost data\n"
47 ":param x: state point (dim. state.nx)")
48
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("updateForce", &Model::updateForce,
49 bp::args("self", "data", "force"),
50 "Convert the force into a stack of spatial forces.\n\n"
51 ":param data: cost data\n"
52 ":param force: force vector (dimension 2)")
53
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,
54 ✗ bp::with_custodian_and_ward_postcall<0, 2>(),
55 bp::args("self", "data"),
56 "Create the 2D contact data.\n\n"
57 "Each contact model has its own data that needs to be allocated. "
58 "This function returns the allocated data for a predefined cost.\n"
59 ":param data: Pinocchio data\n"
60 ":return contact data.")
61
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .add_property("reference",
62 bp::make_function(&Model::get_reference,
63 40 bp::return_internal_reference<>()),
64 &Model::set_reference, "reference contact translation")
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.
 80 .add_property(
66 "gains",
67 bp::make_function(&Model::get_gains,
68 40 bp::return_value_policy<bp::return_by_value>()),
69 "contact gains");
70 40 }
71 };
72
73 template <typename Data>
74 struct ContactData2DVisitor
75 : public bp::def_visitor<ContactData2DVisitor<Data>> {
76 template <class PyClass>
77 40 void visit(PyClass& cl) const {
78
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 cl.add_property(
79 "v",
80 bp::make_getter(&Data::v,
81 40 bp::return_value_policy<bp::return_by_value>()),
82 "spatial velocity of the contact body")
83
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
84 "a",
85 bp::make_getter(&Data::a,
86 40 bp::return_value_policy<bp::return_by_value>()),
87 "spatial acceleration of the contact body")
88
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
89 "fJf",
90 40 bp::make_getter(&Data::fJf, bp::return_internal_reference<>()),
91 "local Jacobian of the contact frame")
92
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("v_partial_dq",
93 bp::make_getter(&Data::v_partial_dq,
94 40 bp::return_internal_reference<>()),
95 "Jacobian of the spatial body velocity")
96
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("a_partial_dq",
97 bp::make_getter(&Data::a_partial_dq,
98 40 bp::return_internal_reference<>()),
99 "Jacobian of the spatial body acceleration")
100
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("a_partial_dv",
101 bp::make_getter(&Data::a_partial_dv,
102 40 bp::return_internal_reference<>()),
103 "Jacobian of the spatial body acceleration")
104
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property("a_partial_da",
105 bp::make_getter(&Data::a_partial_da,
106 40 bp::return_internal_reference<>()),
107 "Jacobian of the spatial body acceleration")
108
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
109 "oRf",
110 40 bp::make_getter(&Data::oRf, bp::return_internal_reference<>()),
111 "Rotation matrix of the contact body expressed in the world frame");
112 40 }
113 };
114
115 #define CROCODDYL_CONTACT_MODEL_2D_PYTHON_BINDINGS(Scalar) \
116 typedef ContactModel2DTpl<Scalar> Model; \
117 typedef ContactModelAbstractTpl<Scalar> ModelBase; \
118 typedef Model::StateMultibody State; \
119 typedef Model::Vector2s Vector2s; \
120 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
121 bp::class_<Model, bp::bases<ModelBase>>( \
122 "ContactModel2D", \
123 "Rigid 2D contact model.\n\n" \
124 "It defines a rigid 2D contact models (point contact) based on " \
125 "acceleration-based holonomic constraints, in x,z directions. The calc " \
126 "and calcDiff functions compute the contact Jacobian and drift " \
127 "(holonomic constraint) or the derivatives of the holonomic " \
128 "constraint, respectively.", \
129 bp::init<std::shared_ptr<State>, pinocchio::FrameIndex, Vector2s, \
130 std::size_t, bp::optional<Vector2s>>( \
131 bp::args("self", "state", "id", "xref", "nu", "gains"), \
132 "Initialize the contact model.\n\n" \
133 ":param state: state of the multibody system\n" \
134 ":param id: reference frame id of the contact\n" \
135 ":param xref: contact position used for the Baumgarte " \
136 "stabilization\n" \
137 ":param nu: dimension of control vector\n" \
138 ":param gains: gains of the contact model (default " \
139 "np.matrix([0.,0.]))")) \
140 .def(ContactModel2DVisitor<Model>()) \
141 .def(CastVisitor<Model>()) \
142 .def(PrintableVisitor<Model>()) \
143 .def(CopyableVisitor<Model>());
144
145 #define CROCODDYL_CONTACT_DATA_2D_PYTHON_BINDINGS(Scalar) \
146 typedef ContactData2DTpl<Scalar> Data; \
147 typedef ContactDataAbstractTpl<Scalar> DataBase; \
148 typedef ContactModel2DTpl<Scalar> Model; \
149 typedef pinocchio::DataTpl<Scalar> PinocchioData; \
150 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
151 bp::class_<Data, bp::bases<DataBase>>( \
152 "ContactData2D", "Data for 2D contact.\n\n", \
153 bp::init<Model*, PinocchioData*>( \
154 bp::args("self", "model", "data"), \
155 "Create 2D contact data.\n\n" \
156 ":param model: 2D contact model\n" \
157 ":param data: Pinocchio data")[bp::with_custodian_and_ward< \
158 1, 2, bp::with_custodian_and_ward<1, 3>>()]) \
159 .def(ContactData2DVisitor<Data>()) \
160 .def(CopyableVisitor<Data>());
161
162 10 void exposeContact2D() {
163 #pragma GCC diagnostic push // TODO: Remove once the deprecated FrameXX has
164 // been removed in a future release
165 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
166
167
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_CONTACT_MODEL_2D_PYTHON_BINDINGS)
168
169 #pragma GCC diagnostic pop
170
171
15/30
✓ 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 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 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.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 10 times.
✗ Branch 51 not taken.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_CONTACT_DATA_2D_PYTHON_BINDINGS)
172 10 }
173
174 } // namespace python
175 } // namespace crocoddyl
176