Program Listing for File frame.hpp
↰ Return to documentation for file (include/pinocchio/multibody/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__