simulator修正、次pedstate

Author: Yamabooks

.vscode/launch.json

@@ -0,0 +1,23 @@
+{
+    // IntelliSense を使用して利用可能な属性を学べます。
+    // 既存の属性の説明をホバーして表示します。
+    // 詳細情報は次を確認してください: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+
+        {
+            "name": "Python デバッガー: 現在のファイル",
+            "type": "debugpy",
+            "request": "launch",
+            "program": "${file}",
+            "console": "integratedTerminal"
+        },
+        {
+            "name": "Python: main",
+            "type": "debugpy",
+            "request": "launch",
+            "program": "pysocialforce-master/mains/main.py",
+            "console": "integratedTerminal"
+        }
+    ]
+}

PySocialForce-master/mains/main.py

@@ -4,24 +4,26 @@
 
 
 if __name__ == "__main__":
-    # initial states, each entry is the position, velocity and goal of a pedestrian in the form of (px, py, vx, vy, gx, gy)
+    # 歩行者の位置、速度、目標を次の形式で表します (px, py, vx, vy, gx, gy)
     initial_state = np.array(
         [
-            [0.0, 10, -0.5, -0.5, 0.0, 0.0],
-            [0.5, 10, -0.5, -0.5, 0.5, 0.0],
-            [0.0, 0.0, 0.0, 0.5, 1.0, 10.0],
+            [0.0, 10, 0.0, -0.5, 0.0, 0.0],
+            [0.5, 10, 0.0, -0.5, 0.5, 0.0],
+            #[0.0, 0.0, 0.0, 0.5, 0.0, 10.0],
             # [1.0, 0.0, 0.0, 0.5, 2.0, 10.0],
             # [2.0, 0.0, 0.0, 0.5, 3.0, 10.0],
             # [3.0, 0.0, 0.0, 0.5, 4.0, 10.0],
         ]
     )
     # age type
-    types = [0,1,0,]    # 0: adult, 1: elderly, 2: child(未実装)
+    #types = [0,1,0,]    # 0: adult, 1: elderly, 2: child(未実装)
+    types = [0,1]
     # social groups informoation is represented as lists of indices of the state array
-    groups = [[0, 1], [2]] # 括弧内がグループ
+    #groups = [[0, 1], [2]] # 括弧内がグループ
+    groups = [[0],[1]]
     # list of linear obstacles given in the form of (x_min, x_max, y_min, y_max)
     # obs = [[-1, -1, -1, 11], [3, 3, -1, 11]]
-    obs = [[1, 2, 7, 8]]
+    obs = [[0, 1, 5, 5]]
     # obs = None
     # initiate the simulator,
     s = psf.Simulator(

PySocialForce-master/mains/main.toml

@@ -78,7 +78,7 @@ n = 2
 n_prime = 3
 
 [types.0.forces.obstacle_force]
-factor = 2.0
+factor = 10.0
 sigma = 0.2
 threshold = 3.0
 

PySocialForce-master/output/animation.gif

@@ -43,7 +43,7 @@ def _get_force(self) -> np.ndarray:
         raise NotImplementedError
 
     def get_force(self, debug=False):
-        force = self._get_force()
+        force = self._get_force()   # 現在のインスタンスの _get_force を呼び出す
         if debug:
             logger.debug(f"{camel_to_snake(type(self).__name__)}:\n {repr(force)}")
         return force
@@ -280,24 +280,30 @@ def __init__(self, config):
         self.n_prime = config("n_prime", 3)
 
     def _get_force(self):
-        pos_diff = stateutils.each_diff(self.peds.pos())
-        diff_direction, diff_length = stateutils.normalize(pos_diff)
-        vel_diff = -1.0 * stateutils.each_diff(self.peds.vel())
+        pos_diff = stateutils.each_diff(self.peds.pos())    # 歩行者間の位置差ベクトル
+        diff_direction, diff_length = stateutils.normalize(pos_diff)    # 正規化された方向ベクトル、歩行者間の距離
+        vel_diff = -1.0 * stateutils.each_diff(self.peds.vel()) # 歩行者間の速度差ベクトル
 
         # 相互作用方向
-        interaction_vec = self.lambda_importance * vel_diff + diff_direction
-        interaction_direction, interaction_length = stateutils.normalize(interaction_vec)
+        interaction_vec = self.lambda_importance * vel_diff + diff_direction    # 速度差と位置さを組み合わせた相互作用方向ベクトル
+        interaction_direction, interaction_length = stateutils.normalize(interaction_vec)   # 相互作用ベクトルの正規化方向、相互作用ベクトルの大きさ(スカラー値)
 
-        theta = stateutils.vector_angles(interaction_direction) - stateutils.vector_angles(diff_direction)
-        B = self.gamma * interaction_length
+        theta = stateutils.vector_angles(interaction_direction) - stateutils.vector_angles(diff_direction)  # 相互作用方向と位置差の方向の角度差(歩行者がどの程度別の方向を向いているか)
+        B = self.gamma * interaction_length # 相互作用の減衰係数(距離を基づいて力を調整)
 
         # 力の計算
-        force_velocity = np.exp(-diff_length / B - (self.n_prime * B * theta) ** 2).reshape(-1, 1) * interaction_direction
-        force_angle = -np.sign(theta).reshape(-1, 1) * np.exp(-diff_length / B - (self.n * B * theta) ** 2) * stateutils.left_normal(interaction_direction)
-        force = force_velocity + force_angle
-
-        return np.sum(force.reshape((self.peds.size(), -1, 2)), axis=1) * self.factor
+        force_velocity_amount = np.exp(-1.0 * diff_length / B - np.square(self.n_prime * B * theta))
+        force_angle_amount = -np.sign(theta) * np.exp(
+            -1.0 * diff_length / B - np.square(self.n * B * theta)
+        )
+        force_velocity = force_velocity_amount.reshape(-1, 1) * interaction_direction
+        force_angle = force_angle_amount.reshape(-1, 1) * stateutils.left_normal(
+            interaction_direction
+        )
 
+        force = force_velocity + force_angle  # n*(n-1) x 2
+        force = np.sum(force.reshape((self.peds.size(), -1, 2)), axis=1)
+        return force * self.factor
 
 class ObstacleForce(Force):
     """Calculates the force between this agent and the nearest obstacle in this

PySocialForce-master/pysocialforce/forces.py

@@ -19,14 +19,12 @@ def __init__(self, state, types, groups, scene_configs):
         self.types = types
         self.scene_configs = scene_configs
 
-        agent_settings = self.initialize_agent_settings()  # 各歩行者の設定を適応
-
         self.max_speeds = None
         self.initial_speeds = None
 
         self.ped_states = []
         self.group_states = []
-        self.update(state, groups, agent_settings)
+        self.update(state, groups)
 
     def initialize_agent_settings(self):
         """タイプごとにシーン設定を適用"""
@@ -46,12 +44,12 @@ def initialize_agent_settings(self):
             })  # 辞書形式でリストに保存
         return settings
 
-    def update(self, state, groups, agent_setting):
+    def update(self, state, groups):
         # タイプごとの初期化処理
+        self.agent_settings = self.initialize_agent_settings() # 各歩行者の設定を適応
         self.state = state
         self.groups = groups
-        self.agent_settings = agent_setting  # 各歩行者の設定を適応
-
+        
     @property
     def state(self):
         """歩行者の状態を取得・設定"""
@@ -115,8 +113,8 @@ def step(self, force, groups=None):
             next_groups = groups
         self.update(next_state, next_groups)
 
-    # def initial_speeds(self):
-    #     return stateutils.speeds(self.ped_states[0])
+    def initial_speeds(self):
+         return stateutils.speeds(self.ped_states[0])
 
     def desired_directions(self):
         """各歩行者が目標地点に向かう方向ベクトルを返す"""

PySocialForce-master/pysocialforce/scene.py

@@ -8,6 +8,7 @@
 from pysocialforce.scene import PedState, EnvState
 from pysocialforce import forces
 
+import json
 
 class Simulator:
     """Simulate social force model.
@@ -42,18 +43,17 @@ def __init__(self, state, types=None, groups=None, obstacles=None, config_file=N
         self.config = DefaultConfig()
         if config_file:
             self.config.load_config(config_file)
-        self.group_config = self.config.sub_config("scene")
 
+        print("Config内容:", vars(self.config))  # オブジェクトの属性を表示
+            
         # typeごとの設定を保持
         self.types = types # 0: adult, 1: elderly, 2: child
 
-        # シーン設定の読み込み
-        # setで重複を削除しユニークなタイプの集合を作成([0,1,0,1]→[0,1])
-        self.scene_configs = {t: self.config.sub_config(f"types.{t}.scene") for t in set(self.types)}
-        self.force_configs = {t: self.config.sub_config(f"types.{t}.forces") for t in set(self.types)}
+        self.scene_configs = self.get_scene_configs(types, self.config)
+        print("\nscene_configs: ", json.dumps(self.scene_configs, indent=4))
 
         # initiate obstacles
-        self.env = EnvState(obstacles, self.config("resolution", 10.0))
+        self.env = EnvState(obstacles, self.config("resolution", 50.0))
 
         # initiate agents
         self.peds = PedState(state, types, groups, self.scene_configs)
@@ -69,6 +69,15 @@ def __init__(self, state, types=None, groups=None, obstacles=None, config_file=N
         # construct forces
         self.forces = self.make_forces()
 
+    # configからtypeとsceneを抽出し、辞書を構築
+    def get_scene_configs(self, types, config):
+        scene_configs = {}
+        # setで種類の数だけ実行([0,1,0,2] → [0,1,2])
+        for i in set(types):
+            type_config = config.sub_config("types").sub_config(f"{i}").sub_config("scene").config  # typeに対応するsceneを抽出
+            scene_configs[str(i)] = {"scene": type_config}  # typeを追加し結果に追加           
+        return scene_configs
+
     def make_forces(self):
         """Construct forces for each pedestrian type and include group forces if enabled."""
         force_list = []
@@ -82,10 +91,10 @@ def make_forces(self):
                 forces.DesiredForce(type_force_config),
                 forces.SocialForce(type_force_config),
                 forces.ObstacleForce(type_force_config),
-                forces.PedRepulsiveForce(type_force_config),
-                forces.SpaceRepulsiveForce(type_force_config),
+                #forces.PedRepulsiveForce(type_force_config),
+                #forces.SpaceRepulsiveForce(type_force_config),
             ]
-        print("Force list after adding individual forces:", force_list)
+        print("Force list:", force_list)
 
         # グループ関連の力を有効化する場合
         if self.config("scene", {}).get("enable_group", False):

PySocialForce-master/pysocialforce/simulator.py

@@ -5,13 +5,15 @@
 
 # Create a custom logger
 logger = logging.getLogger("root")
-logger.setLevel(logging.DEBUG)
+#logger.setLevel(logging.DEBUG)
+logger.setLevel(logging.WARNING)
 FORMAT = "%(levelname)s:[%(filename)s:%(lineno)s %(funcName)20s() ] %(message)s"
 
 # Create handlers
 c_handler = logging.StreamHandler()
 f_handler = logging.FileHandler("file.log")
-c_handler.setLevel(logging.DEBUG)
+#c_handler.setLevel(logging.DEBUG)
+c_handler.setLevel(logging.WARNING)
 f_handler.setLevel(logging.WARNING)
 
 # Create formatters and add it to handlers