GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: bindings/python/algorithm/expose-frames.cpp Lines: 38 55 69.1 %
Date: 2024-04-26 13:14:21 Branches: 20 58 34.5 %

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/frames.hpp"
7
8
namespace pinocchio
9
{
10
  namespace python
11
  {
12
13
    static Data::Matrix6x get_frame_jacobian_proxy(const Model & model,
14
                                                   Data & data,
15
                                                   const Model::FrameIndex frame_id,
16
                                                   ReferenceFrame rf)
17
    {
18
      Data::Matrix6x J(6,model.nv); J.setZero();
19
      getFrameJacobian(model, data, frame_id, rf, J);
20
21
      return J;
22
    }
23
24
    static Data::Matrix6x compute_frame_jacobian_proxy(const Model & model,
25
                                                       Data & data,
26
                                                       const Eigen::VectorXd & q,
27
                                                       Model::FrameIndex frame_id)
28
    {
29
      Data::Matrix6x J(6,model.nv); J.setZero();
30
      computeFrameJacobian(model, data, q, frame_id, J);
31
32
      return J;
33
    }
34
35
4
    static Data::Matrix6x compute_frame_jacobian_proxy(const Model & model,
36
                                                       Data & data,
37
                                                       const Eigen::VectorXd & q,
38
                                                       Model::FrameIndex frame_id,
39
                                                       ReferenceFrame reference_frame)
40
    {
41

4
      Data::Matrix6x J(6,model.nv); J.setZero();
42
4
      computeFrameJacobian(model, data, q, frame_id, reference_frame, J);
43
44
4
      return J;
45
    }
46
47
    static Data::Matrix6x get_frame_jacobian_time_variation_proxy(const Model & model,
48
                                                                  Data & data,
49
                                                                  Model::FrameIndex jointId,
50
                                                                  ReferenceFrame rf)
51
    {
52
      Data::Matrix6x dJ(6,model.nv); dJ.setZero();
53
      getFrameJacobianTimeVariation(model,data,jointId,rf,dJ);
54
55
      return dJ;
56
    }
57
58
    static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model & model,
59
                                                              Data & data,
60
                                                              const Eigen::VectorXd & q,
61
                                                              const Eigen::VectorXd & v,
62
                                                              const Model::FrameIndex frame_id,
63
                                                              const ReferenceFrame rf)
64
    {
65
      computeJointJacobiansTimeVariation(model,data,q,v);
66
      updateFramePlacements(model,data);
67
68
      return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
69
    }
70
71
38
    BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameVelocity_overload, (getFrameVelocity<double,0,JointCollectionDefaultTpl>), 3, 4)
72
19
    BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameAcceleration_overload, (getFrameAcceleration<double,0,JointCollectionDefaultTpl>), 3, 4)
73
19
    BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameClassicalAcceleration_overload, (getFrameClassicalAcceleration<double,0,JointCollectionDefaultTpl>), 3, 4)
74
75
19
    void exposeFramesAlgo()
76
    {
77
      using namespace Eigen;
78
79
19
      bp::def("updateFramePlacements",
80
              &updateFramePlacements<double,0,JointCollectionDefaultTpl>,
81
38
              bp::args("model","data"),
82
              "Computes the placements of all the operational frames according to the current joint placement stored in data"
83
              "and puts the results in data.");
84
85
19
      bp::def("updateFramePlacement",
86
              &updateFramePlacement<double,0,JointCollectionDefaultTpl>,
87
38
              bp::args("model","data","frame_id"),
88
              "Computes the placement of the given operational frame (frame_id) according to the current joint placement stored in data, stores the results in data and returns it.",
89
              bp::return_value_policy<bp::return_by_value>());
90
91
19
      bp::def("getFrameVelocity",
92
              &getFrameVelocity<double,0,JointCollectionDefaultTpl>,
93
19
              getFrameVelocity_overload(
94
38
                bp::args("model","data","frame_id","reference_frame"),
95
                "Returns the spatial velocity of the frame expressed in the coordinate system given by reference_frame.\n"
96
                "forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint spatial velocity stored in data.v"));
97
98
19
      bp::def("getFrameAcceleration",
99
              &getFrameAcceleration<double,0,JointCollectionDefaultTpl>,
100
19
              getFrameAcceleration_overload(
101
38
                bp::args("model","data","frame_id","reference_frame"),
102
                "Returns the spatial acceleration of the frame expressed in the coordinate system given by reference_frame.\n"
103
                "forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
104
105
19
      bp::def("getFrameClassicalAcceleration",
106
              &getFrameClassicalAcceleration<double,0,JointCollectionDefaultTpl>,
107
19
              getFrameClassicalAcceleration_overload(
108
38
                bp::args("model","data","frame_id","reference_frame"),
109
                "Returns the \"classical\" acceleration of the frame expressed in the coordinate system given by reference_frame.\n"
110
                "forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
111
112
19
      bp::def("framesForwardKinematics",
113
              &framesForwardKinematics<double,0,JointCollectionDefaultTpl,VectorXd>,
114
38
              bp::args("model","data","q"),
115
              "Calls first the forwardKinematics(model,data,q) and then update the Frame placement quantities (data.oMf).");
116
117
19
      bp::def("computeFrameJacobian",
118
              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &, Model::FrameIndex, ReferenceFrame))&compute_frame_jacobian_proxy,
119
38
              bp::args("model","data","q","frame_id","reference_frame"),
120
              "Computes the Jacobian of the frame given by its frame_id in the coordinate system given by reference_frame.\n");
121
122
19
      bp::def("computeFrameJacobian",
123
              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &, Model::FrameIndex))&compute_frame_jacobian_proxy,
124
38
              bp::args("model","data","q","frame_id"),
125
              "Computes the Jacobian of the frame given by its frame_id.\n"
126
              "The columns of the Jacobian are expressed in the coordinates system of the Frame itself.\n"
127
              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
128
              "where v is the joint velocity.");
129
130
19
      bp::def("getFrameJacobian",
131
              &get_frame_jacobian_proxy,
132
38
              bp::args("model","data","frame_id","reference_frame"),
133
              "Computes the Jacobian of the frame given by its ID either in the local or the world frames.\n"
134
              "The columns of the Jacobian are expressed in the LOCAL frame coordinates system.\n"
135
              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
136
              "where v is the joint velocity.\n"
137
              "computeJointJacobians(model,data,q) and updateFramePlacements(model,data) must have been called first.");
138
139
19
      bp::def("frameJacobianTimeVariation",&frame_jacobian_time_variation_proxy,
140
38
              bp::args("model","data","q","v","frame_id","reference_frame"),
141
              "Computes the Jacobian Time Variation of the frame given by its frame_id either in the reference frame provided by reference_frame.\n");
142
143
19
      bp::def("getFrameJacobianTimeVariation",get_frame_jacobian_time_variation_proxy,
144
38
              bp::args("model","data","frame_id","reference_frame"),
145
              "Returns the Jacobian time variation of the frame given by its frame_id either in the reference frame provided by reference_frame.\n"
146
              "You have to call computeJointJacobiansTimeVariation(model,data,q,v) and updateFramePlacements(model,data) first.");
147
148
19
      bp::def("computeSupportedInertiaByFrame",
149
              &computeSupportedInertiaByFrame<double,0,JointCollectionDefaultTpl>,
150
38
              bp::args("model", "data", "frame_id", "with_subtree"),
151
              "Computes the supported inertia by the frame (given by frame_id) and returns it.\n"
152
              "The supported inertia corresponds to the sum of the inertias of all the child frames (that belongs to the same joint body) and the child joints, if with_subtree=True.\n"
153
              "You must first call pinocchio::forwardKinematics to update placement values in data structure.");
154
155
19
      bp::def("computeSupportedForceByFrame",
156
              &computeSupportedForceByFrame<double,0,JointCollectionDefaultTpl>,
157
38
              bp::args("model","data","frame_id"),
158
              "Computes the supported force of the frame (given by frame_id) and returns it.\n"
159
              "The supported force corresponds to the sum of all the forces experienced after the given frame.\n"
160
              "You must first call pinocchio::rnea to update placement values in data structure.");
161
162
19
    }
163
  } // namespace python
164
165
} // namespace pinocchio