Directory: | ./ |
---|---|
File: | bindings/python/algorithm/expose-constrained-dynamics.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 19 | 24 | 79.2% |
Branches: | 13 | 26 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2020-2021 INRIA | ||
3 | // | ||
4 | |||
5 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" | ||
6 | #include "pinocchio/bindings/python/algorithm/contact-info.hpp" | ||
7 | #include "pinocchio/bindings/python/algorithm/proximal.hpp" | ||
8 | #include "pinocchio/bindings/python/algorithm/contact-cholesky.hpp" | ||
9 | |||
10 | #include "pinocchio/bindings/python/utils/std-vector.hpp" | ||
11 | #include "pinocchio/bindings/python/utils/registration.hpp" | ||
12 | |||
13 | #include "pinocchio/algorithm/constrained-dynamics.hpp" | ||
14 | |||
15 | namespace bp = boost::python; | ||
16 | |||
17 | namespace pinocchio | ||
18 | { | ||
19 | namespace python | ||
20 | { | ||
21 | |||
22 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintModel) | ||
23 | RigidConstraintModelVector; | ||
24 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintData) | ||
25 | RigidConstraintDataVector; | ||
26 | |||
27 | #ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
28 | |||
29 | ✗ | static const context::VectorXs constraintDynamics_proxy( | |
30 | const context::Model & model, | ||
31 | context::Data & data, | ||
32 | const context::VectorXs & q, | ||
33 | const context::VectorXs & v, | ||
34 | const context::VectorXs & tau, | ||
35 | const RigidConstraintModelVector & contact_models, | ||
36 | RigidConstraintDataVector & contact_datas, | ||
37 | context::ProximalSettings & prox_settings) | ||
38 | { | ||
39 | ✗ | return constraintDynamics( | |
40 | ✗ | model, data, q, v, tau, contact_models, contact_datas, prox_settings); | |
41 | } | ||
42 | |||
43 | ✗ | static const context::VectorXs constraintDynamics_proxy_default( | |
44 | const context::Model & model, | ||
45 | context::Data & data, | ||
46 | const context::VectorXs & q, | ||
47 | const context::VectorXs & v, | ||
48 | const context::VectorXs & tau, | ||
49 | const RigidConstraintModelVector & contact_models, | ||
50 | RigidConstraintDataVector & contact_datas) | ||
51 | { | ||
52 | ✗ | return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas); | |
53 | } | ||
54 | |||
55 | #endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
56 | |||
57 | 20 | void exposeConstraintDynamics() | |
58 | { | ||
59 | using namespace Eigen; | ||
60 | |||
61 | // Expose type of contacts | ||
62 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | if (!register_symbolic_link_to_registered_type<ContactType>()) |
63 | { | ||
64 | 40 | bp::enum_<ContactType>("ContactType") | |
65 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | .value("CONTACT_3D", CONTACT_3D) |
66 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | .value("CONTACT_6D", CONTACT_6D) |
67 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | .value("CONTACT_UNDEFINED", CONTACT_UNDEFINED); |
68 | } | ||
69 | |||
70 | 20 | ProximalSettingsPythonVisitor<context::ProximalSettings>::expose(); | |
71 | |||
72 | 20 | RigidConstraintModelPythonVisitor<context::RigidConstraintModel>::expose(); | |
73 | 20 | RigidConstraintDataPythonVisitor<context::RigidConstraintData>::expose(); | |
74 | |||
75 |
3/6✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20 times.
✗ Branch 10 not taken.
|
20 | StdVectorPythonVisitor<RigidConstraintModelVector>::expose("StdVec_RigidConstraintModel"); |
76 | |||
77 |
3/6✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20 times.
✗ Branch 10 not taken.
|
20 | StdVectorPythonVisitor<RigidConstraintDataVector>::expose("StdVec_RigidConstraintData"); |
78 | |||
79 | #ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
80 | 20 | ContactCholeskyDecompositionPythonVisitor<context::ContactCholeskyDecomposition>::expose(); | |
81 | |||
82 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
83 | "initConstraintDynamics", | ||
84 | &initConstraintDynamics< | ||
85 | context::Scalar, context::Options, JointCollectionDefaultTpl, | ||
86 | typename RigidConstraintModelVector::allocator_type>, | ||
87 | 40 | bp::args("model", "data", "contact_models"), | |
88 | "This function allows to allocate the memory before hand for contact dynamics algorithms.\n" | ||
89 | "This allows to avoid online memory allocation when running these algorithms."); | ||
90 | |||
91 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
92 | "constraintDynamics", constraintDynamics_proxy, | ||
93 | 40 | bp::args( | |
94 | "model", "data", "q", "v", "tau", "contact_models", "contact_datas", "prox_settings"), | ||
95 | "Computes the forward dynamics with contact constraints according to a given list of " | ||
96 | "Contact information.\n" | ||
97 | "When using constraintDynamics for the first time, you should call first " | ||
98 | "initConstraintDynamics to initialize the internal memory used in the algorithm.\n" | ||
99 | "This function returns joint acceleration of the system. The contact forces are " | ||
100 | "stored in the list data.contact_forces."); | ||
101 | |||
102 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
103 | "constraintDynamics", constraintDynamics_proxy_default, | ||
104 | 40 | bp::args("model", "data", "q", "v", "tau", "contact_models", "contact_datas"), | |
105 | "Computes the forward dynamics with contact constraints according to a given list of " | ||
106 | "Contact information.\n" | ||
107 | "When using constraintDynamics for the first time, you should call first " | ||
108 | "initConstraintDynamics to initialize the internal memory used in the algorithm.\n" | ||
109 | "This function returns joint acceleration of the system. The contact forces are " | ||
110 | "stored in the list data.contact_forces."); | ||
111 | |||
112 | #endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
113 | 20 | } | |
114 | } // namespace python | ||
115 | } // namespace pinocchio | ||
116 |