GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: bindings/python/algorithm/expose-aba-derivatives.cpp Lines: 12 18 66.7 %
Date: 2023-08-09 08:43:58 Branches: 5 16 31.2 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2018-2021 CNRS INRIA
3
//
4
5
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
6
#include "pinocchio/algorithm/aba-derivatives.hpp"
7
#include "pinocchio/bindings/python/utils/eigen.hpp"
8
9
#include <eigenpy/eigen-to-python.hpp>
10
11
namespace pinocchio
12
{
13
  namespace python
14
  {
15
16
    namespace bp = boost::python;
17
1
    bp::tuple computeABADerivativesDefault(const Model & model, Data & data,
18
                                           const Eigen::VectorXd & q,
19
                                           const Eigen::VectorXd & v,
20
                                           const Eigen::VectorXd & tau)
21
    {
22
1
      computeABADerivatives(model,data,q,v,tau);
23
1
      make_symmetric(data.Minv);
24
1
      return bp::make_tuple(make_ref(data.ddq_dq),
25
1
                            make_ref(data.ddq_dv),
26
3
                            make_ref(data.Minv));
27
    }
28
29
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
30
31
    bp::tuple computeABADerivatives_fext(const Model & model, Data & data,
32
                                         const Eigen::VectorXd & q,
33
                                         const Eigen::VectorXd & v,
34
                                         const Eigen::VectorXd & tau,
35
                                         const ForceAlignedVector & fext)
36
    {
37
      computeABADerivatives(model,data,q,v,tau,fext);
38
      make_symmetric(data.Minv);
39
      return bp::make_tuple(make_ref(data.ddq_dq),
40
                            make_ref(data.ddq_dv),
41
                            make_ref(data.Minv));
42
    }
43
44
19
    void exposeABADerivatives()
45
    {
46
      using namespace Eigen;
47
48
19
      bp::def("computeABADerivatives",
49
              computeABADerivativesDefault,
50
38
              bp::args("model","data","q","v","tau"),
51
              "Computes the ABA derivatives, store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n"
52
              "which correspond to the partial derivatives of the joint acceleration vector output with respect to the joint configuration,\n"
53
              "velocity and torque vectors.\n\n"
54
              "Parameters:\n"
55
              "\tmodel: model of the kinematic tree\n"
56
              "\tdata: data related to the model\n"
57
              "\tq: the joint configuration vector (size model.nq)\n"
58
              "\tv: the joint velocity vector (size model.nv)\n"
59
              "\ttau: the joint torque vector (size model.nv)\n\n"
60
              "Returns: (ddq_dq, ddq_dv, ddq_da)");
61
62
19
      bp::def("computeABADerivatives",
63
              computeABADerivatives_fext,
64
38
              bp::args("model","data","q","v","tau","fext"),
65
              "Computes the ABA derivatives with external contact foces,\n"
66
              "store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n"
67
              "which correspond to the partial derivatives of the acceleration output with respect to the joint configuration,\n"
68
              "velocity and torque vectors.\n\n"
69
              "Parameters:\n"
70
              "\tmodel: model of the kinematic tree\n"
71
              "\tdata: data related to the model\n"
72
              "\tq: the joint configuration vector (size model.nq)\n"
73
              "\tv: the joint velocity vector (size model.nv)\n"
74
              "\ttau: the joint torque vector (size model.nv)\n"
75
              "\tfext: list of external forces expressed in the local frame of the joints (size model.njoints)\n\n"
76
              "Returns: (ddq_dq, ddq_dv, ddq_da)");
77
19
    }
78
  } // namespace python
79
} // namespace pinocchio