Skip to content

Commit

Permalink
phi angle bug
Browse files Browse the repository at this point in the history
  • Loading branch information
Nitish Bhupathi Raju authored and Nitish Bhupathi Raju committed Feb 23, 2024
1 parent 3f42f53 commit f4d3e2a
Show file tree
Hide file tree
Showing 4 changed files with 116 additions and 47 deletions.
137 changes: 101 additions & 36 deletions sense.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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':
Expand All @@ -66,42 +89,61 @@ 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",
("T0", "Left", "Left"): "T0",("T0", "Right", "Right"): "T0"}

#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]
Expand All @@ -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
Expand All @@ -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:
Expand All @@ -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()

'''



Expand Down
15 changes: 4 additions & 11 deletions sensedl.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
5 changes: 5 additions & 0 deletions verse/map/lane.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
6 changes: 6 additions & 0 deletions verse/map/lane_segment.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit f4d3e2a

Please sign in to comment.