frames.hpp 5.65 KB
Newer Older
1
//
2
// Copyright (c) 2015-2017 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// 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/>.

18
19
#ifndef __se3_frames_hpp__
#define __se3_frames_hpp__
20
21
22
23

#include "pinocchio/multibody/model.hpp"

#include "pinocchio/algorithm/kinematics.hpp"
24
#include "pinocchio/algorithm/jacobian.hpp"
25
#include "pinocchio/algorithm/check.hpp"
26
27
28
29
30

namespace se3
{

  /**
jcarpent's avatar
jcarpent committed
31
32
33
34
   * @brief      Updates the position of each frame contained in the model
   *
   * @param[in]  model  The kinematic model.
   * @param      data   Data associated to model.
35
36
37
   *
   * @warning    One of the algorithms forwardKinematics should have been called first
   */
jcarpent's avatar
jcarpent committed
38
39
40
  inline void framesForwardKinematics(const Model & model,
                                      Data & data
                                      );
41
42

  /**
jcarpent's avatar
jcarpent committed
43
44
   * @brief      First calls the forwardKinematics on the model, then computes the placement of each frame.
   *             /sa se3::forwardKinematics
45
   *
jcarpent's avatar
jcarpent committed
46
47
48
   * @param[in]  model                    The kinematic model.
   * @param      data                     Data associated to model.
   * @param[in]  q                        Configuration vector.
49
   */
jcarpent's avatar
jcarpent committed
50
51
52
53
  inline void framesForwardKinematics(const Model & model,
                                      Data & data,
                                      const Eigen::VectorXd & q
                                      );
54
55

  /**
jcarpent's avatar
jcarpent committed
56
57
58
59
   * @brief      Returns the jacobian of the frame expresssed in the world frame or
     in the local frame depending on the template argument. 
   
   * @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system. Expressed in the global frame, the jacobian maps to the spatial velocity of the point coinciding with the center of the world and attached to the frame.
60
61
62
   *
   * @param[in]  model       The kinematic model
   * @param[in]  data        Data associated to model
63
   * @param[in]  frame_id    Id of the operational frame we want to compute the jacobian
jcarpent's avatar
jcarpent committed
64
   * @param[out] J           The Jacobian of the
65
   *
jcarpent's avatar
jcarpent committed
66
   * @tparam     local_frame  If true, the jacobian is expressed in the local frame. Otherwise, the jacobian is expressed in the world frame.
67
   * 
jcarpent's avatar
jcarpent committed
68
   * @warning    The function se3::computeJacobians should have been called first
69
   */
70
  template<bool local_frame>
71
  inline void getFrameJacobian(const Model & model,
jcarpent's avatar
jcarpent committed
72
73
74
75
                               const Data& data,
                               const Model::FrameIndex frame_id,
                               Data::Matrix6x & J
                               );
76
 
jcarpent's avatar
jcarpent committed
77
} // namespace se3
78
79
80
81
82
83

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
  
  
jcarpent's avatar
jcarpent committed
84
85
86
  inline void framesForwardKinematics(const Model & model,
                                      Data & data
                                      )
87
  {
88
89
90
91
#ifndef NDEBUG
    assert(model.check(data) && "data is not consistent with model.");
#endif
    
92
93
94
    // The following for loop starts by index 1 because the first frame is fixed
    // and corresponds to the universe.s
    for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
jcarpent's avatar
jcarpent committed
95
    {
96
97
      const Frame & frame = model.frames[i];
      const Model::JointIndex & parent = frame.parent;
98
      if (frame.placement.isIdentity())
99
100
        data.oMf[i] = data.oMi[parent];
      else
101
        data.oMf[i] = data.oMi[parent]*frame.placement;
jcarpent's avatar
jcarpent committed
102
    }
103
  }
104
  
jcarpent's avatar
jcarpent committed
105
106
107
108
109
  inline void framesForwardKinematics(const Model & model,
                                      Data & data,
                                      const Eigen::VectorXd & q
                                      )
  {
110
111
112
113
#ifndef NDEBUG
    assert(model.check(data) && "data is not consistent with model.");
#endif
    
jcarpent's avatar
jcarpent committed
114
115
116
117
118
119
    forwardKinematics(model, data, q);
    framesForwardKinematics(model, data);
  }
  
  
  
jcarpent's avatar
jcarpent committed
120
  template<bool local_frame>
jcarpent's avatar
jcarpent committed
121
122
123
124
125
  inline void getFrameJacobian(const Model & model,
                               const Data & data,
                               const Model::FrameIndex frame_id,
                               Data::Matrix6x & J)
  {
126
127
    assert(J.cols() == model.nv);
    assert(data.J.cols() == model.nv);
128
129
130
#ifndef NDEBUG
    assert(model.check(data) && "data is not consistent with model.");
#endif
jcarpent's avatar
jcarpent committed
131
    
132
    const Model::JointIndex & parent = model.frames[frame_id].parent;
133
    const SE3 & oMframe = data.oMf[frame_id];
134
    const Frame & frame = model.frames[frame_id];
jcarpent's avatar
jcarpent committed
135
136
137
138
    
    const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
    
    // Lever between the joint center and the frame center expressed in the global frame
139
    const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
jcarpent's avatar
jcarpent committed
140
    
141
    getJacobian<local_frame>(model, data, parent, J);
142

143
    if (!frame.placement.isIdentity())
144
    {
145
146
      for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
      {
147
        if(!local_frame)
148
149
150
151
          J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
        else
          J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
      }
152
    }
jcarpent's avatar
jcarpent committed
153
  }
154
155
156

} // namespace se3

157
#endif // ifndef __se3_frames_hpp__
158