Program Listing for File motion-ref.hpp
↰ Return to documentation for file (include/pinocchio/spatial/motion-ref.hpp
)
//
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_spatial_motion_ref_hpp__
#define __pinocchio_spatial_motion_ref_hpp__
namespace pinocchio
{
template<typename Vector6ArgType>
struct traits<MotionRef<Vector6ArgType>>
{
typedef typename Vector6ArgType::Scalar Scalar;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
enum
{
LINEAR = 0,
ANGULAR = 3,
Options = Vector6::Options
};
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
typedef Matrix6 ActionMatrixType;
typedef Matrix4 HomogeneousMatrixType;
typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef MotionTpl<Scalar, Options> MotionPlain;
typedef MotionPlain PlainReturnType;
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
typedef DataRefType ToVectorReturnType;
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
typedef ConstDataRefType ToVectorConstReturnType;
typedef MotionRef<Vector6ArgType> MotionRefType;
}; // traits MotionRef
template<typename Vector6ArgType>
struct SE3GroupAction<MotionRef<Vector6ArgType>>
{
typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename MotionDerived>
struct MotionAlgebraAction<MotionRef<Vector6ArgType>, MotionDerived>
{
typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
};
namespace internal
{
template<typename Vector6ArgType, typename Scalar>
struct RHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
{
typedef typename pinocchio::traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename Scalar>
struct LHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
{
typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
};
} // namespace internal
template<typename Vector6ArgType>
class MotionRef : public MotionDense<MotionRef<Vector6ArgType>>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef MotionDense<MotionRef> Base;
typedef typename traits<MotionRef>::DataRefType DataRefType;
MOTION_TYPEDEF_TPL(MotionRef);
using Base::operator=;
using Base::angular;
using Base::linear;
using Base::__mequ__;
using Base::__minus__;
using Base::__mult__;
using Base::__opposite__;
using Base::__pequ__;
using Base::__plus__;
MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
: m_ref(v_like)
{
EIGEN_STATIC_ASSERT(
Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
assert(v_like.size() == 6);
}
MotionRef(const MotionRef & other)
: m_ref(other.m_ref)
{
}
ToVectorConstReturnType toVector_impl() const
{
return m_ref;
}
ToVectorReturnType toVector_impl()
{
return m_ref;
}
// Getters
ConstAngularType angular_impl() const
{
return ConstAngularType(m_ref.derived(), ANGULAR);
}
ConstLinearType linear_impl() const
{
return ConstLinearType(m_ref.derived(), LINEAR);
}
AngularType angular_impl()
{
return m_ref.template segment<3>(ANGULAR);
}
LinearType linear_impl()
{
return m_ref.template segment<3>(LINEAR);
}
template<typename V3>
void angular_impl(const Eigen::MatrixBase<V3> & w)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
angular_impl() = w;
}
template<typename V3>
void linear_impl(const Eigen::MatrixBase<V3> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
linear_impl() = v;
}
// Specific operators for MotionTpl and MotionRef
template<typename S1, int O1>
MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
{
return MotionPlain(m_ref + v.toVector());
}
template<typename Vector6Like>
MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
{
return MotionPlain(m_ref + v.toVector());
}
template<typename S1, int O1>
MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
{
return MotionPlain(m_ref - v.toVector());
}
template<typename Vector6Like>
MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
{
return MotionPlain(m_ref - v.toVector());
}
template<typename S1, int O1>
MotionRef & __pequ__(const MotionTpl<S1, O1> & v)
{
m_ref += v.toVector();
return *this;
}
template<typename Vector6Like>
MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
{
m_ref += v.toVector();
return *this;
}
template<typename S1, int O1>
MotionRef & __mequ__(const MotionTpl<S1, O1> & v)
{
m_ref -= v.toVector();
return *this;
}
template<typename Vector6Like>
MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
{
m_ref -= v.toVector();
return *this;
}
template<typename OtherScalar>
MotionPlain __mult__(const OtherScalar & alpha) const
{
return MotionPlain(alpha * m_ref);
}
MotionRef & ref()
{
return *this;
}
inline PlainReturnType plain() const
{
return PlainReturnType(m_ref);
}
protected:
DataRefType m_ref;
}; // class MotionRef<Vector6Like>
template<typename Vector6ArgType>
struct traits<MotionRef<const Vector6ArgType>>
{
typedef typename Vector6ArgType::Scalar Scalar;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
enum
{
LINEAR = 0,
ANGULAR = 3,
Options = Vector6::Options
};
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
typedef Matrix6 ActionMatrixType;
typedef Matrix4 HomogeneousMatrixType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef ConstLinearType LinearType;
typedef ConstAngularType AngularType;
typedef MotionTpl<Scalar, Options> MotionPlain;
typedef MotionPlain PlainReturnType;
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
typedef ConstDataRefType ToVectorConstReturnType;
typedef ConstDataRefType DataRefType;
typedef DataRefType ToVectorReturnType;
typedef MotionRef<const Vector6ArgType> MotionRefType;
}; // traits MotionRef<const Vector6ArgType>
template<typename Vector6ArgType>
class MotionRef<const Vector6ArgType> : public MotionDense<MotionRef<const Vector6ArgType>>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef MotionDense<MotionRef> Base;
typedef typename traits<MotionRef>::DataRefType DataRefType;
MOTION_TYPEDEF_TPL(MotionRef);
using Base::operator=;
using Base::angular;
using Base::linear;
using Base::__minus__;
using Base::__mult__;
using Base::__opposite__;
using Base::__plus__;
MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
: m_ref(v_like)
{
EIGEN_STATIC_ASSERT(
Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
assert(v_like.size() == 6);
}
MotionRef(const MotionRef & other)
: m_ref(other.m_ref)
{
}
ToVectorConstReturnType toVector_impl() const
{
return m_ref;
}
// Getters
ConstAngularType angular_impl() const
{
return ConstAngularType(m_ref.derived(), ANGULAR);
}
ConstLinearType linear_impl() const
{
return ConstLinearType(m_ref.derived(), LINEAR);
}
// Specific operators for MotionTpl and MotionRef
template<typename S1, int O1>
MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
{
return MotionPlain(m_ref + v.toVector());
}
template<typename Vector6Like>
MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
{
return MotionPlain(m_ref + v.toVector());
}
template<typename S1, int O1>
MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
{
return MotionPlain(m_ref - v.toVector());
}
template<typename Vector6Like>
MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
{
return MotionPlain(m_ref - v.toVector());
}
template<typename OtherScalar>
MotionPlain __mult__(const OtherScalar & alpha) const
{
return MotionPlain(alpha * m_ref);
}
const MotionRef & ref() const
{
return *this;
}
inline PlainReturnType plain() const
{
return PlainReturnType(m_ref);
}
protected:
DataRefType m_ref;
}; // class MotionRef<const Vector6Like>
} // namespace pinocchio
#endif // ifndef __pinocchio_spatial_motion_ref_hpp__