frames.hpp 4.22 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
//
// 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

#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
22
#include "pinocchio/multibody/data.hpp"
23
24
25
26
27

namespace se3
{

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

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

  /**
51
52
53
54
55
   * @brief      Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
   *             depending on the value of the template parameter rf.
   *             You must first call se3::computeJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
   *
   * @tparam     rf Reference frame in which the columns of the Jacobian are expressed.
jcarpent's avatar
jcarpent committed
56
   
57
58
59
   * @remark     Similarly to se3::getJacobian with LOCAL or WORLD templated parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
   *             in the local coordinates of the frame, or if rl == WORDL, it returns the Jacobian expressed of the point coincident with the origin
   *             and expressed in a coordinate system aligned with the WORLD.
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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
  template<ReferenceFrame rf>
  void getFrameJacobian(const Model & model,
                        const Data & data,
                        const Model::FrameIndex frame_id,
                        Data::Matrix6x & J);
  
  /**
   * @brief      Returns the jacobian of the frame expresssed the LOCAL frame coordinate system.
   *             You must first call se3::computeJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
   *
   * @param[in]  model       The kinematic model
   * @param[in]  data        Data associated to model
   * @param[in]  frame_id    Id of the operational Frame
   * @param[out] J           The Jacobian of the Frame expressed in the coordinates Frame.
   *
   * @warning    The function se3::computeJacobians and se3::framesForwardKinematics should have been called first.
   */
   
86
  inline void getFrameJacobian(const Model & model,
87
                               const Data & data,
jcarpent's avatar
jcarpent committed
88
                               const Model::FrameIndex frame_id,
89
                               Data::Matrix6x & J);
90
 
jcarpent's avatar
jcarpent committed
91
} // namespace se3
92
93

/* --- Details -------------------------------------------------------------------- */
94
#include "pinocchio/algorithm/frames.hxx"
95

96
#endif // ifndef __se3_frames_hpp__