expose-frames.cpp 7.47 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
49
      framesForwardKinematics(model,data);
  
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
77
78
79
80
81
    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);
      framesForwardKinematics(model,data);
  
      return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
    }        

82
    
83
84
    void exposeFramesAlgo()
    {
jcarpent's avatar
jcarpent committed
85
86
87
88
89
90
91
92
      bp::def("framesKinematics",
              (void (*)(const Model &, Data &))&framesForwardKinematics,
              bp::args("Model","Data"),
              "Computes the placements of all the operational frames according to the current joint placement stored in data"
              "and put the results in data.");
      
      bp::def("framesKinematics",
              (void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
93
94
              bp::args("Model","Data",
                       "Configuration q (size Model::nq)"),
jcarpent's avatar
jcarpent committed
95
96
              "Update first the placement of the joints according to the given configuration value."
              "And computes the placements of all the operational frames"
97
98
              "and put the results in data.");
      
99
      bp::def("frameJacobian",
100
              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &, const Model::FrameIndex, ReferenceFrame))&frame_jacobian_proxy,
101
              bp::args("Model","Data",
102
                       "Configuration q (size Model::nq)",
103
                       "Operational frame ID (int)",
104
105
                       "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."
106
107
108
109
              "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.");
      
110
111
      bp::def("getFrameJacobian",
              (Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, ReferenceFrame))&get_frame_jacobian_proxy,
112
              bp::args("Model","Data",
113
                       "Operational frame ID (int)",
114
115
                       "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."
116
117
118
              "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"
119
              "Be aware that computeJointJacobians and framesKinematics must have been called first.");
120

121
122
123
124
125
126
127
128
129
130
131
132
      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.");      

133
134
135
136
137
138
139
140
141
      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.");

142
143
144
    }
  } // namespace python
} // namespace se3