EigenConverters.h

Go to the documentation of this file.

 
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <RobotRaconteur/DataTypes.h>
#include <RobotRaconteurCompanion/Util/UtilMacros.h>
 
#include <com__robotraconteur__geometry.h>
 
#pragma once
 
namespace RobotRaconteur
{
namespace Companion
{
namespace Converters
{
namespace Eigen
{
 
template <typename T>
class RRArrayEigenTraits
{
};
 
template <typename T, int N>
class RRArrayEigenTraits< ::Eigen::Matrix<T, N, 1> >
{
  public:
    typedef RobotRaconteur::RRArrayPtr<T> RRArrayPtrType;
    typedef T NumberType;
    enum
    {
        Rows = N
    };
 
    static ::Eigen::Matrix<T, N, 1> CopyToEigen(RRArrayPtrType rr_array)
    {
        RobotRaconteur::rr_null_check(rr_array);
        if (rr_array->size() != N)
        {
            throw RobotRaconteur::InvalidArgumentException("Invalid array length for Eigen vector, expected " +
                                                           boost::lexical_cast<std::string>(N) + " got " +
                                                           boost::lexical_cast<std::string>(rr_array->size()));
        }
 
        return ::Eigen::Map< ::Eigen::Matrix<T, N, 1> >(rr_array->data());
    }
};
 
template <typename T>
class RRArrayEigenTraits< ::Eigen::Matrix<T, ::Eigen::Dynamic, 1> >
{
  public:
    typedef RobotRaconteur::RRArrayPtr<T> RRArrayPtrType;
    typedef T NumberType;
 
    static ::Eigen::Matrix<T, ::Eigen::Dynamic, 1> CopyToEigen(RRArrayPtrType rr_array)
    {
        RobotRaconteur::rr_null_check(rr_array);
 
        return ::Eigen::Map< ::Eigen::Matrix<T, ::Eigen::Dynamic, 1> >(rr_array->data(), rr_array->size());
    }
};

template <typename T, int N>
RobotRaconteur::RRArrayPtr<T> EigenToRRArray(const ::Eigen::Matrix<T, N, 1>& eigen_in)
{
    return RobotRaconteur::AttachRRArrayCopy<T>(eigen_in.data(), eigen_in.rows());
}

template <typename T>
T RRArrayToEigen(typename RRArrayEigenTraits<T>::RRArrayPtrType rr_array)
{
    return RRArrayEigenTraits<T>::CopyToEigen(rr_array);
}

template <typename T, int R, int C>
RobotRaconteur::RRMultiDimArrayPtr<T> EigenToRRMultiDimArray(const ::Eigen::Matrix<T, R, C>& eigen_in)
{
    std::vector<uint32_t> dims{(uint32_t)eigen_in.rows(), (uint32_t)eigen_in.cols()};
    RobotRaconteur::RRMultiDimArrayPtr<T> out = RobotRaconteur::AllocateEmptyRRMultiDimArray<T>(dims);
    ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic, ::Eigen::ColMajor> eigen_in2 = eigen_in;
    size_t total_elems = eigen_in.rows() * eigen_in.cols();
    memcpy(&out->Array->at(0), eigen_in2.data(), sizeof(T) * total_elems);
    return out;
}

template <typename T>
::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic> RRMultiDimArrayToEigen(
    RobotRaconteur::RRMultiDimArrayPtr<T> array_in)
{
    RobotRaconteur::rr_null_check(array_in);
    RobotRaconteur::rr_null_check(array_in->Dims);
    RobotRaconteur::rr_null_check(array_in->Array);
 
    switch (array_in->Dims->size())
    {
    case 0:
        throw RobotRaconteur::InvalidArgumentException("Invalid matrix dimensions");
    case 1: {
        ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic, ::Eigen::ColMajor> o(array_in->Dims->at(0), 1);
        memcpy(o.data(), array_in->Array->data(), sizeof(T) * array_in->Array->size());
        return o;
    }
    case 2: {
        ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic, ::Eigen::ColMajor> o(array_in->Dims->at(0),
                                                                                    array_in->Dims->at(1));
        memcpy(o.data(), array_in->Array->data(), sizeof(T) * array_in->Array->size());
        return o;
    }
    default:
        throw RobotRaconteur::InvalidArgumentException("Invalid matrix dimensions");
    }
}

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Vector2 ToVector2(
    const ::Eigen::Ref<const ::Eigen::Vector2d>& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Vector2d ToVector(const com::robotraconteur::geometry::Vector2& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Vector3 ToVector3(
    const ::Eigen::Ref<const ::Eigen::Vector3d>& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Vector3d ToVector(const com::robotraconteur::geometry::Vector3& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Vector6 ToVector6(
    const ::Eigen::Ref<const ::Eigen::Matrix<double, 6, 1> >& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Matrix<double, 6, 1> ToVector(
    const com::robotraconteur::geometry::Vector6& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Point2D ToPoint2D(
    const ::Eigen::Ref<const ::Eigen::Vector2d>& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Vector2d ToVector(const com::robotraconteur::geometry::Point2D& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Point ToPoint(
    const ::Eigen::Ref<const ::Eigen::Vector3d>& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Vector3d ToVector(const com::robotraconteur::geometry::Point& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Size2D ToSize2D(
    const ::Eigen::Ref<const ::Eigen::Vector2d>& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Vector2d ToVector(const com::robotraconteur::geometry::Size2D& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Size ToSize(
    const ::Eigen::Ref<const ::Eigen::Vector3d>& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Vector3d ToVector(const com::robotraconteur::geometry::Size& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Quaterniond ToQuaternion(const com::robotraconteur::geometry::Quaternion& q);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Quaternion ToQuaternion(const ::Eigen::Quaterniond& q);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Isometry3d ToIsometry(const com::robotraconteur::geometry::Transform& t);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Transform ToTransform(const ::Eigen::Isometry3d& iso);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Isometry3d ToIsometry(const com::robotraconteur::geometry::Pose& t);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Pose ToPose(const ::Eigen::Isometry3d& iso);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::SpatialVelocity ToSpatialVelocity(
    const ::Eigen::Ref<const ::Eigen::Matrix<double, 6, 1> >& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Matrix<double, 6, 1> ToVector(
    const com::robotraconteur::geometry::SpatialVelocity& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::SpatialAcceleration ToSpatialAcceleration(
    const ::Eigen::Ref<const ::Eigen::Matrix<double, 6, 1> >& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Matrix<double, 6, 1> ToVector(
    const com::robotraconteur::geometry::SpatialAcceleration& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API com::robotraconteur::geometry::Wrench ToWrench(
    const ::Eigen::Ref<const ::Eigen::Matrix<double, 6, 1> >& vs);

ROBOTRACONTEUR_COMPANION_UTIL_API ::Eigen::Matrix<double, 6, 1> ToVector(
    const com::robotraconteur::geometry::Wrench& vs);
 
} // namespace Eigen
} // namespace Converters
} // namespace Companion
} // namespace RobotRaconteur