privateTask.hpp

#ifndef PRIVATETASK_HPP
#define PRIVATETASK_HPP

#include "task.hpp"

class PrivateTask : public Task
{
    MOVE3D_STATIC_LOGGER;
public:
    PrivateTask();
    virtual bool initAll();
    virtual bool initialize();
    virtual bool finalize();
    virtual bool findCandidateSolutions() = 0;
    //virtual bool setToSolution(int solutionId, bool computeMP);
    virtual bool getRandomSol() = 0;
    virtual bool testConstraints(int alternativeId);
    virtual bool testConstraints(TaskSolution *TS);
//    virtual vector<TaskSubSolution*> setNextTraj(TaskSubSolution* doneTSS) =0;

    bool isPathStraight(Robot *r,p3d_traj *t);


    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);
    virtual double computeCost(int altId);
    virtual TaskSolution* findAlternative(bool computeMP);
    armLabel getArmLabelFromId(int armId);
    bool setArmToUse(armLabel arm_t);
    bool setArmToUse(int armId);
    bool initMotionPlaner(Robot *rob);
    p3d_traj* compute_traj(confPtr_t q_init, confPtr_t q_goal, Robot* object, int& status);
    p3d_traj* compute_traj_using_localPath(confPtr_t q_init, confPtr_t q_goal, Robot* object, int& status, bool testingStraightPath = true);

    virtual bool attachObject(Robot* rob, Robot* obj, bool attach);
    virtual bool attachObject(Robot *rob, Robot *obj, bool attach, int armId); //true for attaching, false for detaching

    bool computeInverseKinematic(Robot* ag, p3d_matrix4 target_pos, confPtr_t  &solution, int armId, bool fromCurPos);
    bool computeManipulationConfs(confPtr_t &approachC, confPtr_t &graspC, confPtr_t &graspOC, confPtr_t &escapeC, p3d_matrix4 objectPosMat,
                                  gpGrasp grasp, p3d_vector3 approachDir, double approachDist, p3d_vector3 escapeDir, double escapeDist);

    //this is a specific function used in case we need ids.
    int getArmId();

    bool setMainAgent(std::multimap<std::string,std::string> agents_name);

    Robot* getMainAgent(){return mainAgent;}

    virtual bool isPossibleMainAgent(Robot* r);

    void openGripper(confPtr_t &q, bool open, int armId);

    void activateFingertipCol(bool val, Robot* robi, Robot *object, Robot *supportObj);

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



protected:
    std::vector<GTPCell> candidatePoints;
    //trajs
    //configurations
    //WorldState
    Robot* mainAgent;
    armLabel m_arm;
    gpHand_type handType;
    int _armId;

    std::map<std::string, ManipulationPlanner*> ManipPlMap;

    std::vector<HRI_AGENT_TYPE> _possibleMainAgentType;

};

#endif // PRIVATETASK_HPP