HRICS_otpmotionpl.hpp

#ifndef HRICS_OTPMOTIONPL_HPP
#define HRICS_OTPMOTIONPL_HPP

/*
 *  HRICS_CostSpace.h
 *  BioMove3D
 *
 *  Created by Mamoun Gharbi on 20/04/11.
 *  Copyright 2011 magharbi@laas.fr All rights reserved.
 *
 */
#include "API/planningAPI.hpp"

#include "API/Trajectory/trajectory.hpp"
#include "planner/planner.hpp"

#include "HRICS_Workspace.hpp"

#include "Grid/HRICS_EnvGrid.hpp"
#include "Grid/HRICS_TwoDGrid.hpp"
#include "utils/OtpUtils.hpp"
#include "utils/ConfGenerator.h"

#include "LightPlanner-pkg.h"
#include "planner/TrajectoryOptim/plannarTrajectorySmoothing.hpp"




#include <Eigen/Core>
#include <Eigen/StdVector>

extern std::string global_ActiveRobotName;
extern API::TwoDGrid* API_activeRobotGrid;

extern Eigen::Vector3d current_WSPoint;
extern std::pair<double,Eigen::Vector3d > current_cost;


void hrics_otp_fct();
namespace HRICS
{
    class Trajectory;

    class OTPMotionPl : public HumanAwareMotionPlanner
    {

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;


        OTPMotionPl();
        OTPMotionPl(Robot* R, Robot* H);

        ~OTPMotionPl();

        //getters and setters
        Robot* getHuman() { return m_Human; }
        Robot* getRobot() { return _Robot; }

        EnvGrid* getPlanGrid() { return m_2DGrid; }
        std::vector<Eigen::Vector3d> getOTPList() { return m_OTPList; }
        int getNbConf(){ return m_confList.size(); }

        Eigen::Vector3d getHumanActualPos();
        Eigen::Vector3d getRobotActualPos();

        void clearOTPList() { m_OTPList.clear(); }

        ConfGenerator* getConfGenerator();

        std::vector<double> getHumanPos(){return m_humanPos;}
        Eigen::Vector3d getRobotPos(){return m_robotPos;}

        std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > getRobotTraj(){return m_2DPath;}
        void setRobotTraj(std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > traj){m_2DPath = traj;}

        std::vector<double> getMultipleData(){return m_multipleData;}

        double getInitTime() {return initTime;}
        void setInitTime(double d) {initTime = d;}
        bool finalConfAchieved(double humanError, double robotError);
          

        void compteTraj(bool compute);

        void draw2dPath();

        bool changeHumanByName(std::string humanName);

        //##########################################
        //functions for changing config list #######
        //##########################################

        //about the robot
        bool ComputePR2Gik();

        void initPR2GiveConf();

        void placeHuman();


        // about the OTP list (of point to compute GIK)


        int loadConfsFromXML(std::string filename, bool isStanding, bool isSlice);

        int getConfListSize();

        configPt getRobotConfigAt(int i);

        std::vector<ConfigHR> getConfList();

        // after loading confs, computing distances
        std::pair<double,double> computeHumanRobotDist();

        double getHumanRobotDist();

        //##########################################
        //Tools for Computing OTP ##################
        //##########################################

        //initialisation
        void initGrid();

        void initAll();

        //Place from a stored config
        std::pair<std::tr1::shared_ptr<Configuration>, std::tr1::shared_ptr<Configuration> > setRobotsToConf(int id, bool isStanding);

        std::pair<std::tr1::shared_ptr<Configuration>, std::tr1::shared_ptr<Configuration> > setRobotsToConf(int id, bool isStanding, double x, double y, double Rz);

        // saving and reloading a configuration
        void saveInitConf();

        void loadInitConf(bool reloadHuman, bool reloadRobot);

        void clearConfList();

        int copyConfList();

        //actual algorithm
        double ComputePlanarDistancesLineToSegment(Eigen::Vector2d p, Eigen::Vector2d p1, Eigen::Vector2d p2);

        bool getRandomPoints(double id, Vector3d& vect);

        bool isAlreadyTested(Eigen::Vector3d p);

        bool newComputeOTP();

        OutputConf lookForBestLocalConf(double x, double y, double Rz, double objectNecessity);

        OutputConf findBestPosForHumanSitConf(double objectNecessity);

        // redundant call of OTP Computing
        void getInputs();

        void setInputs( Eigen::Vector3d humanPos, Eigen::Vector3d robotPos,bool isStanding, double mobility);

        void addVectorToRealTreajectory(Eigen::Vector2d vect);

        void setRobotPos();

        void clearRealTreajectory(){m_2DHumanRealPath.clear();}

        bool isTheRealNearThePredicted(double threshold);

        //other
        void sortConfigList(double nbNode, bool isStanding, bool isSlice);

        bool testCol(bool isHuman, bool useConf);

        bool standUp();


        // Showing the configurations
        double showConf(unsigned int i);

        OutputConf showBestConf();

        std::tr1::shared_ptr<Configuration> getBestConf();

        void showBestConfRobOnly();

        double getConfigCost();

        //creating trajectories
        bool createTrajectoryFromOutputConf(OutputConf conf);

        std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > smoothTrajectory(
                std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > trajectory);

        bool testCurrentTraj();

        void initTrajTest();

        void navigate();

        void goToNextStep();

        // results functions
        void saveCostsTofile(double cost, double randomCost);

        void saveAllCostsToFile();

        void saveAlltimesToFile();

        void saveCostsTofile(std::string cost);

        void clearCostsfile();

        double multipliComputeOtp(int n);

        //##########################################
        //Tools for Using from as lib ##############
        //##########################################

        //about cells
        double getDistFromCell(int x, int y, bool isHuman);

        double getrotFromCell(int x, int y);

        //about values
        void dumpCosts();

        void dumpVar();

        // using from mhp
        void setVar();

        bool getOtp(std::string humanName, Eigen::Vector3d &dockPos, std::vector<SM_TRAJ>& smTraj, configPt& handConf,bool isStanding, double objectNessecity);


        bool getOtp(std::string humanName, Eigen::Vector3d &dockPos, std::vector<std::pair<double,double> >& traj, configPt& handConf,bool isStanding, double objectNessecity);

        bool getOtp(std::string humanName, std::vector<std::vector<double> >& traj, configPt& handConf,bool isStanding, double objectNessecity);


        bool InitMhpObjectTransfert(std::string humanName);

        bool testTrajectories(bool fullbody);


        bool getSimplePath(double x, double y, double theta, std::vector<std::vector<double> >& path);

        bool hasHumanMovedAccordingToPlan(double error);

    private:

        Robot* m_Human;

        EnvGrid* m_2DGrid;

        std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> >   m_2DPath;

        std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > m_robotTraj3D;

        std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> >   m_2DHumanPath;

        std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> >   m_2DHumanRealPath;

        ManipulationPlanner* m_ManipPl;

        ManipulationPlanner* m_ManipPlHum;

        bool m_PathExist;

        bool m_HumanPathExist;

        std::vector<Eigen::Vector3d> m_OTPList;

        std::vector<ConfigHR> m_configList;

        std::vector<ConfigHR> m_sittingConfigList;

        std::vector<ConfigHR> m_configListSlice;

        std::vector<ConfigHR> m_sittingConfigListSlice;

        OutputConf m_savedConf;

        std::vector<OutputConf> m_confList;

        bool m_isInitSiting;

        Eigen::Vector3d m_sliceVect;

        bool m_humanCanStand;

        double m_time;

        double m_sittingTime;

        std::vector<double> m_multipleData;

        Robot* m_simpleChair;

        std::vector<std::vector<double> > m_multipliComputeCostVector;

        std::vector<std::vector<double> > m_multipliComputeTimeVector;

        std::vector<double> m_costVector;

        std::vector<double> m_timeVector;

        std::vector<SM_TRAJ> m_smTrajs;

        PlannarTrajectorySmoothing* m_pts;

        std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> >   m_testedVectors;

//        Eigen::Vector3d m_humanPos;
        std::vector<double> m_humanPos;
        Eigen::Vector3d m_robotPos;
        bool m_isStanding;
        double m_mobility;

        ConfGenerator* m_ConfGen;

        double initTime;

        bool m_OTPsuceed;



    };



}

#endif // HRICS_OTPMOTIONPL_HPP