frame.hpp

//
// Copyright (c) 2016-2021 CNRS INRIA
//
 
#ifndef __pinocchio_multibody_frame_hpp__
#define __pinocchio_multibody_frame_hpp__
 
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model-item.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 traits<FrameTpl<_Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    enum
    {
      Options = _Options
    };
  };
 
  template<typename _Scalar, int _Options>
  struct FrameTpl : ModelItem<FrameTpl<_Scalar, _Options>>
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef FrameTpl<_Scalar, _Options> ModelItemDerived;
    typedef typename traits<ModelItemDerived>::Scalar Scalar;
    enum
    {
      Options = traits<ModelItemDerived>::Options
    };
    typedef ModelItem<ModelItemDerived> Base;
 
    typedef SE3Tpl<Scalar, Options> SE3;
    typedef InertiaTpl<Scalar, Options> Inertia;
    typedef pinocchio::JointIndex JointIndex;
 
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    FrameTpl()
    : Base()
    , parent(Base::parentJoint)
    , previousFrame(Base::parentFrame)
    , type()
    , inertia(Inertia::Zero())
    {
    } // needed by std::vector
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    FrameTpl(
      const std::string & name,
      const JointIndex parentJoint,
      const SE3 & frame_placement,
      const FrameType type,
      const Inertia & inertia = Inertia::Zero())
    : Base(name, parentJoint, 0, frame_placement)
    , parent(Base::parentJoint)
    , previousFrame(Base::parentFrame)
    , type(type)
    , inertia(inertia)
    {
    }
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    FrameTpl(
      const std::string & name,
      const JointIndex parent_joint,
      const FrameIndex parent_frame,
      const SE3 & frame_placement,
      const FrameType type,
      const Inertia & inertia = Inertia::Zero())
    : Base(name, parent_joint, parent_frame, frame_placement)
    , parent(Base::parentJoint)
    , previousFrame(Base::parentFrame)
    , type(type)
    , inertia(inertia)
    {
    }
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    FrameTpl(const FrameTpl & other)
    : Base(other.name, other.parentJoint, other.parentFrame, other.placement)
    , parent(Base::parentJoint)
    , previousFrame(Base::parentFrame)
    , type(other.type)
    , inertia(other.inertia)
    {
    }
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    template<typename S2, int O2>
    explicit FrameTpl(const FrameTpl<S2, O2> & other)
    : Base(
        other.name, other.parentJoint, other.parentFrame, other.placement.template cast<Scalar>())
    , parent(Base::parentJoint)
    , previousFrame(Base::parentFrame)
    , type(other.type)
    , inertia(other.inertia.template cast<Scalar>())
    {
    }
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
 
    FrameTpl<Scalar, Options> & operator=(const FrameTpl<Scalar, Options> & other)
    {
      name = other.name;
      parentJoint = other.parentJoint;
      parentFrame = other.parentFrame;
      placement = other.placement;
      type = other.type;
      inertia = other.inertia;
      return *this;
    }
 
    template<typename S2, int O2>
    bool operator==(const FrameTpl<S2, O2> & other) const
    {
      return name == other.name && parentJoint == other.parentJoint
             && parentFrame == other.parentFrame && 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, parentJoint, parentFrame, placement.template cast<NewScalar>(), type,
        inertia.template cast<NewScalar>());
      return res;
    }
 
    // data
    PINOCCHIO_DEPRECATED JointIndex & parent;
 
    PINOCCHIO_DEPRECATED FrameIndex & previousFrame;
 
    using Base::name;
    using Base::parentFrame;
    using Base::parentJoint;
    using Base::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/ parent frame)"
       << "(" << f.parentJoint << "/" << f.parentFrame << ")" << 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_multibody_frame_hpp__