""" USER INTERFACES
- Define grid
- Generate initial values for grid using shape functions
- Time length for computations
- Initialize plotting option
- Call HJSolver function
"""
# Second Scenario
g = Grid(np.array([-3.0, -1.0, 0.0, -math.pi]),
np.array([3.0, 4.0, 4.0, math.pi]),
4, np.array([60, 60, 20, 36]), [3])
pdb.set_trace()
# Define my object
my_car = DubinsCar4D2(x = None,
uMode = 'max')
# Use the grid to initialize initial value function
Initial_value_f = Lower_Half_Space(g, 1, 0) # Left halfplane of X
#Initial_value_f = CylinderShape(g, [2,3], np.zeros(4), 1)
pdb.set_trace()
# Look-back lenght and time step
lookback_length = 10.0
t_step = 0.05
small_number = 1e-5
tau = np.arange(start=0, stop=lookback_length + small_number, step=t_step)
po = PlotOptions("3d_plot", [0,1,3], [19])
HJSolver(my_car, g, Initial_value_f, tau, "minVWithV0", po)
(optional)
Please specify this mode if you would like to solve a reach-avoid problem
"TargetSetMode":
{
"min" -> min with target set,
"max" -> max with taget set
}
"""
compMethods = {"PrevSetsMode": "minVWithV0"}
# HJSolver(dynamics object, grid, initial value function, time length, system objectives, plotting options)
result = HJSolver(my_car,
g,
Initial_value_f,
tau,
compMethods,
po2,
saveAllTimeSteps=True)
# Second Scenario
g = Grid(np.array([-3.0, -1.0, 0.0, -math.pi]),
np.array([3.0, 4.0, 4.0, math.pi]), 4, np.array([60, 60, 20, 36]),
[3])
# Define my object
my_car = DubinsCar4D2()
# Use the grid to initialize initial value function
Initial_value_f = CylinderShape(g, [2, 3], np.zeros(4), 1)
#print("what is target set shape:" , Initial_value_f.shape[0])
## a = Lower_Half_Space(g, 0, 0.01) # 10km/s
## b = Upper_Half_Space(g , 0, 0.08) #
## c = Lower_Half_Space(g , 1, 0.01) # 10km/s
## d = Upper_Half_Space(g , 1, 0.08) #
## part e - change control
a = Lower_Half_Space(g, 2, 0.08)
b = Upper_Half_Space(g, 2, 0.3)
# print(init_val_f)
# print(init_val_f[:, :, 0, :])
# print(init_val_f[:, :, 1, :])
init_val_f = np.subtract(init_val_f, a)
init_val_f = np.subtract(init_val_f, b)
# init_val_f = np.subtract(init_val_f, c)
# init_val_f = np.subtract(init_val_f, d)
print("shape: ", init_val_f.shape)
# Look-back length and time step
lookback_length = 6.0 # was 3.0
t_step = 0.05 # 0.075 #0.1
small_number = 1e-5
tau = np.arange(start=0, stop=lookback_length + small_number, step=t_step)
po = PlotOptions("3d_plot", [0, 1, 3], [15])
HJSolver(my_car, g, init_val_f, tau, "minVWithV0", po)
np.array([x_max, y_max + 1, v_max, yaw_max]), 4,
np.array([15, 15, 15, 18]), [3])
# The value function should be calculated in the global coordinate.
basis = np.array([[np.cos(yaw), -np.sin(yaw), start_pt[0]],
[np.sin(yaw), np.cos(yaw), start_pt[1]]])
V0, XX, YY = track.get_init_value(g, u_init=u_init, basis=basis)
return g, V0, XX, YY
# Look-back lenght and time step
lookback_length = 0.1 * 6
t_step = 0.1
small_number = 1e-5
tau = np.arange(start=0, stop=lookback_length + small_number, step=t_step)
po = PlotOptions("3d_plot", [0, 1, 3], [5])
for idx in [7440]: #np.arange(0, track.raceline_length, step = step):
g, V0, XX, YY = get_mesh_value(idx)
V1 = HJSolver(my_car, g, V0, tau, "minVWithV0", po, plot_flag=False)
#pdb.set_trace()
np.savez_compressed(
f"{save_path}/{trackName}/{idx}",
V=V1,
x=g.vs[0][:, 0, 0, 0],
y=g.vs[1][0, :, 0, 0],
v=g.vs[2][0, 0, :, 0],
yaw=g.vs[3][0, 0, 0, :],
xx=XX,
yy=YY ## (X, Y) in global coordindate
)