expose-contact-dynamics.cpp 6.95 KB
Newer Older
1
//
2
// Copyright (c) 2015-2019 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
11
{
  namespace python
  {
jcarpent's avatar
jcarpent committed
12
   
13
14
15
16
17
18
19
    static const Eigen::VectorXd forwardDynamics_proxy(const Model & model,
                                                       Data & data,
                                                       const Eigen::VectorXd & q,
                                                       const Eigen::VectorXd & v,
                                                       const Eigen::VectorXd & tau,
                                                       const Eigen::MatrixXd & J,
                                                       const Eigen::VectorXd & gamma,
20
                                                       const double inv_damping = 0.0)
21
    {
22
      return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
23
24
    }
    
25
26
27
28
29
30
31
32
33
34
35
    BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 8)

    static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model & model,
                                                            Data & data,
                                                            const Eigen::VectorXd & tau,
                                                            const Eigen::MatrixXd & J,
                                                            const Eigen::VectorXd & gamma,
                                                            const double inv_damping = 0.0)
    {
      return forwardDynamics(model, data, tau, J, gamma, inv_damping);
    }
36
    
37
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads_no_q, forwardDynamics_proxy_no_q, 5, 6)

39
40
41
42
43
    static const Eigen::VectorXd impulseDynamics_proxy(const Model & model,
                                                       Data & data,
                                                       const Eigen::VectorXd & q,
                                                       const Eigen::VectorXd & v_before,
                                                       const Eigen::MatrixXd & J,
44
45
                                                       const double r_coeff = 0.,
                                                       const double inv_damping = 0.)
46
    {
47
      return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
48
    }
49
50

    BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7)
51
52
53
54
55
    
    static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model & model,
                                                            Data & data,
                                                            const Eigen::VectorXd & v_before,
                                                            const Eigen::MatrixXd & J,
56
57
                                                            const double r_coeff = 0.,
                                                            const double inv_damping = 0.)
58
    {
59
      return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
60
61
    }

62
63
    BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6)

64
65
66
67
68
69
70
71
72
73
    static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
                                                                         Data & data,
                                                                         const Eigen::MatrixXd & J)
    {
      Eigen::MatrixXd MJtJ_inv(model.nv+J.rows(), model.nv+J.rows());
      getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
      return MJtJ_inv;
    }
    

74
75
    void exposeDynamics()
    {
jcarpent's avatar
jcarpent committed
76
77
78
      using namespace Eigen;
      
      bp::def("forwardDynamics",
79
              &forwardDynamics_proxy,
80
              forwardDynamics_overloads(
81
82
83
84
85
86
              bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)",
                       "Joint velocity v (size Model::nv)",
                       "Joint torque tau (size Model::nv)",
                       "Contact Jacobian J (size nb_constraint * Model::nv)",
                       "Contact drift gamma (size nb_constraint)",
87
88
89
90
91
92
93
94
95
96
97
98
                       "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank."),
              "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"
              ));

      bp::def("forwardDynamics",
              &forwardDynamics_proxy_no_q,
              forwardDynamics_overloads_no_q(
              bp::args("Model","Data",
                       "Joint torque tau (size Model::nv)",
                       "Contact Jacobian J (size nb_constraint * Model::nv)",
                       "Contact drift gamma (size nb_constraint)",
                       "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank."),
99
              "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"
100
              ));
101
102
103

      bp::def("impulseDynamics",
              &impulseDynamics_proxy,
104
              impulseDynamics_overloads(
105
106
107
108
              bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)",
                       "Joint velocity before impact v_before (size Model::nv)",
                       "Contact Jacobian J (size nb_constraint * Model::nv)",
109
110
111
                       "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)",
                       "Damping factor when J is rank deficient."
                       ),
112
              "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c"
113
              ));
114
115
116
      
      bp::def("impulseDynamics",
              &impulseDynamics_proxy_no_q,
117
              impulseDynamics_overloads_no_q(
118
119
120
              bp::args("Model","Data",
                       "Joint velocity before impact v_before (size Model::nv)",
                       "Contact Jacobian J (size nb_constraint * Model::nv)",
121
122
                       "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)",
                       "Damping factor when J is rank deficient."),
123
              "Solves the impact dynamics problem with contacts, put the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c"
124
              ));
125
      
126
127
128
129
      bp::def("getKKTContactDynamicMatrixInverse",getKKTContactDynamicMatrixInverse_proxy,
              bp::args("Model","Data",
                       "Contact Jacobian J(size nb_constraint * Model::nv)"),
              "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.");
130
131
132
    }
    
  } // namespace python
133
} // namespace pinocchio