From f4d3e2aeafed339c47520b789b78a16a2fefc0d4 Mon Sep 17 00:00:00 2001 From: Nitish Bhupathi Raju Date: Fri, 23 Feb 2024 15:38:49 -0600 Subject: [PATCH] phi angle bug --- sense.py | 137 ++++++++++++++++++++++++++++---------- sensedl.py | 15 ++--- verse/map/lane.py | 5 ++ verse/map/lane_segment.py | 6 ++ 4 files changed, 116 insertions(+), 47 deletions(-) diff --git a/sense.py b/sense.py index 5ece4fe0..a45882ba 100644 --- a/sense.py +++ b/sense.py @@ -9,21 +9,39 @@ import plotly.graph_objects as go from verse.plotter.plotter2D import * from verse.map.lane_map import LaneMap -from verse.map.lane_segment import StraightLane +from verse.map.lane_segment import StraightLane, CircularLane from verse.map.lane import Lane from math import pi import random +from verse.analysis.utils import wrap_to_pi -def left(): - num = random.randint(1, 10) - if num >=1 and num <= 9: + + +def very_left(): + num = random.randint(1,100) + if num >=1 and num <= 98: return 0 else: return 1 -def right(): - num = random.randint(1, 10) - if num >=1 and num <= 8: +def slightly_left(): + num = random.randint(1, 100) + if num >=1 and num <= 90: + return 0 + else: + return 1 + + +def slightly_right(): + num = random.randint(1, 100) + if num >=1 and num <= 80: + return 1 + else: + return 0 + +def very_right(): + num = random.randint(1, 100) + if num >=1 and num <= 95: return 1 else: return 0 @@ -48,11 +66,16 @@ def sense(self, agent: BaseAgent, state_dict, lane_map): cont['ego.t'] = state_dict['car'][0][5] disc['ego.agent_mode'] = state_dict['car'][1][0] x = cont['ego.x'] - - if(cont['ego.x'] < 7.5): - cont['ego.s'] = 0 #left() #0 + y = cont['ego.y'] + + if((x-14)**2 + (y-14)**2 > 13**2): + cont['ego.s'] = 0#very_left() + elif((x-14)**2 + (y-14)**2 > 12**2): + cont['ego.s'] = 0#slightly_left() + elif((x-14)**2 + (y-14)**2 > 11**2): + cont['ego.s'] = 1#slightly_right() else: - cont['ego.s'] = 1 #right() #1 + cont['ego.s'] = 1#very_right() else: if agent.id == 'car': @@ -66,30 +89,49 @@ def sense(self, agent: BaseAgent, state_dict, lane_map): state1 = -1 state2 = -1 - if(cont['ego.x'][0] < 7.5): - state1 = 0#left() #0 + x = cont['ego.x'][0] + y = cont['ego.y'][0] + + if((x-14)**2 + (y-14)**2 > 13**2): + state1 = very_left() + elif((x-14)**2 + (y-14)**2 > 12**2): + state1 = slightly_left() + elif((x-14)**2 + (y-14)**2 > 11**2): + state1 = slightly_right() else: - state1 = 1#right() #1 - if(cont['ego.x'][1] < 7.5): - state2 = 0#left() #0 + state1 = very_right() + + x = cont['ego.x'][1] + y = cont['ego.y'][1] + + + if((x-14)**2 + (y-14)**2 > 13**2): + state2 = very_left() + elif((x-14)**2 + (y-14)**2 > 12**2): + state2 = slightly_left() + elif((x-14)**2 + (y-14)**2 > 11**2): + state2 = slightly_right() else: - state2 = 1 # right() #1 + state2 = very_right() + if(state1 > state2): state1 = state2 cont['ego.s'] = [state1, state2] - + ''' print(cont['ego.s']) if(cont['ego.s'] == [0,1]): print("hello") - + ''' return cont, disc, len_dict class M1(LaneMap): def __init__(self): super().__init__() - segment0 = StraightLane("Seg0", [0, 0], [0, 100], 3) + #segment0 = StraightLane("Seg0", [0, 0], [0, 100], 3) + segment0 = CircularLane("Seg0", [14, 14], 12, np.pi * 3 / 2, np.pi * (-7/2), True, 4) + #lane0 = Lane("T0", [segment0]) lane0 = Lane("T0", [segment0]) self.add_lanes([lane0]) self.h_dict = {("T0", "Left", "Right"): "T0", ("T0", "Right", "Left"): "T0", @@ -97,11 +139,11 @@ def __init__(self): #modified T def car_dynamics(t, state, u): - x, y, theta, v, t1 = state - delta, a, time_step = u - x_dot = v * np.cos((theta*pi/180) + (delta*pi/180)) - y_dot = v * np.sin((theta*pi/180) + (delta*pi/180)) - theta_dot = delta#/time_step + x, y, theta, v, t = state + delta, a = u + x_dot = v * np.cos(theta + delta) + y_dot = v * np.sin(theta + delta) + theta_dot = v / 1.75 * np.sin(delta) v_dot = a t_dot = 1 return [x_dot, y_dot, theta_dot, v_dot, t_dot] @@ -110,17 +152,38 @@ def car_action_handler(mode: List[str], state, lane_map) -> Tuple[float, float]: x, y, theta, v, t = state vehicle_mode = mode[0] vehicle_lane = mode[1] + heading = 0 + d = 0 + pos = np.array([x,y]) + + ''' + center = np.array([14,14]) + v = pos - center + if(np.sum(v) == 0): + track_pos = pos + else: + track_pos = center + (v/np.linalg.norm(v))*12 + ''' - new_theta = theta + new_theta = 0 if vehicle_mode == "Left": - new_theta = 135 - elif vehicle_mode == "Right": new_theta = 45 + elif vehicle_mode == "Right": + new_theta = -45 else: raise ValueError(f"Invalid mode: {vehicle_mode}") + + #print(pos) + + new_theta = np.radians(new_theta) + #heading = np.radians(heading) + heading = lane_map.get_lane_heading(vehicle_lane, pos) + np.arctan2(0.45 * new_theta, v) + psi = wrap_to_pi(heading - theta) + steering = psi + steering = np.clip(steering, -0.40, 0.40) + - delta = new_theta - theta - return delta, 0 + return steering, 0 def TC_simulate( mode: List[str], init, time_bound, time_step, lane_map: LaneMap = None @@ -131,9 +194,11 @@ def TC_simulate( trace[1:, 0] = [round(i * time_step, 10) for i in range(num_points)] trace[0, 1:] = init for i in range(num_points): + if i==300: + print("stop") steering, a = car_action_handler(mode, init, lane_map) r = ode(car_dynamics) - r.set_initial_value(init).set_f_params([steering, a, time_step]) + r.set_initial_value(init).set_f_params([steering, a]) res: np.ndarray = r.integrate(r.t + time_step) init = res.flatten() if init[3] < 0: @@ -156,22 +221,22 @@ def __init__(self, id, code=None, file_name=None): car1 = CarAgent("car", file_name="sensedl.py") -car1.set_initial([[13, 0, 135, 3, 0], [14, 0, 135, 3, 0]], (AgentMode.Left, TrackMode.T0)) +car1.set_initial([[14, 1, np.radians(180), 0.75, 0], [14, 1, np.radians(180), 0.75, 0]], (AgentMode.Right, TrackMode.T0)) scenario.add_agent(car1) -traces_simu = scenario.simulate(30,0.01) -traces_veri = scenario.verify(30, 0.01) +traces_simu = scenario.simulate(100,0.01) +#traces_veri = scenario.verify(15, 0.01) fig = go.Figure() fig = simulation_tree(traces_simu, None, fig, 1, 2, [0, 1], "lines", "trace") fig.show() - +''' fig = go.Figure() fig = reachtube_tree(traces_veri, None, fig, 1, 2, [0, 1], "lines", "trace") fig.show() - +''' diff --git a/sensedl.py b/sensedl.py index 1ba2d60f..3300ed93 100644 --- a/sensedl.py +++ b/sensedl.py @@ -30,21 +30,14 @@ def __init__(self, x, y, theta, v,t, agent_mode: AgentMode, track_mode: TrackMod def decisionLogic(ego: State, track_map): output = copy.deepcopy(ego) - if ego.s == 0 and ego.t >= 2: #we're on the left side + if ego.s == 0 and ego.t >= 1: #we're on the left side output.agent_mode = AgentMode.Right #go right output.t = 0 - elif ego.s == 1 and ego.t >= 2: #we're on the right side + elif ego.s == 1 and ego.t >= 1: #we're on the right side output.agent_mode = AgentMode.Left #go left output.t = 0 - ''' - if ego.s == -1 and ego.agent_mode == AgentMode.Left and ego.t >= 5: - output.t = 0 - output.agent_mode = AgentMode.Right - elif ego.s == 1 and ego.agent_mode == AgentMode.Right and ego.t >= 5: - output.agent_mode = AgentMode.Left - output.t = 0 - ''' - assert ego.x >= -2 and ego.x <= 17 + + assert (ego.x-14)**2 + (ego.y-14)**2 >= 10**2 and (ego.x-14)**2 + (ego.y-14)**2 <= 14**2 return output diff --git a/verse/map/lane.py b/verse/map/lane.py index 226b2531..b1967597 100644 --- a/verse/map/lane.py +++ b/verse/map/lane.py @@ -27,12 +27,17 @@ def get_lane_segment(self, position: np.ndarray) -> AbstractLane: seg = None for seg_idx, segment in enumerate(self.segment_list): logitudinal, lateral = segment.local_coordinates(position) + #if(logitudinal > 37.5): + #print(logitudinal) is_on = 0 - Lane.COMPENSATE <= logitudinal < segment.length if is_on: if lateral < min_lateral: idx = seg_idx seg = segment min_lateral = lateral + if seg is None: + print("here") + segment.local_coordinates(position) return idx, seg def get_heading(self, position: np.ndarray) -> float: diff --git a/verse/map/lane_segment.py b/verse/map/lane_segment.py index 346f0e4a..9640a475 100644 --- a/verse/map/lane_segment.py +++ b/verse/map/lane_segment.py @@ -280,8 +280,14 @@ def width_at(self, longitudinal: float) -> float: def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]: delta = position - self.center phi = np.arctan2(delta[1], delta[0]) + wrap = (wrap_to_pi(phi - self.start_phase)) + #if(wrap <= 0): phi = self.start_phase + wrap_to_pi(phi - self.start_phase) + if(wrap > 0): + #print("hi") + print(phi) r = np.linalg.norm(delta) + # longitudinal = (phi - self.start_phase) * self.radius longitudinal = self.direction * (phi - self.start_phase) * self.radius lateral = self.direction * (self.radius - r) return longitudinal, lateral