Directory: | ./ |
---|---|
File: | bindings/python/algorithm/expose-contact-inverse-dynamics.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 8 | 14 | 57.1% |
Branches: | 29 | 58 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2024 INRIA | ||
3 | // | ||
4 | |||
5 | #define BOOST_PYTHON_MAX_ARITY 24 | ||
6 | |||
7 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" | ||
8 | #include "pinocchio/bindings/python/utils/std-vector.hpp" | ||
9 | #include "pinocchio/algorithm/contact-inverse-dynamics.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | namespace python | ||
14 | { | ||
15 | |||
16 | #ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
17 | typedef context::Scalar Scalar; | ||
18 | typedef context::VectorXs VectorXs; | ||
19 | typedef const Eigen::Ref<const VectorXs> ConstRefVectorXs; | ||
20 | enum | ||
21 | { | ||
22 | Options = context::Options | ||
23 | }; | ||
24 | |||
25 | ✗ | static ConstRefVectorXs computeContactImpulses_wrapper( | |
26 | const ModelTpl<Scalar, Options, JointCollectionDefaultTpl> & model, | ||
27 | DataTpl<Scalar, Options, JointCollectionDefaultTpl> & data, | ||
28 | const ConstRefVectorXs & c_ref, | ||
29 | const context::RigidConstraintModelVector & contact_models, | ||
30 | context::RigidConstraintDataVector & contact_datas, | ||
31 | const context::CoulombFrictionConeVector & cones, | ||
32 | const ConstRefVectorXs & R, | ||
33 | const ConstRefVectorXs & constraint_correction, | ||
34 | ProximalSettingsTpl<Scalar> & settings, | ||
35 | const boost::optional<ConstRefVectorXs> & lambda_guess = boost::none) | ||
36 | { | ||
37 | ✗ | return computeContactImpulses( | |
38 | model, data, c_ref, contact_models, contact_datas, cones, R, constraint_correction, | ||
39 | ✗ | settings, lambda_guess); | |
40 | } | ||
41 | |||
42 | ✗ | static ConstRefVectorXs contactInverseDynamics_wrapper( | |
43 | const ModelTpl<Scalar, Options, JointCollectionDefaultTpl> & model, | ||
44 | DataTpl<Scalar, Options, JointCollectionDefaultTpl> & data, | ||
45 | ConstRefVectorXs & q, | ||
46 | ConstRefVectorXs & v, | ||
47 | ConstRefVectorXs & a, | ||
48 | Scalar dt, | ||
49 | const context::RigidConstraintModelVector & contact_models, | ||
50 | context::RigidConstraintDataVector & contact_datas, | ||
51 | const context::CoulombFrictionConeVector & cones, | ||
52 | ConstRefVectorXs & R, | ||
53 | ConstRefVectorXs & constraint_correction, | ||
54 | ProximalSettingsTpl<Scalar> & settings, | ||
55 | const boost::optional<ConstRefVectorXs> & lambda_guess = boost::none) | ||
56 | { | ||
57 | ✗ | return contactInverseDynamics( | |
58 | model, data, q, v, a, dt, contact_models, contact_datas, cones, R, constraint_correction, | ||
59 | ✗ | settings, lambda_guess); | |
60 | } | ||
61 | #endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
62 | |||
63 | 20 | void exposeContactInverseDynamics() | |
64 | { | ||
65 | #ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
66 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
67 | "computeContactForces", computeContactImpulses_wrapper, | ||
68 |
7/14✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20 times.
✗ Branch 21 not taken.
|
40 | (bp::arg("model"), "data", "c_ref", "contact_models", "contact_datas", "cones", "R", |
69 |
5/10✓ 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.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
|
40 | "constraint_correction", bp::arg("settings"), bp::arg("lambda_guess") = boost::none), |
70 | "Compute the inverse dynamics with frictional contacts, store the result in Data and " | ||
71 | "return it.\n\n" | ||
72 | "Parameters:\n" | ||
73 | "\tmodel: model of the kinematic tree\n" | ||
74 | "\tdata: data related to the model\n" | ||
75 | "\tc_ref: the reference velocity of contact points\n" | ||
76 | "\tcontact_models: list of contact models\n" | ||
77 | "\tcontact_datas: list of contact datas\n" | ||
78 | "\tcones: list of friction cones\n" | ||
79 | "\tR: vector representing the diagonal of the compliance matrix\n" | ||
80 | "\tconstraint_correction: vector representing the constraint correction\n" | ||
81 | "\tsettings: the settings of the proximal algorithm\n" | ||
82 | "\tlambda_guess: initial guess for contact forces\n"); | ||
83 | |||
84 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
85 | "contactInverseDynamics", contactInverseDynamics_wrapper, | ||
86 |
9/18✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 20 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 20 times.
✗ Branch 27 not taken.
|
40 | (bp::arg("model"), "data", "q", "v", "a", "dt", "contact_models", "contact_datas", "cones", |
87 |
6/12✓ 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.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
|
40 | "R", "constraint_correction", bp::arg("settings"), bp::arg("lambda_guess") = boost::none), |
88 | "Compute the inverse dynamics with frictional contacts, store the result in Data and " | ||
89 | "return it.\n\n" | ||
90 | "Parameters:\n" | ||
91 | "\tmodel: model of the kinematic tree\n" | ||
92 | "\tdata: data related to the model\n" | ||
93 | "\tq: the joint configuration vector (size model.nq)\n" | ||
94 | "\tv: the joint velocity vector (size model.nv)\n" | ||
95 | "\ta: the joint acceleration vector (size model.nv)\n" | ||
96 | "\tdt: the time step\n" | ||
97 | "\tcontact_models: list of contact models\n" | ||
98 | "\tcontact_datas: list of contact datas\n" | ||
99 | "\tcones: list of friction cones\n" | ||
100 | "\tR: vector representing the diagonal of the compliance matrix\n" | ||
101 | "\tconstraint_correction: vector representing the constraint correction\n" | ||
102 | "\tsettings: the settings of the proximal algorithm\n" | ||
103 | "\tlambda_guess: initial guess for contact forces\n"); | ||
104 | #endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
105 | 20 | } | |
106 | } // namespace python | ||
107 | } // namespace pinocchio | ||
108 |