frames.hpp 4.83 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2016 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
26
27
28
29
30
31
32
33
34
35

namespace se3
{

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

  /**
41
   * @brief      Compute Kinematics of full model, then the position of each operational frame
42
43
44
45
46
   *
   * @param[in]  model                    The kinematic model
   * @param      data                     Data associated to model
   * @param[in]  q                        Configuration vector
   */
jcarpent's avatar
jcarpent committed
47
48
49
50
  inline void framesForwardKinematics(const Model & model,
                                      Data & data,
                                      const Eigen::VectorXd & q
                                      );
51
52

  /**
53
   * @brief      Return the jacobian of the operational frame in the world frame or
54
55
56
57
     in the local frame depending on the template argument.
   *
   * @param[in]  model       The kinematic model
   * @param[in]  data        Data associated to model
58
   * @param[in]  frame_id    Id of the operational frame we want to compute the jacobian
59
60
61
62
63
64
65
66
   * @param      J           The filled Jacobian Matrix
   *
   * @tparam     localFrame  Express the jacobian in the local or global frame
   * 
   * @warning    The function computeJacobians should have been called first
   */
  template<bool localFrame>
  inline void getFrameJacobian(const Model & model,
jcarpent's avatar
jcarpent committed
67
68
69
70
                               const Data& data,
                               const Model::FrameIndex frame_id,
                               Data::Matrix6x & J
                               );
71
 
jcarpent's avatar
jcarpent committed
72
} // namespace se3
73
74
75
76
77
78

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
  
  
jcarpent's avatar
jcarpent committed
79
80
81
  inline void framesForwardKinematics(const Model & model,
                                      Data & data
                                      )
82
  {
83
    for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
jcarpent's avatar
jcarpent committed
84
    {
85
86
87
88
89
90
91
92
93
94
      const Frame & frame = model.frames[i];
      const Model::JointIndex & parent = frame.parent;
      if (frame.placement == SE3::Identity())
      {
        data.oMf[i] = data.oMi[parent];
      }
      else
      {
        data.oMf[i] = (data.oMi[parent] * frame.placement);
      }
jcarpent's avatar
jcarpent committed
95
    }
96
  }
97
  
jcarpent's avatar
jcarpent committed
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
  inline void framesForwardKinematics(const Model & model,
                                      Data & data,
                                      const Eigen::VectorXd & q
                                      )
  {
    forwardKinematics(model, data, q);
    framesForwardKinematics(model, data);
  }
  
  
  
  template<bool localFrame>
  inline void getFrameJacobian(const Model & model,
                               const Data & data,
                               const Model::FrameIndex frame_id,
                               Data::Matrix6x & J)
  {
    assert( J.rows() == data.J.rows() );
    assert( J.cols() == data.J.cols() );
    
118
    const Model::JointIndex & parent = model.frames[frame_id].parent;
119
    const SE3 & oMframe = data.oMf[frame_id];
120
    const Frame & frame = model.frames[frame_id];
jcarpent's avatar
jcarpent committed
121
122
123
124
    
    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
125
    const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.placement.translation()));
jcarpent's avatar
jcarpent committed
126
127
    
    getJacobian<localFrame>(model, data, parent, J);
128
129

    if (frame.placement == SE3::Identity())
130
131
132
133
    {
      // do nothing
    }
    else
134
    {
135
136
137
138
139
140
141
      for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
      {
        if(! localFrame )
          J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
        else
          J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
      }
142
    }
jcarpent's avatar
jcarpent committed
143
  }
144
145
146

} // namespace se3

147
#endif // ifndef __se3_frames_hpp__
148