frame.hpp

//
// 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
{
  enum FrameType
  {
    OP_FRAME     = 0x1 << 0, 
    JOINT        = 0x1 << 1, 
    FIXED_JOINT  = 0x1 << 2, 
    BODY         = 0x1 << 3, 
    SENSOR       = 0x1 << 4  
  };
  
  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;
    
    FrameTpl()
    : name()
    , parent()
    , placement()
    , type()
    , inertia(Inertia::Zero())
    {} // needed by std::vector
    
    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)
    {}
    
    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;
    }
 
    template<typename S2, int O2>
    bool operator != (const FrameTpl<S2,O2> & other) const
    {
      return !(*this == other);
    }
 
    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
    
    std::string name;
    
    JointIndex parent;
    
    FrameIndex previousFrame;
    
    SE3 placement;
 
    FrameType type;
    
    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__