From a69a14d861faecb53a6f91600eaba82eb5c31992 Mon Sep 17 00:00:00 2001
From: sayanmitracode <sayan.mitra@gmail.com>
Date: Sun, 19 Jun 2022 16:42:02 -0500
Subject: [PATCH] working version of ball bounces
---
demo/ball_bounces.py | 91 +++++++++++++++++++
.../example/example_agent/ball_agent.py | 15 ++-
.../example/example_sensor/fake_sensor.py | 42 +++++++++
3 files changed, 144 insertions(+), 4 deletions(-)
create mode 100644 demo/ball_bounces.py
diff --git a/demo/ball_bounces.py b/demo/ball_bounces.py
new file mode 100644
index 00000000..718b881a
--- /dev/null
+++ b/demo/ball_bounces.py
@@ -0,0 +1,91 @@
+from enum import Enum, auto
+import copy
+from typing import List
+# from dryvr_plus_plus.scene_verifier.map.lane import Lane
+
+class BallMode(Enum):
+ # Any model should have at least one mode
+ Normal = auto()
+ # The one mode of this automation is called "Normal" and auto assigns it an integer value.
+ # Ultimately for simple models we would like to write
+ # E.g., Mode = makeMode(Normal, bounce,...)
+
+class LaneMode(Enum):
+ Lane0 = auto()
+ # For now this is a dummy notion of Lane
+
+class State:
+ '''Defines the state variables of the model
+ Both discrete and continuous variables
+ '''
+ x:float
+ y = 0.0
+ vx = 0.0
+ vy = 0.0
+ mode: BallMode
+ lane_mode: LaneMode
+ def __init__(self, x, y, vx, vy, ball_mode:BallMode, lane_mode:LaneMode):
+ pass
+
+def controller(ego:State, others:State):
+ '''Computes the possible mode transitions'''
+ output = copy.deepcopy(this)
+ '''Ego and output variable names should be flexible but
+ currently these are somehow harcoded with the sensor'''
+ # Stores the prestate first
+ if ego.x<0:
+ output.vx = -ego.vx
+ output.x=0
+ if ego.y<0:
+ output.vy = -ego.vy
+ output.y=0
+ if ego.x>20:
+ output.vx = -ego.vx
+ output.x=20
+ if ego.y>20:
+ output.vy = -ego.vy
+ output.y=20
+ return output
+
+
+
+from dryvr_plus_plus.example.example_agent.ball_agent import BallAgent
+from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
+from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap3
+from dryvr_plus_plus.example.example_sensor.fake_sensor import FakeSensor4
+from dryvr_plus_plus.plotter.plotter2D import *
+import plotly.graph_objects as go
+
+if __name__ == "__main__":
+ ball_controller = '/Users/mitras/Dpp/GraphGeneration/demo/ball_bounces.py'
+ bouncingBall = Scenario()
+ myball1 = BallAgent('red-ball', file_name=ball_controller)
+ myball2 = BallAgent('green-ball', file_name=ball_controller)
+ bouncingBall.add_agent(myball1)
+ bouncingBall.add_agent(myball2)
+ #
+ tmp_map = SimpleMap3()
+ bouncingBall.set_map(tmp_map)
+ # If there is no useful map, then we should not have to write this.
+ # Default to some empty map
+ bouncingBall.set_sensor(FakeSensor4())
+ # There should be a way to default to some well-defined sensor
+ # for any model, without having to write an explicit sensor
+ bouncingBall.set_init(
+ [
+ [[5, 10, 2, 2], [5, 10, 2, 2]],
+ [[15, 1, 1, -2], [15, 1, 1, -2]],
+ ],
+ [
+ (BallMode.Normal, LaneMode.Lane0),
+ (BallMode.Normal, LaneMode.Lane0),
+ ]
+ )
+ # WE should be able to initialize each of the balls separately
+ # Longer term: We should initialize by writing expressions like "-2 \leq myball1.x \leq 5"
+ # "-2 \leq myball1.x + myball2.x \leq 5"
+ traces = bouncingBall.simulate(10)
+ fig = go.Figure()
+ fig = plotly_simulation_anime(traces, tmp_map, fig)
+ fig.show()
+
diff --git a/dryvr_plus_plus/example/example_agent/ball_agent.py b/dryvr_plus_plus/example/example_agent/ball_agent.py
index 72157f50..7a1c4a13 100644
--- a/dryvr_plus_plus/example/example_agent/ball_agent.py
+++ b/dryvr_plus_plus/example/example_agent/ball_agent.py
@@ -13,10 +13,16 @@ class BallAgent(BaseAgent):
'''Dynamics of a frictionless billiard ball
on a 2D-plane'''
def __init__(self, id, code = None, file_name = None):
+ '''Contructor for tha agent
+ EXACTLY one of the following should be given
+ file_name: name of the controller
+ code: pyhton string ddefning the controller
+ '''
+ # Calling the constructor of tha base class
super().__init__(id, code, file_name)
@staticmethod
- def dynamic(t, state, u):
+ def dynamic(t, state):
'''Defines the RHS of the ODE used to simulate trajectories'''
x, y, vx, vy = state
x_dot = vx
@@ -26,8 +32,9 @@ class BallAgent(BaseAgent):
return [x_dot, y_dot, vx_dot, vy_dot]
def TC_simulate(self, mode: List[str], initialCondition, time_bound, lane_map:LaneMap=None)->np.ndarray:
+ # P1. Should TC_simulate really be part of the agent definition or should it be something more generic?
time_step = 0.05
- # Looks like this should be a global parameter; some config file should be setting this.
+ # P2. Looks like this should be a global parameter; some config file should be setting this.
time_bound = float(time_bound)
number_points = int(np.ceil(time_bound/time_step))
t = [round(i*time_step,10) for i in range(0,number_points)]
@@ -36,7 +43,7 @@ class BallAgent(BaseAgent):
trace = [[0]+init]
for i in range(len(t)):
r = ode(self.dynamic)
- r.set_initial_value(init).set_f_params([steering, a])
+ r.set_initial_value(init)
res:np.ndarray = r.integrate(r.t + time_step)
init = res.flatten().tolist()
if init[3] < 0:
@@ -46,4 +53,4 @@ class BallAgent(BaseAgent):
return np.array(trace)
if __name__ == '__main__':
- aball = BallAgent('red_ball',file_name=input_code_name)
\ No newline at end of file
+ aball = BallAgent('red_ball',file_name="/Users/mitras/Dpp/GraphGeneration/demo/ball_bounces.py")
\ No newline at end of file
diff --git a/dryvr_plus_plus/example/example_sensor/fake_sensor.py b/dryvr_plus_plus/example/example_sensor/fake_sensor.py
index 30d4a985..a982ef6b 100644
--- a/dryvr_plus_plus/example/example_sensor/fake_sensor.py
+++ b/dryvr_plus_plus/example/example_sensor/fake_sensor.py
@@ -106,4 +106,46 @@ class FakeSensor3:
set_states_3d(cont, disc, "ego", state_dict[agent_id])
else:
add_states_3d(cont, disc, 'others', state_dict[agent_id])
+ return cont, disc, len_dict
+
+
+def set_states_2d_ball(cnts, disc, thing, val):
+ state, mode = val
+ sets(cnts, thing, ["x", "y", "vx", "vy"], state[1:5])
+ sets(disc, thing, ["ball_mode", "lane_mode"], mode)
+def set_states_3d_ball(cnts, disc, thing, val):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ sets(cnts, thing, ["x", "y", "vx", "vy"], transp)
+ sets(disc, thing, ["ball_mode", "lane_mode"], mode)
+def add_states_2d_ball(cont, disc, thing, val):
+ state, mode = val
+ adds(cont, thing, ['x','y','vx','vy'], state[1:5])
+ adds(disc, thing, ["ball_mode", "lane_mode", "type"], mode)
+def add_states_3d_ball(cont, disc, thing, val):
+ state, mode = val
+ transp = np.transpose(np.array(state)[:, 1:5])
+ assert len(transp) == 4
+ adds(cont, thing, ['x','y','vx','vy'], transp)
+ adds(disc, thing, ["ball_mode", "lane_mode", "type"], mode)
+
+class FakeSensor4:
+ def sense(self, scenario, agent, state_dict, lane_map):
+ cont = {}
+ disc = {}
+ len_dict = {'others':len(state_dict)-1}
+ tmp = np.array(list(state_dict.values())[0])
+ if tmp.ndim < 2:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ set_states_2d_ball(cont, disc, 'ego', state_dict[agent_id])
+ else:
+ add_states_2d_ball(cont, disc, 'others', state_dict[agent_id])
+ else:
+ for agent_id in state_dict:
+ if agent_id == agent.id:
+ set_states_3d_ball(cont, disc, "ego", state_dict[agent_id])
+ else:
+ add_states_3d_ball(cont, disc, 'others', state_dict[agent_id])
return cont, disc, len_dict
\ No newline at end of file
--
GitLab