void consumer_controller_t::query_planner()
{
// PRX_DEBUG_S("Query planner");
PRX_DEBUG_COLOR("Query planner", PRX_TEXT_BLUE);
// If we haven't reached our final destination
if( active_queries && goal_index < (int)goal_states.size() )
{
// PRX_DEBUG_S("We're not done yet");
PRX_DEBUG_COLOR("Goal_index: " << goal_index, PRX_TEXT_LIGHTGRAY);
// Get the current state
child_state_space->copy_to_point(get_state);
// state_space->copy_to_point(init_state);
// Publish a ground truth message
((comm::simulation_comm_t*)comm::sim_comm)->publish_ground_truth((*state_memory[0]), planning_node, this->get_pathname());
// Make a start state vector
std::vector<double> start_state;
for( unsigned i = 0; i < child_state_space->get_dimension(); i++ )
{
start_state.push_back(get_state->at(i));
}
// Query planning
current_goal_state = goal_states[goal_index]->get_first_goal_point();
((comm::planning_comm_t*)comm::plan_comm)->publish_planning_query(
start_state, goal_states[goal_index]->get_goal_vec(), goal_states[goal_index]->get_radius(), pathname, planning_node, true, smooth, true, homogeneous_setup);
goal_index++;
queried_planner = true;
}
PRX_DEBUG_COLOR("END QUery planner", PRX_TEXT_BLUE);
}
bool unigripper_motoman_plant_t::IK_steering( const config_t& start_config, const config_t& goal_config, sim::plan_t& result_plan, bool set_grasping )
{
//Begin by determining a distance between the start and goal configurations
double distance = start_config.get_position().distance( goal_config.get_position() );
//Set the number of interpolation steps
double steps = std::ceil(distance / MAX_IK_STEP);
//Clear out the plan we were given, we will be filling it in ourselves
result_plan.clear();
//Keep around an intermediate plan for systematically building up the solution
plan_t intermediate_plan = result_plan;
//Store the arm's current state as the previous state
state_space->copy_to_point(prev_st);
//So first, let's do the IK for the start configuration
if( !IK_solver( start_config, prev_st, set_grasping, prev_st, true ) )
{
PRX_DEBUG_COLOR("IK failed in Steering for the start config.", PRX_TEXT_RED);
return false;
}
//Storage for the interpolated configuration
config_t interpolated_config;
//Then, for each step
for( double i=1; i<=steps; ++i )
{
//Compute the next interpolated configuration
double t = PRX_MINIMUM( (i/steps), 1.0 );
interpolated_config.interpolate( start_config, goal_config, t );
//Compute Minimum IK for interpolated configuration (Pose, seed state, soln)
if( IK_solver( interpolated_config, inter_st, set_grasping, prev_st, true ) )
{
//Create a plan by steering between states and add it to the resulting plan
intermediate_plan.clear();
steering_function(prev_st, inter_st, intermediate_plan);
result_plan += intermediate_plan;
//And make sure we store this state as our prior state
state_space->copy_point(prev_st, inter_st);
}
//If the MinIK fails, then the steering is a failure, abort
else
{
//Clear out whatever plan we had started making
PRX_DEBUG_COLOR("IK failed in Steering", PRX_TEXT_BROWN);
result_plan.clear();
return false;
}
}
//Report our Success
return true;
}
void consumer_controller_t::copy_plan(const plan_t& inplan)
{
// control_t* control = sys->pull_control_space()->alloc_point();
// for( unsigned int i = 0; i < control_msg.control.size(); ++i )
// sys->pull_control_space()->set_element(control, i, control_msg.control[i]);
PRX_DEBUG_COLOR("Received plan", PRX_TEXT_CYAN);
plan = inplan;
PRX_DEBUG_COLOR(plan.print(10), PRX_TEXT_LIGHTGRAY);
queried_planner = false;
// state_space->copy_from_point(init_state);
// push_state(init_state);
}
void radial_goal_region_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
PRX_DEBUG_COLOR("Inside radial goal region init!", PRX_TEXT_BLUE);
goal_state_t::init(reader, template_reader);
if( parameters::has_attribute("radius", reader, template_reader) )
{
PRX_DEBUG_S("Reading in goal radius");
radius = parameters::get_attribute_as< double >("radius", reader, template_reader);
PRX_DEBUG_COLOR("Radius is: " << radius, PRX_TEXT_CYAN);
}
else
{
PRX_FATAL_S("Missing radius attribute in input files for the radial_goal_region.");
}
}
void stopping_criteria_t::link_interruption_criteria(const std::vector<criterion_t*>& interruption_set)
{
PRX_DEBUG_COLOR ("Interruption criteria linked", PRX_TEXT_RED);
foreach(criterion_t* crit, interruption_set)
{
interruption_criteria.push_back(crit);
}
void empty_application_t::set_selected_path(const std::string& path)
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = reverse_split_path(path);
selected_path = name;
PRX_DEBUG_COLOR("The selected path is now: " << selected_path.c_str(),PRX_TEXT_GREEN);
}
void disk_t::update_phys_configs(config_list_t& configs, unsigned& index) const
{
if( ros::this_node::getName() == "planning" )
{
PRX_DEBUG_COLOR(config_names[0],PRX_TEXT_CYAN);
}
root_config.set_position(_x, _y, _z);
root_config.set_xyzw_orientation(0.0, 0.0, sin(_theta / 2.0), cos(_theta / 2.0));
plant_t::update_phys_configs(configs, index);
}
void stopping_criteria_t::reset()
{
PRX_DEBUG_COLOR ("Stopping criteria reset", PRX_TEXT_BROWN);
for (unsigned i = 0; i < stopping_criteria.size(); i++)
{
stopping_criteria[i]->reset();
}
for (unsigned i = 0; i < interruption_criteria.size(); i++)
{
interruption_criteria[i]->reset();
}
}
void motion_planner_t::set_param(const std::string& parameter_name, const boost::any& value)
{
PRX_DEBUG_COLOR("Motion Planner set_param : " << parameter_name, PRX_TEXT_MAGENTA);
if( parameter_name == "serialize_flag" )
serialize_flag = boost::any_cast<bool>(value);
else if( parameter_name == "deserialize_flag" )
deserialize_flag = boost::any_cast<bool>(value);
else if( parameter_name == "serialization_file" )
serialization_file = boost::any_cast<std::string>(value);
else if( parameter_name == "deserialization_file" )
deserialization_file = boost::any_cast<std::string>(value);
else
planner_t::set_param(parameter_name, value);
}
void pno_criterion_t::link_motion_planner(motion_planner_t* mp)
{
PRX_DEBUG_COLOR("Linking the motion planner...", PRX_TEXT_CYAN );
criterion_t::link_motion_planner( mp );
//I think we need to store a pointer explicitly to a prm* planner
prm_motion_planner = dynamic_cast<prm_star_t*>( mp );
if( prm_motion_planner == NULL )
PRX_FATAL_S("PNO Criterion requires the linked motion planner be a PRM*");
linked_space = mp->state_space;
total_volume = linked_space->space_size();
d = linked_space->get_dimension();
PRX_PRINT("Working in a " << d << " dimensional space, of size: " << total_volume, PRX_TEXT_LIGHTGRAY );
free_volume = total_volume;
//compute_limit();
//Open up an appropriate file for output...
char* w = std::getenv("PRACSYS_PATH");
std::string fname(w);
fname += ("/prx_output/pno_data/");
//fname += "dim_" + int_to_str( d ) + "_" + int_to_str( time(NULL) ) + ".txt";
fname += "dim_" + int_to_str( d ) + "_" + ros::this_node::getName().substr(1,ros::this_node::getName().length()) + ".txt";
stat_out.open( fname.c_str() );
}
void consumer_controller_t::read_in_goals(const parameter_reader_t * reader, const parameter_reader_t* template_reader)
{
std::vector<const parameter_reader_t*> goal_readers, template_goal_readers;
if( reader->has_attribute("goals") )
goal_readers = reader->get_list("goals");
else
{
PRX_FATAL_S("No goals to read!");
}
std::string template_name;
parameter_reader_t* child_template_reader = NULL;
foreach(const parameter_reader_t* r, goal_readers)
{
PRX_DEBUG_COLOR("Goal reading!", PRX_TEXT_CYAN);
radial_goal_region_t* goal_region = new radial_goal_region_t();
if( r->has_attribute("goal/template") )
{
template_name = r->get_attribute("goal/template");
// TODO: Find a way to load templates under namespaces more cleanly.
child_template_reader = new parameter_reader_t(ros::this_node::getName() + "/" + template_name);
}
goal_region->init(r->get_child("goal").get(), child_template_reader);
goal_region->link_space(child_state_space);
goal_states.push_back(goal_region);
if( child_template_reader == NULL )
{
delete child_template_reader;
child_template_reader = NULL;
}
}
void consumer_controller_t::compute_control()
{
// PRX_DEBUG_COLOR ("Begin compute control", PRX_TEXT_BLUE);
control_t* new_control = plan.get_next_control(simulation::simulation_step);
if( active_queries && goal_index < (int)goal_states.size() && new_control == NULL && !queried_planner )
{
PRX_DEBUG_COLOR("Query planner", PRX_TEXT_MAGENTA);
query_planner();
}
// Case 1: Our plan has given us a non-NULL control
if( new_control != NULL )
{
// PRX_DEBUG_COLOR("New control not NULL", PRX_TEXT_CYAN);
// PRX_INFO_S("********consumer control : " << output_control_space->print_point(new_control));
output_control_space->copy_point(computed_control, new_control);
// PRX_WARN_S("keep_last_control : " << keep_last_control << " c size:" << plan.steps.size());
if( plan.size() == 0 )
{
last_control = new_control;
// PRX_LOG_INFO("Plan size : %u || Holding control", plan.steps.size() );
}
_time = _time + simulation::simulation_step;
}
// Case 2: Last control has been set to a valid control, and we want to keep our last control
else if( last_control != NULL && keep_last_control )
{
// PRX_DEBUG_COLOR("Use last control", PRX_TEXT_LIGHTGRAY);
new_control = last_control;
}
else if( keep_last_state )
{
new_control = get_state;
}
// Case 3: If all else fails, use the contingency plan (system specific)
else
{
// PRX_DEBUG_COLOR("Contingency plan", PRX_TEXT_BROWN);
new_control = contingency_plan.get_control_at(0);
}
// if (queried_planner)
// {
// PRX_DEBUG_COLOR("Queried planner true!", PRX_TEXT_RED);
// }
// else
// {
// PRX_DEBUG_COLOR("Queried planner false!", PRX_TEXT_GREEN);
// }
// Check if we still need to query planner
//PRX_DEBUG_COLOR("New control: " << this->output_control_space->print_point(new_control), PRX_TEXT_MAGENTA);
output_control_space->copy_from_point(new_control);
subsystems.begin()->second->compute_control();
// PRX_DEBUG_COLOR("End control", PRX_TEXT_BLUE);
}
