HRICS_Workspace.hpp

#ifndef HRICS_PLANNER_H
#define HRICS_PLANNER_H

/*
 *  HRICS_CostSpace.h
 *  BioMove3D
 *
 *  Created by Jim Mainprice on 04/12/09.
 *  Copyright 2009 mainprice@laas.fr All rights reserved.
 *
 */

#include "API/planningAPI.hpp"
#include "planner/planner.hpp"
#include "planner/Diffusion/RRT.hpp"

#include "HRICS_Distance.hpp"
#include "HRICS_Visibility.hpp"
#include "HRICS_Natural.hpp"
#include "Grid/HRICS_Grid.hpp"
#include "Grid/HRICS_GridState.hpp"
#include "Grid/HRICS_AgentGrid.hpp"

namespace HRICS
{

        class HumanAwareMotionPlanner : public Planner 
        {
                
        public:
                HumanAwareMotionPlanner() : 
                Planner(),
                m_DistanceSpace(NULL),
                m_VisibilitySpace(NULL),
                m_ReachableSpace(NULL),
                m_NaturalSpace(NULL) { }
                
                HumanAwareMotionPlanner(Robot* rob, Graph* graph) : 
                Planner(rob,graph),
                m_DistanceSpace(NULL),
                m_VisibilitySpace(NULL),
                m_ReachableSpace(NULL),
                m_NaturalSpace(NULL) { }
                
                ~HumanAwareMotionPlanner() 
                {
                        delete m_DistanceSpace;
                        delete m_VisibilitySpace;
                }
                
                Distance*       getDistance() { return m_DistanceSpace; }
                Visibility* getVisibility() { return m_VisibilitySpace; }
                Natural*        getNaturality() { return m_NaturalSpace; }
                Natural*        getReachability() { return m_ReachableSpace; }
                
                
                void setDistance(Distance* dist) 
                { 
                        if( m_DistanceSpace )
                        { 
                                delete m_DistanceSpace;
                        }
                        m_DistanceSpace = dist;
                }
                
                void setVisibility(Visibility* visib) 
                { 
                        if( m_VisibilitySpace )
                        { 
                                delete m_VisibilitySpace;
                        }
                        m_VisibilitySpace = visib;
                }
                
                void setNatural(Natural* nat) 
                { 
                        if( m_NaturalSpace )
                        { 
                                delete m_NaturalSpace;
                        }
                        m_NaturalSpace = nat;
                }
                
                void setReachability(Natural* nat) 
                { 
                        if( m_ReachableSpace )
                        { 
                                delete m_ReachableSpace;
                        }
                        m_ReachableSpace = nat;
                }
                
                unsigned int run() { return 0; }
                
        protected:
                Distance*                       m_DistanceSpace;
                Visibility*             m_VisibilitySpace;
                Natural*                        m_ReachableSpace;
                Natural*                        m_NaturalSpace;
                
        };
        
        class Workspace : public HumanAwareMotionPlanner
        {
                
    public :
                
                Workspace();
                Workspace(Robot* rob, Graph* graph);
                ~Workspace();
                
                void initGrid();
                void deleteGrid();
    
    void initAgentGrids(double cellsize);
    void deleteAgentGrids();
    std::vector<AgentGrid*> getAgentGrids() { return m_HumanGrids; }
    
                void initDistance();
                void initVisibility();
                void initReachable();
                void initNatural();
    void initOtpPlanner();
#ifdef HRI_PLANNER
    void setAgents( HRI_AGENTS* agents ) { m_Agents = agents; }
#endif
                
                Robot* getHuman(){ return mHumans[0]; }
    std::vector<Robot*> getHumans(){ return mHumans; }
                Robot* getRobot(){ return _Robot; }
    Eigen::Vector3d getVisball();
    
                bool computeAStarIn3DGrid();
                double pathCost();
                void draw3dPath();
                
                double distanceToEntirePath();
                double distanceToCellPath();
                
                Grid* getGrid() { return m3DGrid; }
                std::vector<Eigen::Vector3d> get3DPath() { return m3DPath; }
                std::vector<API::ThreeDCell*> getCellPath() { return m3DCellPath; }
                int getIndexObjectDof() 
                { 
                        if(mIndexObjectDof==0)
                        {       
                                std::cout << "Workspace::Warning::mIndexObjectDof == 0" << std::endl;
                        }
                        return mIndexObjectDof; 
                }
                
                bool initHriRRT();
                
                void deactivateOnlyBaseCollision();
                void activateOnlyBaseCollision();
    
                bool sampleRobotBase(std::tr1::shared_ptr<Configuration> q_base, const Eigen::Vector3d& WSPoint);
    
                bool transPFromBaseConf(std::tr1::shared_ptr<Configuration> q_base, std::vector< Eigen::Vector3d > points );
                bool baseInSight(std::tr1::shared_ptr<Configuration> q_base);
                
    void setCurrentOTP(const Eigen::Vector3d& p) { m_OTP = p; }
    
    Eigen::Vector3d getCurrentOTP() { return m_OTP; }
    
    bool testCol(std::tr1::shared_ptr<Configuration> q_base);

    void initPR2RepoConf();

    void initPR2GiveConf();

    void initPR2AndHumanTest();

    void computePR2GIK(bool move);

    void ChangeRobotPos(double value);

    void drawCurrentOTP();
    
    double* testTransferPointToTrajectory( const Eigen::Vector3d& WSPoint, API::Trajectory& traj, unsigned int& id);


    bool chooseBestTransferPoint(Eigen::Vector3d& transfPoint, bool move, unsigned int threshold);

        bool computeBestTransferPoint(Eigen::Vector3d& transfPoint);

    bool computeBestFeasableTransferPoint(Eigen::Vector3d& transfPoint);

    bool ComputeTheObjectTransfertPoint(bool Move, int type, Eigen::Vector3d& transfPoint, int threshold);

    Eigen::Vector3d computeOTPFromHandPose( bool rightHand );

        private:
                
                void solveAStar(State* start,State* goal);
                
                std::vector<Robot*>     mHumans;
                
    int mIndexObjectDof;
    
    Eigen::Vector3d m_OTP;
    
                Grid*                   m3DGrid;
    std::vector<AgentGrid*> m_HumanGrids;

#ifdef HRI_PLANNER
                HRI_AGENTS*             m_Agents;
#endif
                
                bool mPathExist;
                std::vector<Eigen::Vector3d>   m3DPath;
                std::vector<API::ThreeDCell*>  m3DCellPath;
        };
}

#endif