void time_varying_local_planner_t::steer(const state_t* start, const state_t* goal, plan_t& plan, state_t* result, bool connect)
{
int old_num_controls = num_controls;
if( !connect )
{
num_controls = 1;
}
for( int i = 0; i < num_controls; i++ )
{
plans[i].link_control_space(world_model->get_control_space());
plans[i].link_state_space(world_model->get_state_space());
plans[i].clear();
int random_number = uniform_int_random(lower_multiple, upper_multiple);
plans[i].link_control_space(world_model->get_control_space());
plans[i].clear();
plans[i].append_onto_back(random_number*simulation::simulation_step);
sampler->sample(world_model->get_control_space(), plans[i].back().control);
propagate_step(start, plans[i], result);
plans[i].copy_end_state(result);
}
double dist = metric->distance_function(plans[0].get_end_state(), goal);
unsigned index = 0;
for( int i = 1; i < num_controls; i++ )
{
double new_dist = metric->distance_function(plans[i].get_end_state(), goal);
if( new_dist < dist )
{
dist = new_dist;
index = i;
}
}
plan = plans[index];
world_model->get_state_space()->copy_point(result, plans[index].get_end_state());
num_controls = old_num_controls;
}
void gradient_descent_local_planner_t::steer(const state_t* start, const state_t* goal, plan_t& plan, state_t* result, bool connect)
{
if( connect )
{
gradient_steer(start, goal, plan);
}
else
{
plan.clear();
plan.append_onto_front(floor(max_multiple * rand() / RAND_MAX) * simulation::simulation_step);
sampler->sample(world_model->get_control_space(), plan[0].control);
}
propagate_step(start, plan, result);
}
void gradient_descent_local_planner_t::gradient_steer(const state_t* start, const state_t* goal, plan_t& plan)
{
//for now only going to do piecewise constant plans
plan.clear();
traj.link_space(world_model->get_state_space());
plan.append_onto_front(max_multiple * simulation::simulation_step);
const space_t* control_space = world_model->get_control_space();
sampler->sample(control_space, plan[0].control);
unsigned count = 0;
std::vector<double> old_control(control_space->get_dimension());
std::vector<double> test_control(control_space->get_dimension());
std::vector<double> control_below(control_space->get_dimension());
std::vector<double> control_above(control_space->get_dimension());
std::vector<std::pair<double, double> > diffs(control_space->get_dimension());
while( count < attempts )
{
traj.clear();
plan[0].duration = max_multiple * simulation::simulation_step;
propagate(start, plan, traj);
unsigned num_sim_steps = 0;
unsigned best_sim_step = 0;
double best_dist = PRX_INFINITY;
for( trajectory_t::iterator it = traj.begin(); it != traj.end(); it++ )
{
num_sim_steps++;
if( metric->distance_function(*it, goal) < best_dist )
{
best_dist = metric->distance_function(*it, goal);
best_sim_step = num_sim_steps;
}
}
plan[0].duration = best_sim_step * simulation::simulation_step;
if( best_dist < accepted_radius )
{
return;
}
else
{
state_t* state = world_model->get_state_space()->alloc_point();
for( unsigned i = 0; i < control_space->get_dimension(); i++ )
{
old_control[i] = plan[0].control->at(i);
control_below[i] = old_control[i] - .01 * (control_space->get_bounds()[i]->get_upper_bound() - control_space->get_bounds()[i]->get_lower_bound());
control_above[i] = old_control[i] + .01 * (control_space->get_bounds()[i]->get_upper_bound() - control_space->get_bounds()[i]->get_lower_bound());
diffs[i].first = diffs[i].second = 0;
}
for( unsigned i = 0; i < control_space->get_dimension(); i++ )
{
test_control = old_control;
test_control[i] = control_below[i];
control_space->set_from_vector(test_control, plan[0].control);
propagate_step(start, plan, state);
diffs[i].first = metric->distance_function(state, goal);
test_control[i] = control_above[i];
control_space->set_from_vector(test_control, plan[0].control);
propagate_step(start, plan, state);
diffs[i].second = metric->distance_function(state, goal);
}
world_model->get_state_space()->free_point(state);
//now that all the differences have been computed, determine the direction to move
test_control = old_control;
for( unsigned i = 0; i < control_space->get_dimension(); i++ )
{
test_control[i] += (diffs[i].first - diffs[i].second)*(learning_rate);
}
control_space->set_from_vector(test_control, plan[0].control);
}
count++;
}
// PRX_INFO_S(plan.print());
}
