| Directory: | ./ |
|---|---|
| File: | bindings/python/algorithm/expose-contact-inverse-dynamics.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 11 | 14 | 78.6% |
| 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 | 3 | 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 | 3 | return contactInverseDynamics( | |
| 58 | model, data, q, v, a, dt, contact_models, contact_datas, cones, R, constraint_correction, | ||
| 59 | 3 | settings, lambda_guess); | |
| 60 | } | ||
| 61 | #endif // PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
| 62 | |||
| 63 | 69 | void exposeContactInverseDynamics() | |
| 64 | { | ||
| 65 | #ifndef PINOCCHIO_PYTHON_SKIP_ALGORITHM_CONSTRAINED_DYNAMICS | ||
| 66 |
1/2✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
|
65 | bp::def( |
| 67 | "computeContactForces", computeContactImpulses_wrapper, | ||
| 68 |
7/14✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 65 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 65 times.
✗ Branch 21 not taken.
|
130 | (bp::arg("model"), "data", "c_ref", "contact_models", "contact_datas", "cones", "R", |
| 69 |
5/10✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 65 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 65 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 65 times.
✗ Branch 14 not taken.
|
130 | "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 65 times.
✗ Branch 2 not taken.
|
65 | bp::def( |
| 85 | "contactInverseDynamics", contactInverseDynamics_wrapper, | ||
| 86 |
9/18✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 65 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 65 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 65 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 65 times.
✗ Branch 27 not taken.
|
130 | (bp::arg("model"), "data", "q", "v", "a", "dt", "contact_models", "contact_datas", "cones", |
| 87 |
6/12✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 65 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 65 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 65 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 65 times.
✗ Branch 17 not taken.
|
130 | "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 | 69 | } | |
| 106 | } // namespace python | ||
| 107 | } // namespace pinocchio | ||
| 108 |