def _test_init(init_type, start_pos, goal_pos, reverse):
dt = 0.1
max_v = 0.2
max_w = 0.4
init_states = position_control_init_fn(init_type, start_pos, goal_pos, dt,
max_v, max_w, reverse)
file_name = 'position_{:s}_{:s}_reverse{:d}-{:s}.pdf'
goal_pos_str = ['{:0.1f}'.format(x) for x in goal_pos]
file_name = file_name.format(','.join(goal_pos_str), init_type,
int(reverse), _get_time_str())
tmp_dir = os.path.join(os.path.dirname(__file__), 'tmp')
_mkdir_if_missing(tmp_dir)
file_name = os.path.join(tmp_dir, file_name)
#_plot_test(init_states, start_pos, goal_pos, file_name)
_states_close(init_states[-1, :], goal_pos)
def _compute_trajectory_no_map(self, start_pos, goal_pos, smooth, reverse):
typ = 'smooth' if smooth else 'sharp'
init_states = position_control_init_fn(typ, start_pos, goal_pos,
self.dt, self.max_v, self.max_w,
reverse)
return init_states