EigenGraspResult.h

Go to the documentation of this file.

#ifndef GRASP_PLANNING_GRASPIT_EIGENGRASPRESULT_H
#define GRASP_PLANNING_GRASPIT_EIGENGRASPRESULT_H

#include <string>
#include <vector>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <grasp_planning_graspit/PrintHelpers.h>

namespace GraspIt
{

class EigenGraspResult
{
public:
    typedef Eigen::Transform<double, 3, Eigen::Affine> EigenTransform;

    EigenGraspResult() {}
    EigenGraspResult(const EigenGraspResult& o);

    EigenGraspResult(const EigenTransform& _relTransform,
                     const std::vector<double>& _dofsGrasp,
                     const std::vector<double>& _dofsPregrasp,
                     const std::vector<double>& _egVals,
                     const bool _legal,
                     const double _qualEpsilon,
                     const double _qualVolume,
                     const double _energy);


    const EigenTransform& getObjectToHandTransform() const
    {
        return relTransform;
    }

    const std::vector<double>& getGraspJointDOFs() const
    {
        return graspDOFs;
    }
    const std::vector<double>& getPregraspJointDOFs() const
    {
        return pregraspDOFs;
    }

    const std::vector<double>& getEigenGraspValues() const
    {
        return egVals;
    }

    double getEnergy() const
    {
        return energy;
    }

    bool isLegal() const
    {
        return legal;
    }

    double qualityEpsilon() const
    {
        return qualEpsilon;
    }


    double qualityVolume() const
    {
        return qualVolume;
    }

    friend std::ostream& operator<<(std::ostream& o, const EigenGraspResult& r)
    {
        if (r.isLegal()) o << "Legal grasp: ";
        else o << "Illegal grasp: ";

        // o<<"Quality (eps) = "<<r.qualEpsilon<<" Quality (vol) = "<<r.qualVolume<<" ";
        o << "Energy = " << r.energy << " Joint DOFs = [";
        for (std::vector<double>::const_iterator it = r.graspDOFs.begin(); it != r.graspDOFs.end(); ++it)
        {
            o << *it;
            if ((it + 1) != r.graspDOFs.end()) o << ", ";
        }
        o << "] EigenGrasps = [";
        for (std::vector<double>::const_iterator it = r.egVals.begin(); it != r.egVals.end(); ++it)
        {
            o << *it;
            if ((it + 1) != r.egVals.end()) o << ", ";
        }
        o << "] Relative transform = " << r.relTransform;
        return o;
    }

private:
    EigenTransform relTransform;

    std::vector<double> graspDOFs;
    std::vector<double> pregraspDOFs;
    std::vector<double> egVals;

    // The energy used by the simulated annealing-based planners
    double energy;

    // Whether this state is legal or not, usually refers to presence or absence of collisions
    bool legal;

    // The epsilon quality for this grasp
    // Jan 16: The quality value is always 0 for the simulated annealing planner. Have to investigate why.
    double qualEpsilon;

    // The volume quality for this grasp
    // Jan 16: The quality value is always 0 for the simulated annealing planner. Have to investigate why.
    double qualVolume;
};
}  // namespace GraspIt
#endif  //  GRASP_PLANNING_GRASPIT_EIGENGRASPRESULT_H