Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/frame.hpp	Lines:	29	32	90.6 %
Date:	2024-01-23 21:41:47	Branches:	12	24	50.0 %


//
// Copyright (c) 2016-2021 CNRS INRIA
//

#ifndef __pinocchio_frame_hpp__
#define __pinocchio_frame_hpp__

#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"

#include <string>

namespace pinocchio
{
  ///
  /// \brief Enum on the possible types of frames.
  ///
  /// \note
  /// In Pinocchio, the FIXED joints are not included in the kinematic tree but we keep track of them via the vector of frames contained in ModelTpl.
  /// The JOINT frames are duplicate information with respect to the joint information contained in ModelTpl.
  ///
  /// All other frame types are defined for user convenience and code
  /// readability, to also keep track of the information usually stored within URDF models.
  ///
  /// See also https://wiki.ros.org/urdf/XML/joint, https://wiki.ros.org/urdf/XML/link and https://wiki.ros.org/urdf/XML/sensor.
  ///
  enum FrameType
  {
    OP_FRAME     = 0x1 << 0, ///< operational frame: user-defined frames that are defined at runtime
    JOINT        = 0x1 << 1, ///< joint frame: attached to the child body of a joint (a.k.a. child frame)
    FIXED_JOINT  = 0x1 << 2, ///< fixed joint frame: joint frame but for a fixed joint
    BODY         = 0x1 << 3, ///< body frame: attached to the collision, inertial or visual properties of a link
    SENSOR       = 0x1 << 4  ///< sensor frame: defined in a sensor element
  };

  ///
  /// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree
  ///
  template<typename _Scalar, int _Options>
  struct FrameTpl
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef pinocchio::JointIndex JointIndex;
    enum { Options = _Options };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef InertiaTpl<Scalar,Options> Inertia;

    ///
    /// \brief Default constructor of a frame.
    ///
    FrameTpl()
    : name()
    , parent()
    , placement()
    , type()
    , inertia(Inertia::Zero())
    {} // needed by std::vector

    ///
    /// \brief Builds a frame defined by its name, its joint parent id, its placement and its type.
    ///
    /// \param[in] name Name of the frame.
    /// \param[in] parent Index of the parent joint in the kinematic tree.
    /// \param[in] previousFrame Index of the parent frame in the kinematic tree.
    /// \param[in] frame_placement Placement of the frame wrt the parent joint frame.
    /// \param[in] type The type of the frame, see the enum FrameType.
    /// \param[in] inertia Inertia info attached to the frame.
    ///
    FrameTpl(const std::string & name,
             const JointIndex parent,
             const FrameIndex previousFrame,
             const SE3 & frame_placement,
             const FrameType type,
             const Inertia & inertia = Inertia::Zero())
    : name(name)
    , parent(parent)
    , previousFrame(previousFrame)
    , placement(frame_placement)
    , type(type)
    , inertia(inertia)
    {}

    ///
    /// \brief Equality comparison operator.
    ///
    /// \returns true if *this is equal to other.
    ///
    /// \param[in] other The frame to which the current frame is compared.
    ///
    template<typename S2, int O2>
    bool operator == (const FrameTpl<S2,O2> & other) const
    {
      return name == other.name
      && parent == other.parent
      && previousFrame == other.previousFrame
      && placement == other.placement
      && type == other.type
      && inertia == other.inertia;
    }

    ///
    /// \returns true if *this is NOT equal to other.
    ///
    template<typename S2, int O2>
    bool operator != (const FrameTpl<S2,O2> & other) const
    {
      return !(*this == other);
    }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    FrameTpl<NewScalar,Options> cast() const
    {
      typedef FrameTpl<NewScalar,Options> ReturnType;
      ReturnType res(name,
                     parent,
                     previousFrame,
                     placement.template cast<NewScalar>(),
                     type,
                     inertia.template cast<NewScalar>());
      return res;
    }

    // data

    /// \brief Name of the frame.
    std::string name;

    /// \brief Index of the parent joint.
    JointIndex parent;

    /// \brief Index of the previous frame.
    FrameIndex previousFrame;

    /// \brief Placement of the frame wrt the parent joint.
    SE3 placement;

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

    /// \brief Inertia information attached to the frame.
    ///        This inertia will be appended to the inertia supported by the parent joint when calling ModelTpl::addFrame.
    ///        It won't be processed otherwise by the algorithms.
    Inertia inertia;

  }; // struct FrameTpl

  template<typename Scalar, int Options>
  inline std::ostream & operator << (std::ostream& os,
                                     const FrameTpl<Scalar,Options> & f)
  {
    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
    << "containing inertia:\n"
    << f.inertia
    << std::endl;

    return os;
  }

} // namespace pinocchio

#endif // ifndef __pinocchio_frame_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2021 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_frame_hpp__
6			#define __pinocchio_frame_hpp__
7
8			#include "pinocchio/spatial/se3.hpp"
9			#include "pinocchio/spatial/inertia.hpp"
10			#include "pinocchio/multibody/fwd.hpp"
11
12			#include <string>
13
14			namespace pinocchio
15			{
16			///
17			/// \brief Enum on the possible types of frames.
18			///
19			/// \note
20			/// In Pinocchio, the FIXED joints are not included in the kinematic tree but we keep track of them via the vector of frames contained in ModelTpl.
21			/// The JOINT frames are duplicate information with respect to the joint information contained in ModelTpl.
22			///
23			/// All other frame types are defined for user convenience and code
24			/// readability, to also keep track of the information usually stored within URDF models.
25			///
26			/// See also https://wiki.ros.org/urdf/XML/joint, https://wiki.ros.org/urdf/XML/link and https://wiki.ros.org/urdf/XML/sensor.
27			///
28			enum FrameType
29			{
30			OP_FRAME = 0x1 << 0, ///< operational frame: user-defined frames that are defined at runtime
31			JOINT = 0x1 << 1, ///< joint frame: attached to the child body of a joint (a.k.a. child frame)
32			FIXED_JOINT = 0x1 << 2, ///< fixed joint frame: joint frame but for a fixed joint
33			BODY = 0x1 << 3, ///< body frame: attached to the collision, inertial or visual properties of a link
34			SENSOR = 0x1 << 4 ///< sensor frame: defined in a sensor element
35			};
36
37			///
38			/// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree
39			///
40			template<typename _Scalar, int _Options>
41			struct FrameTpl
42			{
43			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44			typedef pinocchio::JointIndex JointIndex;
45			enum { Options = _Options };
46			typedef _Scalar Scalar;
47			typedef SE3Tpl<Scalar,Options> SE3;
48			typedef InertiaTpl<Scalar,Options> Inertia;
49
50			///
51			/// \brief Default constructor of a frame.
52			///
53		925	FrameTpl()
54			: name()
55			, parent()
56			, placement()
57			, type()
58	✓✗✓✗	925	, inertia(Inertia::Zero())
59		925	{} // needed by std::vector
60
61			///
62			/// \brief Builds a frame defined by its name, its joint parent id, its placement and its type.
63			///
64			/// \param[in] name Name of the frame.
65			/// \param[in] parent Index of the parent joint in the kinematic tree.
66			/// \param[in] previousFrame Index of the parent frame in the kinematic tree.
67			/// \param[in] frame_placement Placement of the frame wrt the parent joint frame.
68			/// \param[in] type The type of the frame, see the enum FrameType.
69			/// \param[in] inertia Inertia info attached to the frame.
70			///
71		26440	FrameTpl(const std::string & name,
72			const JointIndex parent,
73			const FrameIndex previousFrame,
74			const SE3 & frame_placement,
75			const FrameType type,
76			const Inertia & inertia = Inertia::Zero())
77			: name(name)
78			, parent(parent)
79			, previousFrame(previousFrame)
80			, placement(frame_placement)
81			, type(type)
82	✓✗✓✗	26440	, inertia(inertia)
83		26440	{}
84
85			///
86			/// \brief Equality comparison operator.
87			///
88			/// \returns true if *this is equal to other.
89			///
90			/// \param[in] other The frame to which the current frame is compared.
91			///
92			template<typename S2, int O2>
93		4952	bool operator == (const FrameTpl<S2,O2> & other) const
94			{
95		4952	return name == other.name
96	✓✗	4952	&& parent == other.parent
97	✓✗	4952	&& previousFrame == other.previousFrame
98	✓✗	4952	&& placement == other.placement
99	✓✗	4952	&& type == other.type
100	✓✗✓✗	9904	&& inertia == other.inertia;
101			}
102
103			///
104			/// \returns true if *this is NOT equal to other.
105			///
106			template<typename S2, int O2>
107			bool operator != (const FrameTpl<S2,O2> & other) const
108			{
109			return !(*this == other);
110			}
111
112			/// \returns An expression of *this with the Scalar type casted to NewScalar.
113			template<typename NewScalar>
114		1043	FrameTpl<NewScalar,Options> cast() const
115			{
116			typedef FrameTpl<NewScalar,Options> ReturnType;
117		1043	ReturnType res(name,
118		1043	parent,
119	✓✗	1043	previousFrame,
120			placement.template cast<NewScalar>(),
121	✓✗	1043	type,
122			inertia.template cast<NewScalar>());
123		1043	return res;
124			}
125
126			// data
127
128			/// \brief Name of the frame.
129			std::string name;
130
131			/// \brief Index of the parent joint.
132			JointIndex parent;
133
134			/// \brief Index of the previous frame.
135			FrameIndex previousFrame;
136
137			/// \brief Placement of the frame wrt the parent joint.
138			SE3 placement;
139
140			/// \brief Type of the frame.
141			FrameType type;
142
143			/// \brief Inertia information attached to the frame.
144			/// This inertia will be appended to the inertia supported by the parent joint when calling ModelTpl::addFrame.
145			/// It won't be processed otherwise by the algorithms.
146			Inertia inertia;
147
148			}; // struct FrameTpl
149
150			template<typename Scalar, int Options>
151		1	inline std::ostream & operator << (std::ostream& os,
152			const FrameTpl<Scalar,Options> & f)
153			{
154			os
155			<< "Frame name: "
156		1	<< f.name
157			<< " paired to (parent joint/ previous frame)"
158		1	<< "(" << f.parent << "/" << f.previousFrame << ")"
159		1	<< std::endl
160		1	<< "with relative placement wrt parent joint:\n"
161		1	<< f.placement
162		1	<< "containing inertia:\n"
163		1	<< f.inertia
164		1	<< std::endl;
165
166		1	return os;
167			}
168
169			} // namespace pinocchio
170
171			#endif // ifndef __pinocchio_frame_hpp__