1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, 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/contacts/multiple-contacts.hpp" |
11 |
|
|
|
12 |
|
|
#include <functional> |
13 |
|
|
#include <map> |
14 |
|
|
#include <memory> |
15 |
|
|
|
16 |
|
|
#include "python/crocoddyl/multibody/multibody.hpp" |
17 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
18 |
|
|
#include "python/crocoddyl/utils/map-converter.hpp" |
19 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
20 |
|
|
#include "python/crocoddyl/utils/set-converter.hpp" |
21 |
|
|
|
22 |
|
|
namespace crocoddyl { |
23 |
|
|
namespace python { |
24 |
|
|
|
25 |
✓✗ |
46 |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ContactModelMultiple_addContact_wrap, |
26 |
|
|
ContactModelMultiple::addContact, 2, 3) |
27 |
|
|
|
28 |
|
10 |
void exposeContactMultiple() { |
29 |
|
|
// Register custom converters between std::map and Python dict |
30 |
|
|
typedef boost::shared_ptr<ContactItem> ContactItemPtr; |
31 |
|
|
typedef boost::shared_ptr<ContactDataAbstract> ContactDataPtr; |
32 |
|
|
StdMapPythonVisitor< |
33 |
|
|
std::string, ContactItemPtr, std::less<std::string>, |
34 |
|
|
std::allocator<std::pair<const std::string, ContactItemPtr>>, |
35 |
✓✗✓✗
✓✗ |
10 |
true>::expose("StdMap_ContactItem"); |
36 |
|
|
StdMapPythonVisitor< |
37 |
|
|
std::string, ContactDataPtr, std::less<std::string>, |
38 |
|
|
std::allocator<std::pair<const std::string, ContactDataPtr>>, |
39 |
✓✗✓✗
✓✗ |
10 |
true>::expose("StdMap_ContactData"); |
40 |
|
|
|
41 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ContactItem>>(); |
42 |
|
|
|
43 |
✓✗ |
10 |
bp::class_<ContactItem>( |
44 |
|
|
"ContactItem", "Describe a contact item.\n\n", |
45 |
✓✗ |
10 |
bp::init<std::string, boost::shared_ptr<ContactModelAbstract>, |
46 |
|
|
bp::optional<bool>>( |
47 |
|
20 |
bp::args("self", "name", "contact", "active"), |
48 |
|
|
"Initialize the contact item.\n\n" |
49 |
|
|
":param name: contact name\n" |
50 |
|
|
":param contact: contact model\n" |
51 |
|
|
":param active: True if the contact is activated (default true)")) |
52 |
✓✗ |
10 |
.def_readwrite("name", &ContactItem::name, "contact name") |
53 |
|
|
.add_property( |
54 |
|
|
"contact", |
55 |
✓✗ |
10 |
bp::make_getter(&ContactItem::contact, |
56 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
57 |
✓✗ |
10 |
"contact model") |
58 |
✓✗ |
10 |
.def_readwrite("active", &ContactItem::active, "contact status") |
59 |
✓✗ |
10 |
.def(CopyableVisitor<ContactItem>()) |
60 |
✓✗ |
10 |
.def(PrintableVisitor<ContactItem>()); |
61 |
|
|
|
62 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ContactModelMultiple>>(); |
63 |
|
|
|
64 |
✓✗ |
10 |
bp::class_<ContactModelMultiple>( |
65 |
|
|
"ContactModelMultiple", |
66 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateMultibody>, bp::optional<std::size_t>>( |
67 |
|
20 |
bp::args("self", "state", "nu"), |
68 |
|
|
"Initialize the multiple contact model.\n\n" |
69 |
|
|
":param state: state of the multibody system\n" |
70 |
|
|
":param nu: dimension of control vector")) |
71 |
|
|
.def( |
72 |
|
|
"addContact", &ContactModelMultiple::addContact, |
73 |
✓✗ |
10 |
ContactModelMultiple_addContact_wrap( |
74 |
✓✗ |
20 |
bp::args("self", "name", "contact", "active"), |
75 |
|
|
"Add a contact item.\n\n" |
76 |
|
|
":param name: contact name\n" |
77 |
|
|
":param contact: contact model\n" |
78 |
✓✗ |
10 |
":param active: True if the contact is activated (default true)")) |
79 |
|
|
.def("removeContact", &ContactModelMultiple::removeContact, |
80 |
✓✗ |
20 |
bp::args("self", "name"), |
81 |
|
|
"Remove a contact item.\n\n" |
82 |
✓✗ |
10 |
":param name: contact name") |
83 |
|
|
.def("changeContactStatus", &ContactModelMultiple::changeContactStatus, |
84 |
✓✗ |
20 |
bp::args("self", "name", "active"), |
85 |
|
|
"Change the contact status.\n\n" |
86 |
|
|
":param name: contact name\n" |
87 |
|
|
":param active: contact status (true for active and false for " |
88 |
✓✗ |
10 |
"inactive)") |
89 |
✓✗ |
20 |
.def("calc", &ContactModelMultiple::calc, bp::args("self", "data", "x"), |
90 |
|
|
"Compute the contact Jacobian and contact acceleration.\n\n" |
91 |
|
|
"The rigid contact model throught acceleration-base holonomic " |
92 |
|
|
"constraint\n" |
93 |
|
|
"of the contact frame placement.\n" |
94 |
|
|
":param data: contact data\n" |
95 |
✓✗ |
10 |
":param x: state point (dim. state.nx)") |
96 |
|
|
.def("calcDiff", &ContactModelMultiple::calcDiff, |
97 |
✓✗ |
20 |
bp::args("self", "data", "x"), |
98 |
|
|
"Compute the derivatives of the contact holonomic constraint.\n\n" |
99 |
|
|
"The rigid contact model throught acceleration-base holonomic " |
100 |
|
|
"constraint\n" |
101 |
|
|
"of the contact frame placement.\n" |
102 |
|
|
"It assumes that calc has been run first.\n" |
103 |
|
|
":param data: contact data\n" |
104 |
✓✗ |
10 |
":param x: state point (dim. state.nx)") |
105 |
|
|
.def("updateAcceleration", &ContactModelMultiple::updateAcceleration, |
106 |
✓✗ |
20 |
bp::args("self", "data", "dv"), |
107 |
|
|
"Update the constrained system acceleration.\n\n" |
108 |
|
|
":param data: contact data\n" |
109 |
✓✗ |
10 |
":param dv: constrained acceleration (dimension nv)") |
110 |
|
|
.def("updateForce", &ContactModelMultiple::updateForce, |
111 |
✓✗ |
20 |
bp::args("self", "data", "force"), |
112 |
|
|
"Update the spatial force in frame coordinate.\n\n" |
113 |
|
|
":param data: contact data\n" |
114 |
✓✗ |
10 |
":param force: force vector (dimension nc)") |
115 |
|
|
.def("updateAccelerationDiff", |
116 |
|
|
&ContactModelMultiple::updateAccelerationDiff, |
117 |
✓✗ |
20 |
bp::args("self", "data", "ddv_dx"), |
118 |
|
|
"Update the Jacobian of the constrained system acceleration.\n\n" |
119 |
|
|
":param data: contact data\n" |
120 |
|
|
":param ddv_dx: Jacobian of the system acceleration in generalized " |
121 |
✓✗ |
10 |
"coordinates (dimension nv*ndx)") |
122 |
|
|
.def("updateForceDiff", &ContactModelMultiple::updateForceDiff, |
123 |
✓✗ |
20 |
bp::args("self", "data", "df_dx", "df_du"), |
124 |
|
|
"Update the Jacobians of the spatial force defined in frame " |
125 |
|
|
"coordinates.\n\n" |
126 |
|
|
":param data: contact data\n" |
127 |
|
|
":param df_dx: Jacobian of the force with respect to the state " |
128 |
|
|
"(dimension nc*ndx)\n" |
129 |
|
|
":param df_du: Jacobian of the force with respect to the control " |
130 |
✓✗ |
10 |
"(dimension nc*nu)") |
131 |
|
|
.def("updateRneaDiff", &ContactModelMultiple::updateRneaDiff, |
132 |
✓✗ |
20 |
bp::args("self", "data", "pinocchio"), |
133 |
|
|
"Update the RNEA derivative dtau_dq by by adding the skew term " |
134 |
|
|
"(necessary for contacts expressed in\n" |
135 |
|
|
"LOCAL_WORLD_ALIGNED / WORLD).\n\n" |
136 |
|
|
":param data: contact data\n" |
137 |
✓✗ |
10 |
":param pinocchio: Pinocchio data") |
138 |
|
|
.def("createData", &ContactModelMultiple::createData, |
139 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
140 |
✓✗ |
20 |
bp::args("self", "data"), |
141 |
|
|
"Create the total contact data.\n\n" |
142 |
|
|
":param data: Pinocchio data\n" |
143 |
✓✗ |
10 |
":return total contact data.") |
144 |
|
|
.add_property( |
145 |
|
|
"contacts", |
146 |
✓✗ |
10 |
bp::make_function(&ContactModelMultiple::get_contacts, |
147 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
148 |
✓✗ |
10 |
"stack of contacts") |
149 |
|
|
.add_property( |
150 |
|
|
"state", |
151 |
✓✗ |
10 |
bp::make_function(&ContactModelMultiple::get_state, |
152 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
153 |
✓✗ |
10 |
"state of the multibody system") |
154 |
✓✗ |
20 |
.add_property("nc", bp::make_function(&ContactModelMultiple::get_nc), |
155 |
✓✗ |
10 |
"dimension of the active contact vector") |
156 |
|
|
.add_property("nc_total", |
157 |
✓✗ |
20 |
bp::make_function(&ContactModelMultiple::get_nc_total), |
158 |
✓✗ |
10 |
"dimension of the total contact vector") |
159 |
✓✗ |
20 |
.add_property("nu", bp::make_function(&ContactModelMultiple::get_nu), |
160 |
✓✗ |
10 |
"dimension of control vector") |
161 |
|
|
.add_property( |
162 |
|
|
"active_set", |
163 |
✓✗ |
10 |
bp::make_function(&ContactModelMultiple::get_active_set, |
164 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
165 |
✓✗ |
10 |
"names of the active set of contact items") |
166 |
|
|
.add_property( |
167 |
|
|
"inactive_set", |
168 |
✓✗ |
10 |
bp::make_function(&ContactModelMultiple::get_inactive_set, |
169 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
170 |
✓✗ |
10 |
"names of the inactive set of contact items") |
171 |
|
|
.def("getContactStatus", &ContactModelMultiple::getContactStatus, |
172 |
✓✗ |
20 |
bp::args("self", "name"), |
173 |
|
|
"Return the contact status of a given contact name.\n\n" |
174 |
✓✗ |
10 |
":param name: contact name") |
175 |
|
|
.add_property( |
176 |
|
|
"computeAllContacts", |
177 |
✓✗ |
10 |
bp::make_function(&ContactModelMultiple::getComputeAllContacts), |
178 |
✓✗ |
20 |
bp::make_function(&ContactModelMultiple::setComputeAllContacts), |
179 |
|
|
"type of contact computation (True for all contacts and False for " |
180 |
✓✗ |
10 |
"active contacts)") |
181 |
✓✗ |
10 |
.def(CopyableVisitor<ContactModelMultiple>()) |
182 |
✓✗ |
10 |
.def(PrintableVisitor<ContactModelMultiple>()); |
183 |
|
|
|
184 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ContactDataMultiple>>(); |
185 |
|
|
|
186 |
✓✗ |
10 |
bp::class_<ContactDataMultiple>( |
187 |
|
|
"ContactDataMultiple", "Data class for multiple contacts.\n\n", |
188 |
✓✗ |
10 |
bp::init<ContactModelMultiple*, pinocchio::Data*>( |
189 |
✓✗ |
10 |
bp::args("self", "model", "data"), |
190 |
|
|
"Create multicontact data.\n\n" |
191 |
|
|
":param model: multicontact model\n" |
192 |
|
10 |
":param data: Pinocchio data")[bp::with_custodian_and_ward< |
193 |
✓✗ |
20 |
1, 2, bp::with_custodian_and_ward<1, 3>>()]) |
194 |
|
|
.add_property("Jc", |
195 |
✓✗ |
10 |
bp::make_getter(&ContactDataMultiple::Jc, |
196 |
|
|
bp::return_internal_reference<>()), |
197 |
✓✗ |
20 |
bp::make_setter(&ContactDataMultiple::Jc), |
198 |
|
|
"contact Jacobian in frame coordinate (memory defined for " |
199 |
✓✗ |
10 |
"active and inactive contacts)") |
200 |
|
|
.add_property("a0", |
201 |
✓✗ |
10 |
bp::make_getter(&ContactDataMultiple::a0, |
202 |
|
|
bp::return_internal_reference<>()), |
203 |
✓✗ |
20 |
bp::make_setter(&ContactDataMultiple::a0), |
204 |
|
|
"desired spatial contact acceleration in frame coordinate " |
205 |
✓✗ |
10 |
"(memory defined for active and inactive contacts)") |
206 |
|
|
.add_property("da0_dx", |
207 |
✓✗ |
10 |
bp::make_getter(&ContactDataMultiple::da0_dx, |
208 |
|
|
bp::return_internal_reference<>()), |
209 |
✓✗ |
20 |
bp::make_setter(&ContactDataMultiple::da0_dx), |
210 |
|
|
"Jacobian of the desired spatial contact acceleration in " |
211 |
|
|
"frame coordinate (memory defined for active and " |
212 |
✓✗ |
10 |
"inactive contacts)") |
213 |
|
|
.add_property( |
214 |
|
|
"dv", |
215 |
✓✗ |
10 |
bp::make_getter(&ContactDataMultiple::dv, |
216 |
|
|
bp::return_internal_reference<>()), |
217 |
✓✗ |
20 |
bp::make_setter(&ContactDataMultiple::dv), |
218 |
✓✗ |
10 |
"constrained system acceleration in generalized coordinates") |
219 |
|
|
.add_property("ddv_dx", |
220 |
✓✗ |
10 |
bp::make_getter(&ContactDataMultiple::ddv_dx, |
221 |
|
|
bp::return_internal_reference<>()), |
222 |
✓✗ |
20 |
bp::make_setter(&ContactDataMultiple::ddv_dx), |
223 |
|
|
"Jacobian of the constrained system acceleration in " |
224 |
✓✗ |
10 |
"generalized coordinates") |
225 |
|
|
.add_property( |
226 |
|
|
"contacts", |
227 |
✓✗ |
10 |
bp::make_getter(&ContactDataMultiple::contacts, |
228 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
229 |
✓✗ |
10 |
"stack of contacts data") |
230 |
|
|
.def_readwrite("fext", &ContactDataMultiple::fext, |
231 |
✓✗ |
10 |
"external spatial forces in join coordinates") |
232 |
✓✗ |
10 |
.def(CopyableVisitor<ContactDataMultiple>()); |
233 |
|
10 |
} |
234 |
|
|
|
235 |
|
|
} // namespace python |
236 |
|
|
} // namespace crocoddyl |