expose-frames.cpp 9.36 KB
Newer Older
1
//
2
// Copyright (c) 2015-2018 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/frames.hpp"

namespace se3
{
  namespace python
  {
    
26
27
28
29
    static Data::Matrix6x get_frame_jacobian_proxy(const Model & model,
                                                   Data & data,
                                                   const Model::FrameIndex frame_id,
                                                   ReferenceFrame rf)
30
    {
jcarpent's avatar
jcarpent committed
31
      Data::Matrix6x J(6,model.nv); J.setZero();
32
      if(rf == LOCAL)
33
34
35
        getFrameJacobian<LOCAL>(model, data, frame_id, J);
      else
        getFrameJacobian<WORLD>(model, data, frame_id, J);
36
37
38
39
      
      return J;
    }
    
40
41
    static Data::Matrix6x frame_jacobian_proxy(const Model & model,
                                               Data & data,
42
                                               const Eigen::VectorXd & q,
43
                                               const Model::FrameIndex frame_id,
44
45
                                               ReferenceFrame rf
                                               )
46
    {
47
      computeJointJacobians(model,data,q);
48
      updateFramePlacements(model,data);
49
  
50
      return get_frame_jacobian_proxy(model, data, frame_id, rf);
51
    }
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67


    static Data::Matrix6x
    get_frame_jacobian_time_variation_proxy(const Model & model,
                                            Data & data,
                                            Model::FrameIndex jointId,
                                            ReferenceFrame rf)
    {
      Data::Matrix6x dJ(6,model.nv); dJ.setZero();
      
      if(rf == LOCAL) getFrameJacobianTimeVariation<LOCAL>(model,data,jointId,dJ);
      else getFrameJacobianTimeVariation<WORLD>(model,data,jointId,dJ);
      
      return dJ;
    }

68
69
70
71
72
73
74
75
76
    static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model & model,
                                                              Data & data,
                                                              const Eigen::VectorXd & q,
                                                              const Eigen::VectorXd & v,
                                                              const Model::FrameIndex frame_id,
                                                              ReferenceFrame rf
                                                              )
    {
      computeJointJacobiansTimeVariation(model,data,q,v);
77
      updateFramePlacements(model,data);
78
79
80
81
  
      return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
    }        

82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
    static Motion get_frame_velocity_proxy(const Model & model,
                                           Data & data,
                                           const Model::FrameIndex frame_id
                                           )
    {
      Motion v;
      getFrameVelocity(model,data,frame_id,v);
      return v;
    }

    static Motion get_frame_acceleration_proxy(const Model & model,
                                               Data & data,
                                               const Model::FrameIndex frame_id
                                               )
    {
      Motion a;
      getFrameAcceleration(model,data,frame_id,a);
      return a;
    }
101
    
102
103
    void exposeFramesAlgo()
    {
104
105
      bp::def("updateFramePlacements",
              (void (*)(const Model &, Data &))&updateFramePlacements,
jcarpent's avatar
jcarpent committed
106
107
              bp::args("Model","Data"),
              "Computes the placements of all the operational frames according to the current joint placement stored in data"
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
              "and puts the results in data.");

      bp::def("updateFramePlacement",
              (const SE3 & (*)(const Model &, Data &, const Model::FrameIndex))&updateFramePlacement,
              bp::args("Model","Data","Operational frame ID (int)"),
              "Computes the placement of the given operational frames according to the current joint placement stored in data,"
              "puts the results in data and returns it.",
              bp::return_value_policy<bp::return_by_value>());

      bp::def("getFrameVelocity",
              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
              bp::args("Model","Data","Operational frame ID (int)"),
              "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
              "Fist or second order forwardKinematics should be called first.");

      bp::def("getFrameAcceleration",
              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_acceleration_proxy,
              bp::args("Model","Data","Operational frame ID (int)"),
              "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
              "Second order forwardKinematics should be called first.");
jcarpent's avatar
jcarpent committed
128
      
129
      bp::def("framesForwardKinematics",
jcarpent's avatar
jcarpent committed
130
              (void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
131
132
              bp::args("Model","Data",
                       "Configuration q (size Model::nq)"),
jcarpent's avatar
jcarpent committed
133
134
              "Update first the placement of the joints according to the given configuration value."
              "And computes the placements of all the operational frames"
135
136
              "and put the results in data.");
      
137
      bp::def("frameJacobian",
138
              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &, const Model::FrameIndex, ReferenceFrame))&frame_jacobian_proxy,
139
              bp::args("Model","Data",
140
                       "Configuration q (size Model::nq)",
141
                       "Operational frame ID (int)",
142
143
                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
              "Computes the Jacobian of the frame given by its ID either in the local or the world frames."
144
145
146
147
              "The columns of the Jacobian are expressed in the frame coordinates.\n"
              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
              "where v is the time derivative of the configuration q.");
      
148
149
      bp::def("getFrameJacobian",
              (Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, ReferenceFrame))&get_frame_jacobian_proxy,
150
              bp::args("Model","Data",
151
                       "Operational frame ID (int)",
152
153
                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
              "Computes the Jacobian of the frame given by its ID either in the local or the world frames."
154
155
156
              "The columns of the Jacobian are expressed in the frame coordinates.\n"
              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
              "where v is the time derivative of the configuration q.\n"
157
              "Be aware that computeJointJacobians and framesKinematics must have been called first.");
158

159
160
161
162
163
164
165
166
167
168
169
170
      bp::def("frameJacobianTimeVariation",
              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &,const Eigen::VectorXd &, const Model::FrameIndex, ReferenceFrame))&frame_jacobian_time_variation_proxy,
              bp::args("Model","Data",
                       "Configuration q (size Model::nq)",
                       "Joint velocity v (size Model::nv)",                       
                       "Operational frame ID (int)",
                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
              "Computes the Jacobian Time Variation of the frame given by its ID either in the local or the world frames."
              "The columns of the Jacobian time variation are expressed in the frame coordinates.\n"
              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
              "where v is the time derivative of the configuration q.");      

171
172
173
174
175
176
177
178
179
      bp::def("getFrameJacobianTimeVariation",get_frame_jacobian_time_variation_proxy,
              bp::args("Model, the model of the kinematic tree",
                       "Data, the data associated to the model where the results are stored",
                       "Frame ID, the index of the frame.",
                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
              "Returns the Jacobian time variation of a specific frame (specified by Frame ID) expressed either in the world or the local frame."
              "You have to call computeJointJacobiansTimeVariation and framesKinematics first."
              "If rf is set to LOCAL, it returns the jacobian time variation associated to the frame index. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");

180
181
182
    }
  } // namespace python
} // namespace se3