frames.hpp 4.96 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

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

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

} // namespace se3

134
#endif // ifndef __se3_frames_hpp__
135