pickMultiRrt.hpp

#ifndef PICK_MULTIRRT_HPP
#define PICK_MULTIRRT_HPP

#include "manipulationTask.hpp"
#include "Planner-pkg.h"


class Pick_MultiRRT : public ManipulationTask
{
    MOVE3D_STATIC_LOGGER;
public:
    Pick_MultiRRT();
    virtual bool init();
    virtual bool findCandidateSolutions();
    virtual bool getRandomSol();
    virtual bool findConfigurations();
    virtual bool findTrajectories();
    virtual bool computeSolutionTrajectories(TaskSolution *TS);
//    virtual bool findAlternative();
    virtual TaskSolution *createSolution(bool computeMP);
//    virtual vector<TaskSubSolution *> setNextTraj(TaskSubSolution* doneTSS);
    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();
private:
    unsigned int current_grasp_id;
    std::vector<gpGrasp> graspList;
    std::set<int> _alternativeIds;
    int _armId;

    std::vector<confPtr_t> graspConfClosedVect;
    std::vector<confPtr_t> graspConfOpenVect;
    std::vector<confPtr_t> approachConfVect;
    std::vector<confPtr_t> escapeConfVect;
    confPtr_t initConf;

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

#endif // PICK_MULTIRRT_HPP