GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: bindings/python/algorithm/expose-contact-dynamics.cpp Lines: 25 39 64.1 %
Date: 2024-01-23 21:41:47 Branches: 11 34 32.4 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2020 CNRS, INRIA
3
//
4
5
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
6
#include "pinocchio/algorithm/contact-dynamics.hpp"
7
8
namespace pinocchio
9
{
10
  namespace python
11
  {
12
13
1
    static const Eigen::VectorXd forwardDynamics_proxy(const Model & model,
14
                                                       Data & data,
15
                                                       const Eigen::VectorXd & q,
16
                                                       const Eigen::VectorXd & v,
17
                                                       const Eigen::VectorXd & tau,
18
                                                       const Eigen::MatrixXd & J,
19
                                                       const Eigen::VectorXd & gamma,
20
                                                       const double inv_damping = 0.0)
21
    {
22
1
      return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
23
    }
24
25
40
    BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 8)
26
27
    static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model & model,
28
                                                            Data & data,
29
                                                            const Eigen::VectorXd & tau,
30
                                                            const Eigen::MatrixXd & J,
31
                                                            const Eigen::VectorXd & gamma,
32
                                                            const double inv_damping = 0.0)
33
    {
34
      return forwardDynamics(model, data, tau, J, gamma, inv_damping);
35
    }
36
37
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads_no_q, forwardDynamics_proxy_no_q, 5, 6)
38
39
    static const Eigen::VectorXd impulseDynamics_proxy(const Model & model,
40
                                                       Data & data,
41
                                                       const Eigen::VectorXd & q,
42
                                                       const Eigen::VectorXd & v_before,
43
                                                       const Eigen::MatrixXd & J,
44
                                                       const double r_coeff = 0.,
45
                                                       const double inv_damping = 0.)
46
    {
47
      return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
48
    }
49
50
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7)
51
52
    static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model & model,
53
                                                            Data & data,
54
                                                            const Eigen::VectorXd & v_before,
55
                                                            const Eigen::MatrixXd & J,
56
                                                            const double r_coeff = 0.,
57
                                                            const double inv_damping = 0.)
58
    {
59
      return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
60
    }
61
62
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6)
63
64
    static Eigen::MatrixXd computeKKTContactDynamicMatrixInverse_proxy(const Model & model,
65
                                                                       Data & data,
66
                                                                       const Eigen::VectorXd & q,
67
                                                                       const Eigen::MatrixXd & J,
68
                                                                       const double mu = 0)
69
    {
70
      Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
71
      computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv, mu);
72
      return KKTMatrix_inv;
73
    }
74
75
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload,
76
                                    computeKKTContactDynamicMatrixInverse_proxy, 4, 5)
77
78
    static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
79
                                                                         Data & data,
80
                                                                         const Eigen::MatrixXd & J)
81
    {
82
      Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
83
      getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
84
      return KKTMatrix_inv;
85
    }
86
87
19
    void exposeDynamics()
88
    {
89
      using namespace Eigen;
90
91
19
      bp::def("forwardDynamics",
92
              &forwardDynamics_proxy,
93
19
              forwardDynamics_overloads(
94
38
              bp::args("Model","Data","q","v","tau","J","gamma","damping"),
95
              "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c."
96
              " Internally, pinocchio.computeAllTerms is called."
97
              ));
98
99
19
      bp::def("forwardDynamics",
100
              &forwardDynamics_proxy_no_q,
101
19
              forwardDynamics_overloads_no_q(
102
38
              bp::args("Model","Data","tau","J","gamma","damping"),
103
              "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c."
104
              " Assumes pinocchio.computeAllTerms has been called."
105
              ));
106
107
19
      bp::def("impulseDynamics",
108
              &impulseDynamics_proxy,
109
19
              impulseDynamics_overloads(
110
38
              bp::args("Model","Data","q","v_before","J","r_coeff","damping"),
111
              "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c."
112
              " Internally, pinocchio.crba is called."
113
              ));
114
115
19
      bp::def("impulseDynamics",
116
              &impulseDynamics_proxy_no_q,
117
19
              impulseDynamics_overloads_no_q(
118
38
              bp::args("Model","Data","v_before","J","r_coeff","damping"),
119
              "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c."
120
              " Assumes pinocchio.crba has been called."
121
              ));
122
123
19
      bp::def("computeKKTContactDynamicMatrixInverse",
124
              computeKKTContactDynamicMatrixInverse_proxy,
125
38
              computeKKTContactDynamicMatrixInverse_overload(bp::args("model","data","q","J","damping"),
126
              "Computes the inverse of the constraint matrix [[M J^T], [J 0]]."));
127
128
19
      bp::def("getKKTContactDynamicMatrixInverse",
129
              getKKTContactDynamicMatrixInverse_proxy,
130
38
              bp::args("Model","Data","J"),
131
              "Computes the inverse of the constraint matrix [[M JT], [J 0]]. forward/impulseDynamics must be called first. The jacobian should be the same that was provided to forward/impulseDynamics.");
132
19
    }
133
134
  } // namespace python
135
} // namespace pinocchio