prada_planner_alg.cpp

Go to the documentation of this file.

#include "prada_planner_alg.h"

PradaPlannerAlgorithm::PradaPlannerAlgorithm(void)
{
        state = NULL;
        planner = NULL;
        rules = NULL;
        reward = NULL;
        srand (time(NULL));
        rnd.seed(rand());
}

PradaPlannerAlgorithm::~PradaPlannerAlgorithm(void)
{
}

void PradaPlannerAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
        bool file_changed = false;
        this->lock();

        if (this->config_.conf_file_language.compare(new_cfg.conf_file_language) != 0) {
                this->conf_file_language = new_cfg.conf_file_language;
                file_changed = true;
        }
        if (this->config_.conf_file_reward.compare(new_cfg.conf_file_reward) != 0) {
                this->conf_file_reward = new_cfg.conf_file_reward;
                file_changed = true;
        }
        if (this->config_.conf_file_rules.compare(new_cfg.conf_file_rules) != 0) {
                this->conf_file_rules = new_cfg.conf_file_rules;
                file_changed = true;
        }
        if (this->config_.conf_file_state.compare(new_cfg.conf_file_state) != 0) {
                this->conf_file_state = new_cfg.conf_file_state;
                file_changed = true;
        }
        
        // prada
        this->max_prada_horizon = new_cfg.max_prada_horizon;
        this->prada_discount_factor = new_cfg.prada_discount_factor;
        this->prada_num_samples = new_cfg.prada_num_samples;
        this->prada_noise_softener = new_cfg.prada_noise_softener;
        
        // true if erasing a board
        this->board_plan = new_cfg.board_plan;
        
        // Get new config
        if (file_changed) {
                initializePlan();
        }
        
        // save the current configuration
        this->config_=new_cfg;
        
        this->unlock();
}

// PradaPlannerAlgorithm Public API
void PradaPlannerAlgorithm::initializePlan()
{
        // -------------------------------------
        //  SET UP LOGIC
        // -------------------------------------
        
        // Set up logic
        ROS_INFO("Reading symbols.");
        relational::SymL symbols;
        relational::ArgumentTypeL types;
        relational::readSymbolsAndTypes(symbols, types, conf_file_language.c_str());


        // -------------------------------------
        // REWARD
        // -------------------------------------
        ROS_INFO("Reading reward");
        // TODO PRADA doesn't support yet reading rewards from a file
        // reward = relational::Reward::read(conf_file_reward.c_str());
        
        // Workaround for literal list
//      relational::LitL lits_reward;
//      relational::Literal::get(lits_reward, MT::String(conf_file_reward.c_str()));
//      reward = new relational::LiteralListReward(lits_reward);
        
        // Workaround for maximize reward
        reward = new relational::MaximizeReward(relational::Literal::get("numClean()"));
           
        
        
        
        // -------------------------------------
        // RULES
        // -------------------------------------
        
        ROS_INFO("Reading rules.");
        // TODO check
        //delete rules;
        rules = new relational::RuleSet();
        relational::RuleSet::read(conf_file_rules.c_str(), *rules);
        
        #if  0
        // Manually create "doNothing"-rule if desired
        relational::Rule* rule_doNothing = relational::Rule::getDoNothingRule();
        rules.append(rule_doNothing);
        rule_doNothing->write();
        #endif
        
        ROS_INFO("Finished reading configuration.");
}

void PradaPlannerAlgorithm::setPlanStateFromFile()
{
        if (state != NULL) {
                delete state;
        }
        state = new relational::SymbolicState();
//         relational::SymbolicState state;
        
        this->lock();
        ifstream in_state(this->conf_file_state.c_str());
        ROS_DEBUG("Reading state");
        state->read(in_state);
        relational::reason::setConstants(state->state_constants);
        ROS_DEBUG("State readed successfully");
        setupPlanner(state);
        
        this->unlock();
}

void PradaPlannerAlgorithm::setupPlanner(relational::SymbolicState *state) {
        // -------------------------------------
        // GROUND THE RULES
        // -------------------------------------
        
        //relational::RuleSet ground_rules;
        relational::RuleSet ground_rules;
//      ground_rules = new relational::RuleSet;
        relational::RuleSet::ground_with_filtering(ground_rules, *rules, relational::reason::getConstants(), *state);
        
        
        // -------------------------------------
        // PLANNER
        // -------------------------------------
        if (planner!= NULL) {
                delete planner;
        }
        planner = new relational::A_PRADA();
        // PRADA parameters
        ((relational::A_PRADA* ) planner)->setNumberOfSamples(prada_num_samples);
        ((relational::A_PRADA* ) planner)->setNoiseSoftener(prada_noise_softener);
        planner->setDiscount(prada_discount_factor);
        planner->setHorizon(max_prada_horizon);
        planner->setReward(reward);
        planner->setGroundRules(ground_rules);
        
        
}


relational::Literal* PradaPlannerAlgorithm::getSinglePlan(uint32_t plan_length) {
        this->lock();
//      TL::Atom* action;
        relational::Literal* action;
        
        ROS_INFO("Planning...");
        
        planner->setHorizon(plan_length);
        action = planner->plan_action(*state);
//      action = planner->generateAction(*state);
        if (action){
                ROS_INFO_STREAM("The planner would like to kindly recommend the following action to you:"<<endl<<*action<<endl);
        }
        else{
                ROS_INFO("Planning failed!");
        }
        //std::stringstream ss;
        //ss << *action;
        
        this->unlock();
        return action;
}

relational::LitL PradaPlannerAlgorithm::getCompletePlan(uint32_t plan_length) {
        this->lock();
//      AtomA plan;
        relational::LitL plan;
        double value;
        
        ROS_DEBUG_STREAM("PRADA new horizon " << plan_length);
        planner->setHorizon(plan_length);
//      ((relational::PRADA*) planner)->generatePlan(plan, value, *state);
        ((relational::PRADA_Planner*) planner)->plan_full(plan, value, *state);
        cout<<endl;
        cout<<"Also, it would like to kindly recommend the following plan with value "<<value<<" to you:"<<endl<<plan<<endl;
        
        this->unlock();
        return plan;
}

uint32_t PradaPlannerAlgorithm::getRecommendedPlanLength(uint32_t ellipses_size)
{
        if (this->board_plan) {
                // plan length is (#ellipses)^2 + 4
                // max_planning_horizon is a parameter to define max plan length
                return std::min((int32_t)(ellipses_size*ellipses_size) + 4, this->max_prada_horizon);
        }
        else{
                // plan length is (#ellipses)*3
                // max_planning_horizon is a parameter to define max plan length
                return std::min((int32_t)(ellipses_size*3), this->max_prada_horizon);
        }
}

void PradaPlannerAlgorithm::reloadRules()
{
        ROS_DEBUG("Reading rules.");
        delete rules;
        rules = new relational::RuleSet();
        relational::RuleSet::read(conf_file_rules.c_str(), *rules);
}