frame.hpp 3.77 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
//
// Copyright (c) 2016 CNRS
//
// 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/>.

#ifndef __se3_frame_hpp__
#define __se3_frame_hpp__

#include "pinocchio/spatial/se3.hpp"
22
#include "pinocchio/multibody/fwd.hpp"
23

jcarpent's avatar
jcarpent committed
24
#include <string>
25
26
27

namespace se3
{
jcarpent's avatar
jcarpent committed
28
  ///
29
  /// \brief Enum on the possible types of frame
jcarpent's avatar
jcarpent committed
30
  ///
31
32
  enum FrameType
  {
33
34
35
36
    OP_FRAME     = 0x1 << 0, // operational frame type
    JOINT        = 0x1 << 1, // joint frame type
    FIXED_JOINT  = 0x1 << 2, // fixed joint frame type
    BODY         = 0x1 << 3, // body frame type
37
    SENSOR       = 0x1 << 4  // sensor frame type
38
  };
jcarpent's avatar
jcarpent committed
39
  
40
  ///
41
  /// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree
42
  ///
43
44
  template<typename _Scalar, int _Options>
  struct FrameTpl
45
  {
jcarpent's avatar
jcarpent committed
46
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47
    typedef se3::JointIndex JointIndex;
48
49
50
    enum { Options = _Options };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> SE3;
51
52
    
    ///
jcarpent's avatar
jcarpent committed
53
54
    /// \brief Default constructor of a frame.
    ///
55
    FrameTpl() : name(), parent(), placement(), type() {} // needed by std::vector
jcarpent's avatar
jcarpent committed
56
57
58
    
    ///
    /// \brief Builds a frame defined by its name, its joint parent id, its placement and its type.
59
60
    ///
    /// \param[in] name Name of the frame.
61
    /// \param[in] parent Index of the parent joint in the kinematic tree.
62
    /// \param[in] previousFrame Index of the parent frame in the kinematic tree.
Guilhem Saurel's avatar
Guilhem Saurel committed
63
    /// \param[in] frame_placement Placement of the frame wrt the parent joint frame.
64
    /// \param[in] type The type of the frame, see the enum FrameType
65
    ///
66
67
68
69
70
    FrameTpl(const std::string & name,
             const JointIndex parent,
             const FrameIndex previousFrame,
             const SE3 & frame_placement,
             const FrameType type)
jcarpent's avatar
jcarpent committed
71
    : name(name)
72
    , parent(parent)
73
    , previousFrame(previousFrame)
74
    , placement(frame_placement)
75
    , type(type)
76
77
78
    {}
    
    ///
jcarpent's avatar
jcarpent committed
79
    /// \returns true if *this and other matches and have the same parent, name and type.
80
81
82
    ///
    /// \param[in] other The frame to which the current frame is compared.
    ///
83
84
    template<typename S2, int O2>
    bool operator == (const FrameTpl<S2,O2> & other) const
85
    {
86
      return name == other.name && parent == other.parent
87
      && previousFrame == other.previousFrame
88
89
      && placement == other.placement
      && type == other.type ;
90
91
92
93
94
95
    }
    
    /// \brief Name of the frame.
    std::string name;
    
    /// \brief Index of the parent joint.
96
    JointIndex parent;
97
    
98
99
100
    /// \brief Index of the previous frame.
    FrameIndex previousFrame;
    
101
    /// \brief Placement of the frame wrt the parent joint.
102
    SE3 placement;
103
104
105

    /// \brief Type of the frame
    FrameType type;
106

107
  }; // struct FrameTpl
108

109
110
  template<typename Scalar, int Options>
  inline std::ostream & operator << (std::ostream& os, const FrameTpl<Scalar,Options> & f)
111
  {
112
113
114
115
116
117
118
119
120
121
    os
    << "Frame name: "
    << f.name
    << " paired to (parent joint/ previous frame)"
    << "(" <<f.parent << "/" << f.previousFrame << ")"
    << std::endl
    << "with relative placement wrt parent joint:\n" <<
    f.placement
    << std::endl;
    
122
123
124
    return os;
  }

125
} // namespace se3
126
127

#endif // ifndef __se3_frame_hpp__