manipulationTask.hpp

#ifndef MANIPULATIONTASK_HPP
#define MANIPULATIONTASK_HPP

#include "GTP/Tasks/privateTask.hpp"

class ManipulationTask : public PrivateTask
{
    MOVE3D_STATIC_LOGGER;
public:
    ManipulationTask();
    //virtual bool init();  //should be called after setting main agent
    virtual bool findCandidateSolutions() = 0;
    virtual bool getRandomSol() = 0;
    virtual bool setToSolution(int solutionId, bool computeMP);
    virtual TaskSolution *createSolution(bool computeMP) = 0;
//    virtual vector<TaskSubSolution*> setNextTraj(TaskSubSolution* doneTSS) =0;
    virtual bool computeSolutionTrajectories(int alternativeId);
    virtual bool computeSolutionTrajectories(TaskSolution *ts) = 0;

    virtual bool run(std::multimap<std::string,std::string> agents_name,
             std::multimap<std::string,std::string> objects_name,
             std::multimap<std::string,p3d_point> point3d,
             std::multimap<std::string,std::string> additionalData,
             WorldState* WS, bool computeMP);

    TaskSolution* findAlternative(bool computeMP);
    TaskSolution* createAlternative(bool computeMP);

    std::vector<confPtr_t> compute_manipulation_data(int armId, Robot *object, gpGrasp grasp);

    bool setMainObject(std::multimap<std::string,std::string> objects_name);

    Robot* getMainObject(){return mainObject;}

    virtual void showMainObjectRandomTransform();

    bool attachObject();
    bool detachObject();
    bool checkHRIAgent(Robot* r);
    bool setMainAgentHand();
    bool isObjectInCollision(Robot *supportObject);
    bool isHandInCollision();

    void removeVirtualObjects();
    void moveBackVirtualObjects();



    bool checkAttachement();

    bool checkAndSetArmAndObj();

    Robot* getMainObjectFromAtt();

    int getArmIdFromAtt();

    bool checkArmAtt();

protected:

    Robot* mainObject;

    confPtr_t graspConfClosed;
    confPtr_t graspConfOpen;
    confPtr_t approachConf;
    confPtr_t escapeConf;

    int currentTransfo;
    std::map<int, Robot*> _hands;
    gpGrasp _curGrasp;

    std::vector<std::pair<Robot*,configPt> > virtualObjs;

    std::vector<std::pair<double,TaskSolution*> > exploredSolutions;

    bool _noArm;


};

#endif // MANIPULATIONTASK_HPP