def get_mesh_value(idx, interval=interval):
start_pt = track.centerline_arr[idx]
#pdb.set_trace()
end_idx = (idx + interval) % track.raceline_length
end_pt = track.centerline_arr[end_idx]
yaw = track.race_yaw[idx]
if idx + interval > track.raceline_length:
yaw_min = min(
np.concatenate([track.race_yaw[idx:], track.race_yaw[:end_idx]
])) - math.pi / 6
yaw_max = max(
np.concatenate([track.race_yaw[idx:], track.race_yaw[:end_idx]
])) + math.pi / 6
else:
yaw_min = min(track.race_yaw[idx:end_idx]) - math.pi / 6
yaw_max = max(track.race_yaw[idx:end_idx]) + math.pi / 6
pts = []
u_init = idx / track.raceline_length
for pt in [start_pt, end_pt]:
for tck in [track.tck_in, track.tck_out]:
_, p_closest, _ = track._calc_shortest_distance(pt,
tck,
u_init=u_init)
pts.append(p_closest)
pts = np.array(pts)
pts_local = toLocal(pts, start_pt, yaw)
x_min, x_max, y_min, y_max = fitRectangle(pts_local)
g = Grid(np.array([x_min, y_min - 1, v_min, yaw_min]),
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
from solver import HJSolver
import math
import pdb
""" 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
- Initialize plotting option
- Call HJSolver function
"""
# Humanoid_6D
########################################################################################
# By defualt, 1st order upwind scheme is used
# To run 2nd order upwind (or ENO) scheme, in solver,py:
# comment out the line `solve_pde = graph_6D(dynamics_obj, grid, compMethod)`
# uncomment the line `solve_pde = graph_6D_2nd(dynamics_obj, grid, compMethod)`
########################################################################################
# Scenario 1
g = Grid(np.array([-0.5, -1.0, 0.5, -20, -math.pi / 2, -8.0]),
np.array([0.5, 1.0, 1.5, 20, math.pi / 2, 8.0]), 6,
np.array([7, 7, 7, 7, 8, 8]))
Initial_value_f = Rectangle6D(g)
# Look-back lenght and time step
lookback_length = 20.0
t_step = 0.05
small_number = 1e-5
tau = np.arange(start=0, stop=lookback_length + small_number, step=t_step)
my_car = Humanoid_6D()
po2 = PlotOptions("3d_plot", [0, 1, 2], [2, 2, 2])
"""
Assign one of the following strings to `compMethod` to specify the characteristics of computation
"none" -> compute Backward Reachable Set
# Plot options
from plot_options import *
# Solver core
from solver import HJSolver
import math
""" USER INTERFACES
- Define grid
- Generate initial values for grid using shape functions
- Time length for computations
- Initialize plotting option
- Call HJSolver function
"""
# Scenario 1
g = Grid(np.array([-4.0, -4.0, -math.pi]), np.array([4.0, 4.0, math.pi]), 3,
np.array([40, 40, 40]), [2])
Initial_value_f = CylinderShape(g, [], np.zeros(3), 1)
# Look-back lenght and time step
lookback_length = 2.0
t_step = 0.01
small_number = 1e-5
tau = np.arange(start=0, stop=lookback_length + small_number, step=t_step)
my_car = DubinsCapture()
po2 = PlotOptions("3d_plot", [0, 1, 2], [])
"""
Assign one of the following strings to `compMethod` to specify the characteristics of computation
"none" -> compute Backward Reachable Set
import math
""" 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([-5.0, -5.0, -5.0, -math.pi]),
np.array([5.0, 5.0, 5.0, math.pi]), 4, np.array([50, 50, 30, 35]),
[3])
# Define my object
dMin = [0, 0]
dMax = [0, 0]
uMin = [-0.01, -0.05]
uMax = [0.01, 0.05]
## [ignore]
#uMin = [0.08, 0.08]
#uMax = [0.3, 0.3]
## Part f - set disturbance
# Solver core
from solver import HJSolver
import math
""" USER INTERFACES
- Define grid
- Generate initial values for grid using shape functions
- Time length for computations
- Initialize plotting option
- Call HJSolver function
"""
# Scenario 1
g = Grid(np.array([-4.0, -4.0, -math.pi]), np.array([4.0, 4.0, math.pi]), 3, np.array([40, 40, 40]), [2])
Initial_value_f = CylinderShape(g, [], np.zeros(3), 1)
# Look-back lenght and time step
lookback_length = 2.0
t_step = 0.05
small_number = 1e-5
tau = np.arange(start=0, stop=lookback_length + small_number, step=t_step)
my_car = DubinsCapture()
po2 = PlotOptions("3d_plot", [0,1,2], [])
"""
Assign one of the following strings to `compMethod` to specify the characteristics of computation
# Use the grid to initialize initial value function
Initial_value_f = Rectangle6D(g)
# Look-back length and time step
lookback_length = 2.0
t_step = 0.05
tau = np.arange(start = 0, stop = lookback_length + t_step, step = t_step)
print("Welcome to optimized_dp \n")
# Use the following variable to specify the characteristics of computation
compMethod = "minVWithVInit"
my_object = my_car
my_shape = Initial_value_f """
g = Grid(np.array([-5.0, -5.0, -1.0, -math.pi]), np.array([5.0, 5.0, 1.0, math.pi]), 4, np.array([40, 40, 50, 50]), [3])
# Define my object
my_car = DubinsCar4D()
# Use the grid to initialize initial value function
Initial_value_f = CylinderShape(g, [3,4], np.zeros(4), 1)
# Look-back lenght and time step
lookback_length = 2.0
t_step = 0.05
small_number = 1e-5
tau = np.arange(start = 0, stop = lookback_length + small_number, step = t_step)
print("Welcome to optimized_dp \n")
import numpy as np
# Utility functions to initialize the problem
from Grid.GridProcessing import Grid
from Shapes.ShapesFunctions import *
# Specify the file that includes dynamic systems
from dynamics.DubinsCar import *
# Plot options
from plot_options import *
# Solver core
from solver import *
import math
# Compute BRS only
g = Grid(minBounds=np.array([-3.0, -1.0, -math.pi]),
maxBounds=np.array([3.0, 4.0, math.pi]),
dims=3,
pts_each_dim=np.array([80, 80, 80]),
periodicDims=[2])
# Car is trying to reach the target
my_car = DubinsCar(uMode="min")
# Initialize target set as a cylinder
targeSet = CylinderShape(g, [2], np.array([0.0, 1.0, 0.0]), 0.70)
po = PlotOptions("3d_plot",
plotDims=[0, 1, 2],
slicesCut=[],
min_isosurface=0,
max_isosurface=0)
lookback_length = 1.5
t_step = 0.05
import numpy as np
# Utility functions to initialize the problem
from Grid.GridProcessing import Grid
from Shapes.ShapesFunctions import *
# Specify the file that includes dynamic systems
from dynamics.DubinsCar import *
# Plot options
from plot_options import *
# Solver core
from solver import *
import math
g = Grid(np.array([-3.0, -1.0, -math.pi]), np.array([3.0, 4.0, math.pi]), 3, np.array([160, 160, 160]), [2])
my_car = DubinsCar()
Initial_value_f = CylinderShape(g, [2], np.array([0.0, 1.0, 0.0, 0.0]), 0.70)
po = PlotOptions("3d_plot", [0,1,2], [])
eps = 0.00001
V_0 = TTRSolver(my_car, g, Initial_value_f, eps, po)
np.save("tt2_array.npy", V_0)
- Generate initial values for grid using shape functions
- Time length for computations
- Initialize plotting option
- Call HJSolver function
"""
question = 'd' #'e', 'f'
# Second Scenario
v_min = 50.
v_max = 350.
grid = [50, 50, 30, 35]
g = Grid(np.array([-3000.0, -3000.0, v_min, -math.pi]), np.array([3000.0, 3000.0, v_max, math.pi]),
4, np.array(grid), [3])
# Define my object
initial = [2000, 2000.0, 0, 0]
umin = [-10, -0.5]
umax = [10, 0.5]
dmin = [0, 0]
dmax = [0, 0]
# Use the grid to initialize initial value function
if question == 'd':
my_car = DubinsCar4D(initial, uMin=umin, uMax=umax, dMin=dmin, dMax=dmax, uMode='max', dMode='min')
Initial_value_f = CylinderShape(g, [2,3], np.zeros(4), 1000)
elif question == 'e':