EigenGraspPlannerNoQt.h

Go to the documentation of this file.

#ifndef GRASP_PLANNING_GRASPIT_EIGENGRASPPLANNERNOQT_H
#define GRASP_PLANNING_GRASPIT_EIGENGRASPPLANNERNOQT_H

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

#include <vector>
#include <string>

#define DEFAULT_MAX_PLANNING_STEPS 70000

#include <EGPlanner/search.h>

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

namespace GraspIt
{

class EigenGraspPlannerNoQt: public GraspItAccessor
{
public:
    // So far, only AxisAngle supported, as others not tested yet.
    // Later: enum SearchType {Complete, AxisAngle, Ellipsoid, Approach};
    // enum SearchType  {AxisAngle};

    // 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};

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

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

    bool plan(const std::string& handName, const std::string& objectName,
              const EigenTransform * objectPose = NULL,
              const int maxPlanningSteps = DEFAULT_MAX_PLANNING_STEPS, const PlannerType& planType = SimAnn);

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

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

protected:
    virtual void idleEventFromSceneManager();

    virtual void onSceneManagerShutdown();

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

    // void stopPlanner();


    void initSearchType(GraspPlanningState& stateTemplate,  const StateType& 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);

    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;

    static void statusThreadLoop(EigenGraspPlannerNoQt * _this);

    // thread that prints the status messages during planning process
    THREAD * statusThread;


    // for test purposes: to separately calculate energy of grasps

    EGPlanner * graspitEgPlanner;
    RECURSIVE_MUTEX graspitEgPlannerMtx;

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

    // The search energy type to use. Is CONTACT_ENERGY 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 CONTACT_ENERGY, though I didn't
    // test all others except ENERGY_CONTACT_QUALITY
    std::string graspitSearchEnergyType;

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

    std::vector<const GraspPlanningState*> results;
};

}  // namespace GraspIt
#endif  //  GRASP_PLANNING_GRASPIT_EIGENGRASPPLANNERNOQT_H