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__