frames.hpp 5.68 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
   * @brief      Returns the jacobian of the frame expresssed in the world frame or
     in the local frame depending on the template argument. 
58
   *             You must first call se3::framesForwardKinematics and se3::computeJacobians to update values in data structure.
jcarpent's avatar
jcarpent committed
59
60
   
   * @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.
61
62
63
   *
   * @param[in]  model       The kinematic model
   * @param[in]  data        Data associated to model
64
   * @param[in]  frame_id    Id of the operational frame we want to compute the jacobian
jcarpent's avatar
jcarpent committed
65
   * @param[out] J           The Jacobian of the
66
   *
jcarpent's avatar
jcarpent committed
67
   * @tparam     local_frame  If true, the jacobian is expressed in the local frame. Otherwise, the jacobian is expressed in the world frame.
68
   * 
jcarpent's avatar
jcarpent committed
69
   * @warning    The function se3::computeJacobians should have been called first
70
   */
71
  template<bool local_frame>
72
  inline void getFrameJacobian(const Model & model,
jcarpent's avatar
jcarpent committed
73
74
75
76
                               const Data& data,
                               const Model::FrameIndex frame_id,
                               Data::Matrix6x & J
                               );
77
 
jcarpent's avatar
jcarpent committed
78
} // namespace se3
79
80
81
82
83
84

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
  
  
jcarpent's avatar
jcarpent committed
85
86
87
  inline void framesForwardKinematics(const Model & model,
                                      Data & data
                                      )
88
  {
89
90
    assert(model.check(data) && "data is not consistent with model.");
    
91
92
93
    // 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
94
    {
95
96
      const Frame & frame = model.frames[i];
      const Model::JointIndex & parent = frame.parent;
97
      if (frame.placement.isIdentity())
98
99
        data.oMf[i] = data.oMi[parent];
      else
100
        data.oMf[i] = data.oMi[parent]*frame.placement;
jcarpent's avatar
jcarpent committed
101
    }
102
  }
103
  
jcarpent's avatar
jcarpent committed
104
105
106
107
108
  inline void framesForwardKinematics(const Model & model,
                                      Data & data,
                                      const Eigen::VectorXd & q
                                      )
  {
109
110
    assert(model.check(data) && "data is not consistent with model.");
    
jcarpent's avatar
jcarpent committed
111
112
113
114
115
116
    forwardKinematics(model, data, q);
    framesForwardKinematics(model, data);
  }
  
  
  
jcarpent's avatar
jcarpent committed
117
  template<bool local_frame>
jcarpent's avatar
jcarpent committed
118
119
120
121
122
  inline void getFrameJacobian(const Model & model,
                               const Data & data,
                               const Model::FrameIndex frame_id,
                               Data::Matrix6x & J)
  {
123
124
    assert(J.cols() == model.nv);
    assert(data.J.cols() == model.nv);
125
    assert(model.check(data) && "data is not consistent with model.");
jcarpent's avatar
jcarpent committed
126
    
127
    const Model::JointIndex & parent = model.frames[frame_id].parent;
128
    const SE3 & oMframe = data.oMf[frame_id];
129
    const Frame & frame = model.frames[frame_id];
jcarpent's avatar
jcarpent committed
130
131
132
    
    const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
    
133
134
    if(!local_frame)
      getJacobian<local_frame>(model, data, parent, J);
135

136
137
138
139
    // Lever between the joint center and the frame center expressed in the global frame
    const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
      
    for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
140
    {
141
142
143
144
      if(!local_frame)
        J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
      else
        J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
145
    }
jcarpent's avatar
jcarpent committed
146
  }
147
148
149

} // namespace se3

150
#endif // ifndef __se3_frames_hpp__
151