SoftmotionReplanner.hpp

/*
 *  SoftmotionReplanner.hpp
 *  libmove3D-planners 
 *
 *  Created by Benjamin Vadant on 01/02/13.
 *  Based on Jim Mainprice replanningAlgorithms.hpp/cpp
 *  Copyright 2013 LAAS/CNRS. All rights reserved.
 *
 */

#ifndef SOFTMOTION_REPLANNER_HPP
#define SOFTMOTION_REPLANNER_HPP

#include "planner/Replanning/Replanner.hpp"
#include "API/Trajectory/trajectory.hpp"
#include "API/ConfigSpace/configuration.hpp"
#include "API/Roadmap/graph.hpp"

#include "HRI_costspace/HRICS_Navigation.hpp"

#include "utils/ConfGenerator.h"

#include "P3d-pkg.h"
#include "LightPlanner-pkg.h"

#include <string>

class SoftmotionReplanner : public Replanner
{   
public:
    SoftmotionReplanner(Robot* r);
    ~SoftmotionReplanner();
    
    bool init();
    void run();
    
private:
    //bool init_manipulationPlanner();
    bool computeSoftmotion(MANPIPULATION_TRAJECTORY_CONF_STR &confs, p3d_traj* traj, SM_TRAJ& smTraj);
    bool computeSoftmotion(p3d_traj* traj);
    bool generateNewTrajectory(const std::vector<p3d_traj*>& trajs);
    
    ManipulationPlanner* manipulationPlanner;
};

#endif