添加��《移动机器人安全控制与安全强化学习》课程资料

README.md

@@ -53,8 +53,9 @@
 19. [《基于MATLAB的十字路口交通流仿真》](https://sdv.h5.xeknow.com/s/Q9GI6)：matlab_traffic_simulation
 20. [《四足机器人VMC算法仿真实现》](https://sdv.xet.tech/s/25iWOI)：quadruped_robot_vmc
 21. [《多刚体系统动力学建模及MATLAB仿真》](https://sdv.xet.tech/s/2xtPGx)：multiple_body_dynamic_modeling
-22. [《自动驾驶入门：从建模到跟随》](https://class.guyuehome.com/detail/p_5f72a976e4b0e95a89c1ab42/6) 
-23. [《无人机仿真开发》](https://class.guyuehome.com/detail/p_5f041b74e4b036f1c0cf25a2/6) 
+22. [《移动机器人安全控制与安全强化学习》](https://zyesr.xet.tech/s/1mMXR1)：robot_safe_navigation
+23. [《自动驾驶入门：从建模到跟随》](https://class.guyuehome.com/detail/p_5f72a976e4b0e95a89c1ab42/6) 
+24. [《无人机仿真开发》](https://class.guyuehome.com/detail/p_5f041b74e4b036f1c0cf25a2/6) 
 
 ------
 

simulation&control/README.md

@@ -21,6 +21,7 @@
 19. matlab_traffic_simulation：[《基于MATLAB的十字路口交通流仿真》](https://sdv.h5.xeknow.com/s/Q9GI6)
 20. quadruped_robot_vmc：[《四足机器人VMC算法仿真实现》](https://sdv.xet.tech/s/25iWOI)
 21. multiple_body_dynamic_modeling：[《多刚体系统动力学建模及MATLAB仿真》](https://sdv.xet.tech/s/2xtPGx)
+22. robot_safe_navigation：[《移动机器人安全控制与安全强化学习》](https://zyesr.xet.tech/s/1mMXR1)
 
 ------
 

simulation&control/robot_safe_navigation/README.md

@@ -0,0 +1,31 @@
+# Robot Safe Navigation in Clustered Dynamic Environments
+
+## Table of Contents
+- [Introduction](#Introduction)
+- [Install](#install)
+- [Usage](#usage)
+- [Acknowledgments](#Acknowledgments)
+
+## Introduction
+We provide code of safe control method - Safe Set Algorithm (SSA) and test it in a crowded dynamic environment.
+
+<img src="SSA_RL.gif" width="400" height="460">
+
+## Install
+
+```
+pip install cvxopt
+```
+
+## Usage
+
+```
+python main.py --no-ssa
+python main.py --ssa
+```
+- `--no-ssa` means that ssa is not used.
+- `--ssa` means that ssa is used for monitoring and modifying the unsafe control.
+
+
+## Acknowledgments
+Part of the simulation environment code is coming from the course CS 7638: Artificial Intelligence for Robotics in GaTech. We get the permission from the lecturor Jay Summet to use this code.

simulation&control/robot_safe_navigation/course.txt

@@ -0,0 +1,2 @@
+code link: https://github.com/hychen-naza/Robot_Safe_Navigation
+paper link: https://arxiv.org/abs/2303.14265

simulation&control/robot_safe_navigation/src/__init__.py

@@ -0,0 +1 @@
+pass

simulation&control/robot_safe_navigation/src/bounds.py

@@ -0,0 +1,13 @@
+from builtins import object
+class BoundsRectangle(object):
+
+    def __init__(self, x_bounds, y_bounds):
+        self.x_bounds = x_bounds
+        self.y_bounds = y_bounds
+
+    def contains(self, xy):
+        return ((self.x_bounds[0] <= xy[0] <= self.x_bounds[1])
+                and (self.y_bounds[0] <= xy[1] <= self.y_bounds[1]))
+
+    def __repr__(self):
+        return f'(({self.x_bounds[0]}, {self.y_bounds[0]}), ({self.x_bounds[1]}, {self.y_bounds[1]}))'

simulation&control/robot_safe_navigation/src/dynamic_obstacle.py

@@ -0,0 +1,129 @@
+import random
+import numpy as np
+import math
+
+def l2( xy0, xy1 ):
+    ox = xy1[0]
+    oy = xy1[1]
+    dx = xy0[0] - xy1[0]
+    dy = xy0[1] - xy1[1]
+    dist = math.sqrt( (dx * dx) + (dy * dy) )
+    if (xy1[0] < -0.9):
+        warp_dx = xy0[0] - (1 + (xy1[0] + 1))
+        dist1 = math.sqrt( (warp_dx * warp_dx) + (dy * dy) )
+        if (dist1 < dist):
+            ox = (1 + (xy1[0] + 1))
+            dist = dist1
+    elif (xy1[0] > 0.9):
+        warp_dx = xy0[0] - (-1 + (xy1[0] - 1))
+        dist1 = math.sqrt( (warp_dx * warp_dx) + (dy * dy) )
+        if (dist1 < dist):
+            ox = (-1 + (xy1[0] - 1))
+            dist = dist1
+    return dist, ox, oy
+
+
+class Obstacle():
+
+    def __init__(self,
+                 a_x, b_x, c_x,
+                 a_y, b_y, c_y, t_start):
+        self.a_x = a_x / 5
+        self.b_x = b_x
+        self.c_x = c_x
+        self.a_y = a_y / 5
+        self.b_y = b_y
+        self.c_y = c_y
+        self.t_start = t_start
+
+    @property
+    def params(self):
+        return { 'a_x':     self.a_x,
+                 'b_x':     self.b_x,
+                 'c_x':     self.c_x,
+                 'a_y':     self.a_y,
+                 'b_y':     self.b_y,
+                 'c_y':     self.c_y,
+                 't_start': self.t_start }
+
+    def x(self, t):
+        t_shifted = t - self.t_start
+        x = ((self.a_x * t_shifted * t_shifted)
+             + (self.b_x * t_shifted)
+             + self.c_x)
+        return x
+
+    def y(self, t):
+        t_shifted = t - self.t_start
+        y = ((self.a_y * t_shifted * t_shifted)
+             + (self.b_y * t_shifted)
+             + self.c_y)
+        return y
+
+    def v_x(self, t):
+        t_shifted = t - self.t_start
+        v_x = ((2 * self.a_x * t_shifted) + self.b_x)
+        return v_x
+
+    def v_y(self, t):
+        t_shifted = t - self.t_start
+        v_y = ((2 * self.a_y * t_shifted) + self.b_y)
+        return v_y
+
+FIELD_X_BOUNDS = (-0.95, 0.95)
+FIELD_Y_BOUNDS = (-0.90, 1.0)
+
+class ObstacleField(object):
+
+    def __init__(self,
+                 x_bounds = FIELD_X_BOUNDS,
+                 y_bounds = FIELD_Y_BOUNDS):
+
+        self.random_init()
+        self.x_bounds = x_bounds
+        self.y_bounds = y_bounds
+
+    def random_init(self):
+        obstacles = []
+        for i in range(50):
+            obstacles.append(self.random_init_obstacle(t = -100))
+        self.obstacles = obstacles
+        return 
+
+    def random_init_obstacle(self, t, vehicle_x = 0, vehicle_y = -1, min_dist = 0.1):
+        dist = -1
+        x = y = -1
+        while (dist < min_dist):
+            x = random.uniform(FIELD_X_BOUNDS[0],FIELD_X_BOUNDS[1])
+            y = random.uniform(FIELD_Y_BOUNDS[0],FIELD_Y_BOUNDS[1])
+            dist = math.sqrt((vehicle_x-x)**2 + (vehicle_y-y)**2)
+        b_x = random.uniform(1e-3, 1e-2) * random.choice([1,-1])
+        b_y = random.uniform(1e-3, 1e-2) * random.choice([1,-1])
+        a_x = random.uniform(5e-6, 1e-4) * random.choice([1,-1])
+        a_y = random.uniform(5e-6, 1e-4) * random.choice([1,-1])
+        return Obstacle(a_x, b_x, x, a_y, b_y, y, t)
+
+    def unsafe_obstacle_locations(self, t, cx, cy, sensing_dist):    
+        locs =  [ (i, a.x(t), a.y(t), a.v_x(t), a.v_y(t), a.a_x, a.a_y)
+                  for i,a in enumerate(self.obstacles)]
+        unsafe_obstacles = []
+        for i,x,y,x_v,y_v,x_a,y_a  in locs:
+            if self.x_bounds[0] <= x <= self.x_bounds[1] and self.y_bounds[0] <= y <= self.y_bounds[1]:
+                dist, ox, oy = l2([cx,cy], [x,y])
+                if dist < sensing_dist:
+                    unsafe_obstacles.append([i,(ox,oy,x_v,y_v,x_a,y_a)])
+        return  unsafe_obstacles
+
+
+    def obstacle_locations(self, t, vehicle_x, vehicle_y, min_dist):
+        """
+        Returns (i, x, y) tuples indicating that the i-th obstacle is at location (x,y).
+        """
+        locs = []
+        for i, a in enumerate(self.obstacles):
+            if self.x_bounds[0] <= a.x(t) <= self.x_bounds[1] and self.y_bounds[0] <= a.y(t) <= self.y_bounds[1]:
+                locs.append((i, a.x(t), a.y(t)))
+            else:
+                self.obstacles[i] = self.random_init_obstacle(t, vehicle_x, vehicle_y, min_dist)
+                locs.append((i, a.x(t), a.y(t)))
+        return locs

simulation&control/robot_safe_navigation/src/main.py

@@ -0,0 +1,99 @@
+from __future__ import print_function
+from __future__ import absolute_import
+
+# python modules
+import argparse
+import numpy as np
+import time
+# project files
+import dynamic_obstacle
+import bounds
+import robot 
+import simu_env
+import runner
+import param
+from turtle_display import TurtleRunnerDisplay
+from ssa import SafeSetAlgorithm
+
+
+def display_for_name( dname ):
+    # choose none display or visual display
+    if dname == 'turtle':
+        return TurtleRunnerDisplay(800,800)
+    else:
+        return runner.BaseRunnerDisplay()
+
+def parser():
+    prsr = argparse.ArgumentParser()
+    prsr.add_argument( '--display',
+                       choices=('turtle','text','none'),
+                       default='turtle' )
+    prsr.add_argument( '--ssa',dest='enable_ssa', action='store_true')
+    prsr.add_argument( '--no-ssa',dest='enable_ssa', action='store_false')
+    return prsr
+
+def run_kwargs( params ):
+    in_bounds = bounds.BoundsRectangle( **params['in_bounds'] )
+    goal_bounds = bounds.BoundsRectangle( **params['goal_bounds'] )
+    min_dist = params['min_dist']
+    ret = { 'field': dynamic_obstacle.ObstacleField(),
+            'robot_state': robot.DoubleIntegratorRobot( **( params['initial_robot_state'] ) ),
+            'in_bounds': in_bounds,
+            'goal_bounds': goal_bounds,
+            'noise_sigma': params['noise_sigma'],
+            'min_dist': min_dist,
+            'nsteps': 1000 }
+    return ret
+
+def navigate(display_name, enable_ssa):
+    try:
+        params = param.params
+    except Exception as e:
+        print(e)
+        return
+    display = display_for_name(display_name)
+    env_params = run_kwargs(params)
+    env = simu_env.Env(display, **(env_params))
+
+    # ssa
+    safe_controller = SafeSetAlgorithm(max_speed = env.robot_state.max_speed, dmin = env.min_dist * 2)
+
+    # dynamic model parameters
+    episode_num = 0
+    collision_num = 0
+    failure_num = 0
+    success_num = 0
+    sensing_range = env.min_dist * 6
+    fx = np.array([[0,0,1,0],[0,0,0,1],[0,0,0,0],[0,0,0,0]])
+    gx = np.array([[1,0],[0,1],[1,0],[0,1]])
+    state, done = env.reset(), False
+
+    while(1):
+      env.display_start()
+      obstacle_ids, obstacles = env.detect_obstacles(sensing_range)
+      action = env.random_action()
+      # compute safe control
+      if (enable_ssa):
+        action, is_safe = safe_controller.get_safe_control(state, obstacles, fx, gx, action)
+      else:
+         is_safe = False
+      state, reward, done = env.step(action, is_safe, obstacle_ids) 
+      env.display_end()
+
+      if (done and reward == -500):          
+        collision_num += 1      
+      elif (done and reward == 2000):
+        success_num += 1
+      elif (done):
+        failure_num += 1
+      time.sleep(0.05)
+      if (done):
+        episode_num += 1
+        print(f"Train: episode_num {episode_num}, success_num {success_num}, collision_num {collision_num}")
+        state, done = env.reset(), False
+
+if __name__ == '__main__':
+    args = parser().parse_args()
+    navigate(display_name = args.display, enable_ssa = args.enable_ssa)
+
+

simulation&control/robot_safe_navigation/src/param.py

@@ -0,0 +1,41 @@
+params = {
+  "in_bounds": {
+    "x_bounds": [
+      -1.0, 
+      1.0
+    ], 
+    "y_bounds": [
+      -1.2, 
+      1.2
+    ]
+  }, 
+  "noise_sigma": 0.03, 
+  "min_dist": 0.03, 
+  "_args": {
+    "noise_sigma": 0.03, 
+    "min_dist": 0.03, 
+    "asteroid_b_max": 0.01, 
+    "robot_max_speed": 0.03, 
+    "t_step": 2, 
+    "asteroid_a_max": 0.0001, 
+    "t_future": 1000, 
+    "t_past": -100
+  }, 
+  "initial_robot_state": {
+    "x": 0.0, 
+    "y": -1.0, 
+    "vx": 0, 
+    "vy": 0, 
+    "max_speed": 0.02
+  }, 
+  "goal_bounds": {
+    "x_bounds": [
+      -1.0, 
+      1.0
+    ], 
+    "y_bounds": [
+      1.0, 
+      1.2
+    ]
+  }
+}

simulation&control/robot_safe_navigation/src/robot.py

@@ -0,0 +1,37 @@
+import math
+import numpy as np
+
+class DoubleIntegratorRobot():
+
+    def __init__(self, x, y, vx, vy, max_speed):
+
+        self.x = x
+        self.y = y
+        self.v_x = vx
+        self.v_y = vy
+        self.max_speed = max_speed
+
+    @property
+    def position(self):
+        heading_angle = math.atan2(self.v_y, self.v_x)
+        return (self.x, self.y, heading_angle)
+
+    def steer(self, vx_speed_change, vy_speed_change):
+        """
+        Returns a new Robot object.
+        """
+        self.v_x += vx_speed_change
+        self.v_y += vy_speed_change
+        self.v_x = max(min(self.v_x, self.max_speed), -self.max_speed)
+        self.v_y = max(min(self.v_y, self.max_speed), -self.max_speed)
+
+        new_x = self.x + self.v_x
+        new_y = self.y + self.v_y
+        # warp around
+        if (new_x > 1):
+            new_x = -1 + (new_x - 1) 
+        if (new_x < -1):
+            new_x = 1 + (new_x + 1) 
+        if (new_y < -1):
+            new_y = -1
+        return DoubleIntegratorRobot(x = new_x, y = new_y, vx = self.v_x, vy = self.v_y, max_speed = self.max_speed)