pinocchio  2.2.1-dirty
multibody/frame.hpp
1 //
2 // Copyright (c) 2016,2018 CNRS
3 //
4 
5 #ifndef __pinocchio_frame_hpp__
6 #define __pinocchio_frame_hpp__
7 
8 #include "pinocchio/spatial/se3.hpp"
9 #include "pinocchio/multibody/fwd.hpp"
10 
11 #include <string>
12 
13 namespace pinocchio
14 {
18  enum FrameType
19  {
20  OP_FRAME = 0x1 << 0, // operational frame type
21  JOINT = 0x1 << 1, // joint frame type
22  FIXED_JOINT = 0x1 << 2, // fixed joint frame type
23  BODY = 0x1 << 3, // body frame type
24  SENSOR = 0x1 << 4 // sensor frame type
25  };
26 
30  template<typename _Scalar, int _Options>
31  struct FrameTpl
32  {
33  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34  typedef pinocchio::JointIndex JointIndex;
35  enum { Options = _Options };
36  typedef _Scalar Scalar;
38 
42  FrameTpl() : name(), parent(), placement(), type() {} // needed by std::vector
43 
53  FrameTpl(const std::string & name,
54  const JointIndex parent,
55  const FrameIndex previousFrame,
56  const SE3 & frame_placement,
57  const FrameType type)
58  : name(name)
59  , parent(parent)
60  , previousFrame(previousFrame)
61  , placement(frame_placement)
62  , type(type)
63  {}
64 
70  template<typename S2, int O2>
71  bool operator == (const FrameTpl<S2,O2> & other) const
72  {
73  return name == other.name
74  && parent == other.parent
75  && previousFrame == other.previousFrame
76  && placement == other.placement
77  && type == other.type ;
78  }
79 
81  template<typename NewScalar>
83  {
84  typedef FrameTpl<NewScalar,Options> ReturnType;
85  ReturnType res(name,
86  parent,
88  placement.template cast<NewScalar>(),
89  type);
90  return res;
91  }
92 
93  // data
94 
96  std::string name;
97 
99  JointIndex parent;
100 
102  FrameIndex previousFrame;
103 
106 
109 
110  }; // struct FrameTpl
111 
112  template<typename Scalar, int Options>
113  inline std::ostream & operator << (std::ostream& os, const FrameTpl<Scalar,Options> & f)
114  {
115  os
116  << "Frame name: "
117  << f.name
118  << " paired to (parent joint/ previous frame)"
119  << "(" <<f.parent << "/" << f.previousFrame << ")"
120  << std::endl
121  << "with relative placement wrt parent joint:\n" <<
122  f.placement
123  << std::endl;
124 
125  return os;
126  }
127 
128 } // namespace pinocchio
129 
130 #endif // ifndef __pinocchio_frame_hpp__
FrameTpl< NewScalar, Options > cast() const
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
SE3 placement
Placement of the frame wrt the parent joint.
std::string name
Name of the frame.
JointIndex parent
Index of the parent joint.
FrameTpl(const std::string &name, const JointIndex parent, const FrameIndex previousFrame, const SE3 &frame_placement, const FrameType type)
Builds a frame defined by its name, its joint parent id, its placement and its type.
FrameIndex previousFrame
Index of the previous frame.
FrameType type
Type of the frame.
FrameTpl()
Default constructor of a frame.
FrameType
Enum on the possible types of frame.
Main pinocchio namespace.
Definition: treeview.dox:24
bool operator==(const FrameTpl< S2, O2 > &other) const