pick.hpp

#ifndef PICK_HPP
#define PICK_HPP

#include "manipulationTask.hpp"
#include "Planner-pkg.h"
#include "Logging/Logger.h"


class Pick : public ManipulationTask
{
    MOVE3D_STATIC_LOGGER;
public:
    Pick();
    virtual bool initAll();
    virtual bool initialize();
    virtual bool finalize();
    virtual bool findCandidateSolutions();
    virtual bool getRandomSol();
    virtual bool findConfigurations();
    virtual bool findTrajectories();
    virtual bool computeSolutionTrajectories(TaskSolution *ts);
    virtual TaskSolution *createSolution(bool computeMP);
    virtual bool setAgents(std::multimap<std::string,std::string> agents_name);
    virtual bool setObjects(std::multimap<std::string,std::string> objects_name);
    virtual bool setPoints(std::multimap<std::string,p3d_point> points);
    virtual bool setdata(std::multimap<std::string,std::string> additionalData);
    virtual void smoothSolution(int alternativeId);
    virtual std::string getDescr();
    virtual std::string getTextFromValues();
    virtual double computeHeuristicCost();
    virtual double getSSSizeLeft();

    virtual std::vector<int> getJointsIdsToUse();
    virtual void setPlanPartToUse(Sem::PlanningPart &planPart);

    virtual std::pair<Sem::PlanningPart,bool> getPlanPartToUse();

    bool getGrasps();

    void correctConfigs();

protected:
    unsigned int current_grasp_id;
    std::map<int,std::vector<gpGrasp> >  graspLists;
//    int _armId;

    p3d_traj* approachTraj;
    p3d_traj* graspTraj;
    p3d_traj* escapeTraj;
    p3d_traj* closeHandTraj;

    Sem::PlanningPart _planPartToUse;

    bool _noArmPreference;
    std::vector<armLabel> _availableArms;
};

#endif // PICK_HPP