goal_bias = [0.5, 0.5, 0, 0, 0, 0]
################################################# PLAN
constraints = lqrrt.Constraints(nstates=nstates,
ncontrols=ncontrols,
goal_buffer=goal_buffer,
is_feasible=is_feasible)
planner = lqrrt.Planner(dynamics,
lqr,
constraints,
horizon=2,
dt=0.1,
FPR=0.5,
error_tol=error_tol,
erf=erf,
min_time=1,
max_time=2,
max_nodes=1E5,
goal0=goal)
planner.update_plan(x0, sample_space, goal_bias=goal_bias, finish_on_goal=True)
################################################# SIMULATION
# Prepare "real" domain
dt = 0.03 # s
T = planner.T + 10 # s
t_arr = np.arange(0, T, dt)
framerate = 10
K = np.hstack((kp.dot(R.T), kd))
return (S, K)
################################################# HEURISTICS
goal_buffer = [0.5*free_radius, 0.5*free_radius, np.inf, np.inf, np.inf, np.inf]
error_tol = np.copy(goal_buffer)/10
def gen_ss(seed, goal):
"""
Returns a sample space given a seed state and goal state.
"""
return [(min([seed[0], goal[0]]) - ss_buff, max([seed[0], goal[0]]) + ss_buff),
(min([seed[1], goal[1]]) - ss_buff, max([seed[1], goal[1]]) + ss_buff),
(-np.pi, np.pi),
(0.9*velmax_pos[0], velmax_pos[0]),
(-abs(velmax_neg[1]), velmax_pos[1]),
(-abs(velmax_neg[2]), velmax_pos[2])]
################################################# MAIN ATTRIBUTES
constraints = lqrrt.Constraints(nstates=nstates, ncontrols=ncontrols,
goal_buffer=goal_buffer, is_feasible=unset)
planner = lqrrt.Planner(dynamics, lqr, constraints,
horizon=horizon, dt=dt, FPR=FPR,
error_tol=error_tol, erf=unset,
min_time=basic_duration, max_time=basic_duration, max_nodes=max_nodes,
sys_time=unset, printing=False)