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