working version of ball bounces

demo/ball_bounces.py

+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()
+

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

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