GraspItHelpers.cpp

Go to the documentation of this file.

#include <grasp_planning_graspit/GraspItHelpers.h>
#include <graspit/matvec3D.h>

transf GraspIt::getGraspitTransform(const GraspIt::EigenTransform& transform)
{
    Eigen::Vector3d trans = transform.translation();
    Eigen::Quaterniond quat(transform.rotation());

    transf t;
    vec3 translation(trans.x(), trans.y(), trans.z());
    Quaternion quaternion(quat.w(), quat.x(), quat.y(), quat.z());
    t.set(quaternion, translation);

    return t;
}

GraspIt::EigenTransform GraspIt::getEigenTransform(const transf& trans)
{
    GraspIt::EigenTransform transform;
    transform.setIdentity();

    Quaternion q = trans.rotation();
    vec3 t = trans.translation();

    Eigen::Vector3d translation(t.x(), t.y(), t.z());
    Eigen::Quaterniond quaternion(q.w(), q.x(), q.y(), q.z());

    transform = transform.translate(translation);
    transform = transform.rotate(quaternion);

    return transform;
}