EigenGraspPlanner.h

Go to the documentation of this file.

#ifndef GRASP_PLANNING_GRASPIT_EIGENGRASPPLANNER_H
#define GRASP_PLANNING_GRASPIT_EIGENGRASPPLANNER_H

#include <grasp_planning_graspit/GraspItSceneManager.h>
#include <grasp_planning_graspit/EigenGraspResult.h>
#include <grasp_planning_graspit/GraspItAccessor.h>

#include <vector>
#include <string>

#include <QObject>

#define DEFAULT_MAX_PLANNING_STEPS 70000
#define DEFAULT_MAX_RESULTS_PER_REPEAT 10
#define DEFAULT_FINISH_WITH_AUTOGRASP false

// To use  during beta phase, until decided which implementation
// is better.
// If this is defined, the implementation uses its own local
// SoIdleSensor to repeatedly call ivIdleCallback(). If
// this is not defined, the GraspItSceneManager event loop
// callback is used to call ivIdleCallback().
// #define USE_SEPARATE_SOSENSOR

class GraspPlanningState;
class GraspableBody;
class EGPlanner;
class SearchEnergy;
class GraspPlanningState;
class GraspItAccessor;

namespace GraspIt
{

class EigenGraspPlanner: public QObject, public GraspItAccessor
{
    Q_OBJECT

public:
    // So far, only AxisAngle supported, as others not tested yet.
    // Later will be: enum  GraspItStateType{Complete, AxisAngle, Ellipsoid, Approach};
    enum GraspItStateType  {AxisAngle};

    // Mapping to GraspIt! search energy type. For now only
    // supports CONTACT_ENERGY. All other types still to be added:
    // CONTACT_ENERGY, POTENTIAL_QUALITY_ENERGY, CONTACT_QUALITY_ENERGY,
    // AUTOGRASP_QUALITY_ENERGY, GUIDED_AUTOGRASP_ENERGY, STRICT_AUTOGRASP_ENERGY,
    // COMPLIANT_ENERGY, DYNAMIC_ENERGY
    enum GraspItSearchEnergyType  {EnergyContact};

    // Type of planner ot use. So far, only simulated annealing supported, as
    // others are not tested.
    // TODO: implement other planner types as well.
    // Later this should be: enum PlannerType {SimAnn, Loop, MultiThreaded };
    enum PlannerType {SimAnn};

    EigenGraspPlanner(const std::string& name, const SHARED_PTR<GraspItSceneManager>& i);
    virtual ~EigenGraspPlanner();

    bool plan(const int maxPlanningSteps,// = DEFAULT_MAX_PLANNING_STEPS,
              const int repeatPlanning,// = 1,
              const int maxResultsPerRepeat,// = DEFAULT_MAX_RESULTS_PER_REPEAT,
              const bool finishWithAutograsp,// = DEFAULT_FINISH_WITH_AUTOGRASP,
              const PlannerType& planType = SimAnn);

    bool plan(const std::string& handName, const std::string& objectName,
              const EigenTransform * objectPose,
              const int maxPlanningSteps,//  = DEFAULT_MAX_PLANNING_STEPS,
              const int repeatPlanning,//   = 1
              const int maxResultsPerRepeat,// = DEFAULT_MAX_RESULTS_PER_REPEAT,
              const bool finishWithAutograsp,// = DEFAULT_FINISH_WITH_AUTOGRASP,
              const PlannerType& planType = SimAnn);

    bool saveResultsAsWorldFiles(const std::string& inDirectory, const std::string& fileNamePrefix,
                                 bool asGraspIt = true, bool asInventor = false, bool createDir = true,
                                 bool saveSeparatePoseIV = false);

    void getResults(std::vector<EigenGraspResult>& allGrasps) const;

protected:
    virtual void idleEventFromSceneManager();

    virtual void onSceneManagerShutdown();

private:
    void deleteResults();

    bool copyResult(const GraspPlanningState * s, EigenGraspResult& result) const;

    void ivIdleCallback();

    bool initPlanner(const int maxPlanningSteps, const PlannerType& planType = SimAnn);

    // void stopPlanner();


    void initSearchType(GraspPlanningState& stateTemplate,  const GraspItStateType& st);

    void initPlannerType(const GraspPlanningState& stateTemplate, const PlannerType& pt);


    void updateResults();

    void plannerUpdate();


    void plannerComplete();


    void setPlanningParameters();

    void printPlanningResults();

    void plannerReset();

    // void inputLoad(const std::string& inputGraspFile);

    void printResult(int i, bool detailed = false);

    // double getEnergy(const GraspPlanningState * s);

    EigenTransform getHandTransform(const GraspPlanningState * s) const;
    EigenTransform getObjectTransform(const GraspPlanningState * s) const;

    bool checkStateValidity(const GraspPlanningState * s) const;

    void getGraspJointDOFs(const GraspPlanningState * s, std::vector<double>& dofs) const;

    void getPregraspJointDOFs(const GraspPlanningState * s, std::vector<double>& dofs) const;

    void getEigenGraspValues(const GraspPlanningState * s, std::vector<double>& egVals) const;


#ifdef USE_SEPARATE_SOSENSOR

    static void sensorCB(void *data, SoSensor *);
#endif

public slots:
    void plannerUpdateSlot();
    void plannerCompleteSlot();

private:
    enum PlannerCommand {NONE, START, STOP};

    PlannerCommand getPlannerCommand();
    void setPlannerCommand(const PlannerCommand c);

    PlannerCommand planCommand;
    MUTEX planCommandMtx;

    // for test purposes: to separately calculate energy of grasps
    // SearchEnergy * mEnergyCalculator;

    EGPlanner * graspitEgPlanner;
    RECURSIVE_MUTEX graspitEgPlannerMtx;

    // The state type to use for GraspIt. Default is STATE_AXIS_ANGLE.
    GraspItStateType graspitStateType;

    // The search energy type to use. Is ENERGY_CONTACT by default.
    // Important: ENERGY_CONTACT_QUALITY doesn't work properly at the moment,
    // at least not with the Jaco hand, also not in the original simulator.... the energy
    // drops to -0. Reason to be found in SearchEnergy::potentialQualityEnergy, variable gq
    // gets 0. So far, only successfully tested method was ENERGY_CONTACT, though I didn't
    // test all others except ENERGY_CONTACT_QUALITY
    GraspItSearchEnergyType graspitSearchEnergyType;

    // whether to use contacts information in the planner or not
    bool useContacts;

    std::vector<const GraspPlanningState*> results;

#ifdef USE_SEPARATE_SOSENSOR
    SoSensor *mIdleSensor;
#endif
};

}  // namespace GraspIt
#endif  //  GRASP_PLANNING_GRASPIT_EIGENGRASPPLANNER_H