From 06bdc20b3cb1553eca4595665ea6693567de435a Mon Sep 17 00:00:00 2001 From: m-guberina <gubi.guberina@gmail.com> Date: Thu, 5 Dec 2024 10:50:58 +0100 Subject: [PATCH] fixed submodules to not be submodules --- .../path_generation/star_navigation | 1 - .../path_generation/star_navigation/README.md | 18 + .../star_navigation/config/__init__.py | 0 .../star_navigation/config/load_config.py | 83 ++ .../config/params/dmp_ctrl_params.yaml | 16 + .../pfmpc_artificial_reference_params.yaml | 40 + .../config/params/pfmpc_ds_params.yaml | 42 + .../pfmpc_obstacle_constraints_params.yaml | 35 + .../config/params/robot_params.yaml | 34 + .../config/params/soads_ctrl_params.yaml | 14 + .../star_navigation/config/scene.py | 769 ++++++++++++++++++ .../motion_control/__init__.py | 0 .../motion_control/dmp/__init__.py | 1 + .../star_navigation/motion_control/dmp/dmp.py | 175 ++++ .../pfmpc_artificial_reference/__init__.py | 2 + .../motion_controller.py | 142 ++++ .../pfmpc_artificial_reference/mpc.py | 489 +++++++++++ .../mpc_build/.gitignore | 4 + .../motion_control/pfmpc_ds/__init__.py | 5 + .../pfmpc_ds/motion_controller.py | 447 ++++++++++ .../motion_control/pfmpc_ds/mpc.py | 364 +++++++++ .../pfmpc_ds/mpc_build/.gitignore | 4 + .../motion_control/pfmpc_ds/path_generator.py | 110 +++ .../pfmpc_ds/workspace_modification.py | 139 ++++ .../pfmpc_obstacle_constraints/__init__.py | 2 + .../motion_controller.py | 138 ++++ .../pfmpc_obstacle_constraints/mpc.py | 398 +++++++++ .../mpc_build/.gitignore | 4 + .../motion_control/soads/__init__.py | 1 + .../motion_control/soads/soads.py | 386 +++++++++ .../star_navigation/robot/__init__.py | 7 + .../star_navigation/robot/bicycle.py | 55 ++ .../star_navigation/robot/mobile_robot.py | 74 ++ .../star_navigation/robot/omnidirectional.py | 23 + .../star_navigation/robot/unicycle.py | 68 ++ .../star_navigation/scripts/compute_time.py | 135 +++ .../star_navigation/scripts/run_simulation.py | 304 +++++++ .../scripts/simulate_several_controllers.py | 158 ++++ .../scripts/simulate_several_initpos.py | 190 +++++ .../star_navigation/scripts/test_soads.py | 161 ++++ .../path_generation/star_navigation/setup.py | 19 + .../star_navigation.egg-info/PKG-INFO | 3 + .../star_navigation.egg-info/SOURCES.txt | 51 ++ .../dependency_links.txt | 1 + .../star_navigation.egg-info/top_level.txt | 4 + .../star_navigation/visualization/__init__.py | 2 + .../visualization/figures/.gitignore | 4 + .../visualization/scene_gui.py | 383 +++++++++ .../visualization/video_writer.py | 45 + .../visualization/videos/.gitignore | 4 + .../path_generation/starworld_tunnel_mpc | 1 - .../starworld_tunnel_mpc/README.md | 22 + .../starworld_tunnel_mpc/config/__init__.py | 0 .../config/load_config.py | 79 ++ .../config/params/robot_params.yaml | 27 + .../config/params/soads_ctrl_params.yaml | 14 + .../params/tunnel_mpc_convergence_params.yaml | 39 + .../config/params/tunnel_mpc_params.yaml | 38 + .../starworld_tunnel_mpc/config/scene.py | 555 +++++++++++++ .../motion_control/__init__.py | 0 .../motion_control/soads/__init__.py | 1 + .../motion_control/soads/soads.py | 260 ++++++ .../motion_control/tunnel_mpc/__init__.py | 4 + .../tunnel_mpc/mpc_build/.gitignore | 4 + .../tunnel_mpc/path_generator.py | 108 +++ .../motion_control/tunnel_mpc/tunnel_mpc.py | 377 +++++++++ .../tunnel_mpc/tunnel_mpc_controller.py | 101 +++ .../tunnel_mpc/workspace_modification.py | 66 ++ .../tunnel_mpc_convergence/__init__.py | 4 + .../mpc_build/.gitignore | 4 + .../tunnel_mpc_convergence/path_generator.py | 108 +++ .../tunnel_mpc_convergence/tunnel_mpc.py | 386 +++++++++ .../tunnel_mpc_controller.py | 107 +++ .../workspace_modification.py | 66 ++ .../starworld_tunnel_mpc/robot/__init__.py | 7 + .../starworld_tunnel_mpc/robot/bicycle.py | 55 ++ .../robot/mobile_robot.py | 62 ++ .../robot/omnidirectional.py | 23 + .../starworld_tunnel_mpc/robot/unicycle.py | 38 + .../starworld_tunnel_mpc/setup.py | 15 + .../tests/test_motion_control.py | 75 ++ .../starworld_tunnel_mpc/tests/test_soads.py | 176 ++++ .../visualization/__init__.py | 1 + .../visualization/scene_gui.py | 182 +++++ 84 files changed, 8557 insertions(+), 2 deletions(-) delete mode 160000 python/ur_simple_control/path_generation/star_navigation create mode 100644 python/ur_simple_control/path_generation/star_navigation/README.md create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/load_config.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/scene.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py create mode 100755 python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py create mode 100755 python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/setup.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore delete mode 160000 python/ur_simple_control/path_generation/starworld_tunnel_mpc create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py create mode 100755 python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py create mode 100755 python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py diff --git a/python/ur_simple_control/path_generation/star_navigation b/python/ur_simple_control/path_generation/star_navigation deleted file mode 160000 index 2e24e2f..0000000 --- a/python/ur_simple_control/path_generation/star_navigation +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 2e24e2ffca8a7662146e90f06fce74f40af6a316 diff --git a/python/ur_simple_control/path_generation/star_navigation/README.md b/python/ur_simple_control/path_generation/star_navigation/README.md new file mode 100644 index 0000000..07a9481 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/README.md @@ -0,0 +1,18 @@ +# star_navigation + +## Installation +Clone repo and submodule +``` +git clone --recur https://github.com/albindgit/star_navigation.git +``` +Create and activate virtual environment +``` +cd star_navigation +python -m venv venv +. venv/bin/activate +``` +Install package +``` +pip install -e . +pip install -e starworlds +``` diff --git a/python/ur_simple_control/path_generation/star_navigation/config/__init__.py b/python/ur_simple_control/path_generation/star_navigation/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/load_config.py b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py new file mode 100644 index 0000000..0858ef1 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py @@ -0,0 +1,83 @@ +import yaml +import pathlib +from config.scene import Scene +from motion_control.soads import SoadsController +from motion_control.dmp import DMPController +from motion_control.pfmpc_ds import MotionController as PFMPC_ds +from motion_control.pfmpc_obstacle_constraints import MotionController as PFMPC_obstacle_constraints +from motion_control.pfmpc_artificial_reference import MotionController as PFMPC_artificial_reference +from robot import Unicycle, Omnidirectional, Bicycle +import numpy as np + + +def load_config(scene_id=None, robot_type_id=None, ctrl_param_file=None, verbosity=0): + param_path = pathlib.PurePath(__file__).parents[0].joinpath('params') + + + # Load robot + robot_param_path = str(param_path.joinpath('robot_params.yaml')) + with open(r'' + robot_param_path) as stream: + params = yaml.safe_load(stream) + if robot_type_id is None: + print("Select robot ID\n -------------") + for i, k in enumerate(params.keys()): + print(str(i) + ": " + k) + robot_type_id = int(input("-------------\nRobot type: ")) + robot_type = list(params.keys())[robot_type_id] + robot_params = params[robot_type] + + scene = Scene(scene_id, robot_radius=robot_params['radius']) + + if robot_params['model'] == 'Omnidirectional': + robot = Omnidirectional(radius=robot_params['radius'], + vel_max=robot_params['vel_max'], + name=robot_type) + x0 = scene.p0 + elif robot_params['model'] == 'Unicycle': + robot = Unicycle(radius=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']], + vel_max=[robot_params['lin_vel_max'], robot_params['ang_vel_max']], + name=robot_type) + try: + x0 = np.append(scene.p0, [scene.theta0]) + except AttributeError: + x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])]) + elif robot_params['model'] == 'Bicycle': + robot = Bicycle(radius=robot_params['radius'], + vel_min=[robot_params['lin_vel_min'], -robot_params['steer_vel_max']], + vel_max=[robot_params['lin_vel_max'], robot_params['steer_vel_max']], + steer_max=robot_params['steer_max'], + name=robot_type) + try: + x0 = np.append(scene.p0, [scene.theta0, 0]) + # x0 = np.append(scene.p0, [scene.theta0]) + except AttributeError: + x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0]), 0]) + # x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])]) + else: + raise Exception("[Load Config]: Invalid robot model.\n" + "\t\t\tSelection: {}\n" + "\t\t\tValid selections: [Omnidirectional, Unicycle, Bicycle]".format(robot_params['model'])) + + # Load control parameters + ctrl_param_path = str(param_path.joinpath(ctrl_param_file)) + # param_file = str(pathlib.PurePath(pathlib.Path(__file__).parent, ctrl_param_file)) + with open(r'' + ctrl_param_path) as stream: + params = yaml.safe_load(stream) + if 'soads' in params: + controller = SoadsController(params['soads'], robot, verbosity) + elif 'dmp' in params: + controller = DMPController(params['dmp'], robot, scene.reference_path, verbosity) + elif 'pfmpc_ds' in params: + # if global_path is None: + # global_path = [scene.p0.tolist(), scene.pg.tolist()] + controller = PFMPC_ds(params['pfmpc_ds'], robot, scene.reference_path, verbosity) + elif 'pfmpc_obstacle_constraints' in params: + controller = PFMPC_obstacle_constraints(params['pfmpc_obstacle_constraints'], robot, scene.reference_path, verbosity) + elif 'pfmpc_artificial_reference' in params: + controller = PFMPC_artificial_reference(params['pfmpc_artificial_reference'], robot, scene.reference_path, verbosity) + else: + raise Exception("[Load Config]: No valid controller selection in param file.\n" + "\t\t\tSelection: {}\n" + "\t\t\tValid selections: [soads, tunnel_mpc, pfmpc_ds]".format(str(list(params.keys())[0]))) + + return scene, robot, controller, x0 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml new file mode 100644 index 0000000..7785e8d --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml @@ -0,0 +1,16 @@ +dmp: + K: 100 + D: 20 + n_bfs: 1000 + + dt: 0.01 + starify: 1 + hull_epsilon: 0.5 + max_compute_time: 1000 + make_convex: 0 + adapt_obstacle_velocity: 0 + crep: 2.5 + reactivity: 1.2 + tail_effect: 0 + convergence_tolerance: 0.01 + dp_decay_rate: 0.5 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml new file mode 100644 index 0000000..344b3bd --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml @@ -0,0 +1,40 @@ + +pfmpc_artificial_reference: + rhrp_steps: 24 + # MPC +# Q: [100, 100, 1] + R: [0.1, .1] +# S: [0.01, 0.01] + S: [0.01, 0.01] +# T: 1 + K: 0.5 + mu: 5.e+5 + + + Q: [10, 10, 10] +# R: [10, 10] +# R: [0, 10] + T: 10 + + obstacle_padding: 0.0 + + #-------- MPC Build params --------- # + build_mode: 'release' + integration_method: 'RK4' + n_pol: 6 + N: 12 + dt: 0.2 + max_No_ell: 10 + N_obs_predict: 12 + max_No_pol: 10 + max_No_vert: 10 + xaeN_con: 0 + dlp_con: 0 + solver_tol: 1.e-5 + solver_max_time: 500 # Maximum duration for mpc solver in milliseconds + solver_max_inner_iterations: 1000 + solver_max_outer_iterations: 10 + solver_initial_tol: 1.e-4 + solver_delta_tol: 1.e-4 + solver_weight_update_factor: 10.0 + solver_initial_penalty: 1000.0 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml new file mode 100644 index 0000000..06d3214 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml @@ -0,0 +1,42 @@ + +pfmpc_ds: + # Workspace modification + rho0: 0.2 + gamma: 0.9 + make_convex: 1 + iterative_rho_reduction: 0 + max_obs_compute_time: 100 + hull_epsilon: 0.2 + use_prev_workspace: 1 + workspace_horizon: 0 + velocity_obstacle: 1 + # Target generation + convergence_tolerance: 0.1 + max_rhrp_compute_time: 46 + crep: 1.2 + reactivity: 1. + buffer: 1 + rhrp_steps: 12 + nominal_rhrp_horizon: 2 + # MPC + ce: 1 + cs: 1. + R: [0.1, 0.1] + DR: [0.1, 0.] + convergence_margin: 0.02 + lambda: 0.5 + + #-------- MPC Build params --------- # + build_mode: 'release' + integration_method: 'RK4' + n_pol: 6 + N: 6 + dt: 0.2 + solver_tol: 1.e-5 + solver_max_time: 100 # Maximum duration for mpc solver in milliseconds + solver_max_inner_iterations: 110 + solver_max_outer_iterations: 10 + solver_initial_tol: 1.e-4 + solver_delta_tol: 1.e-4 + solver_weight_update_factor: 10.0 + solver_initial_penalty: 1000.0 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml new file mode 100644 index 0000000..dd1f359 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml @@ -0,0 +1,35 @@ + +pfmpc_obstacle_constraints: + # Target generation + convergence_tolerance: 1.e-3 + rhrp_steps: 24 +# max_compute_time: 10 +# buffer: 1 + # MPC + ce: 1 + cs: 1. + R: [0.2, 0.1] + DR: [0.01, 0.] + convergence_margin: 0.02 + + obs_pen: 1.e+5 + obstacle_padding: 0.05 + + #-------- MPC Build params --------- # + build_mode: 'release' + integration_method: 'RK4' + n_pol: 6 + N: 12 + dt: 0.2 + max_No_ell: 10 + N_obs_predict: 12 + max_No_pol: 10 + max_No_vert: 10 + solver_tol: 1.e-5 + solver_max_time: 600 # Maximum duration for mpc solver in milliseconds + solver_max_inner_iterations: 100 + solver_max_outer_iterations: 10 + solver_initial_tol: 1.e-4 + solver_delta_tol: 1.e-4 + solver_weight_update_factor: 10.0 + solver_initial_penalty: 1000.0 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml new file mode 100644 index 0000000..0b6c1d5 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml @@ -0,0 +1,34 @@ + +Omnidirectional: + model: 'Omnidirectional' + radius: 0. + vel_max: 1.5 + +Unicycle_forward: + model: 'Unicycle' + radius: 0. + lin_vel_min: 0. + lin_vel_max: 1.5 + ang_vel_max: 1.5 + +Unicycle_path_following_paper: + model: 'Unicycle' + radius: 0. + lin_vel_min: -0.1 + lin_vel_max: 1 + ang_vel_max: 1 + +Unicycle: + model: 'Unicycle' + radius: 0. + lin_vel_min: -0.1 + lin_vel_max: 1. + ang_vel_max: 1.5 + +Bicycle: + model: 'Bicycle' + radius: 0.5 + steer_max: 1.0 + lin_vel_min: -0.5 + lin_vel_max: 1.5 + steer_vel_max: 0.5 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml new file mode 100644 index 0000000..d21a03d --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml @@ -0,0 +1,14 @@ +soads: + dt: 0.1 + starify: 1 + hull_epsilon: 0.5 + max_compute_time: 1000 + make_convex: 0 + adapt_obstacle_velocity: 1 + unit_magnitude: 0 + crep: 1.5 + reactivity: 1. + tail_effect: 0 + convergence_tolerance: 0.01 + lin_vel: 1.5 + dp_decay_rate: 0.5 diff --git a/python/ur_simple_control/path_generation/star_navigation/config/scene.py b/python/ur_simple_control/path_generation/star_navigation/config/scene.py new file mode 100644 index 0000000..9cc21f9 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/config/scene.py @@ -0,0 +1,769 @@ +import numpy as np +from starworlds.obstacles import Ellipse, StarshapedPolygon, motion_model, Polygon, StarshapedPrimitiveCombination +from starworlds.utils.misc import draw_shapely_polygon +import matplotlib.pyplot as plt +import shapely + + +def zig_zag_motion_model(init_pos, p1, p2, vel): + return motion_model.Waypoints(init_pos, [p1, p2] * 10, vel=vel) + + +def scene_config(id=None): + scene_id = 0 + scene_description = {} + obstacles_to_plot = None + workspace = None + ws_attractors = None + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'No obstacle. Single attractor.' + if id == scene_id: + # workspace = StarshapedPolygon([[-5.3, -2.3], [5.3, -2.3], [5.3, 2.3], [-5.3, 2.3]]) + obstacles = [ + ] + p0 = np.array([-5., 2.]) + reference_path = [[3, 0]] + theta0 = 0 * np.pi / 2 + xlim = [-10, 10] + ylim = [-10, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'No obstacle. Infinity path.' + if id == scene_id: + # workspace = StarshapedPolygon([[-6.3, -2.3], [6.3, -2.3], [6., 2.3], [-6., 2.3]]) + obstacles = [ + ] + reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + 3 * np.sin(4 * np.pi / 90 * s)] + for s in range(int(1.5 * 90))] + p0 = np.array(reference_path[0]) + theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + reference_path[1][0] - reference_path[0][0]) + xlim = [-10, 10] + ylim = [-10, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Single circle obstacle. Single attractor.' + if id == scene_id: + workspace = Ellipse([6., 6.], motion_model=motion_model.Static(pos=[0., 0.])) + workspace = StarshapedPolygon([[-6., -6.], [6., -6.], [6., 6.], [-6., 6.]]) + obstacles = [ + Ellipse([2., 7.], motion_model=motion_model.Static(pos=[0., -2.])) + ] + reference_path = [[2.2, -5.2]] + reference_path = [[5, -4.]] + p0 = np.array([-5, 0]) + theta0 = 0 + xlim = [-10, 10] + ylim = [-10, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Single circle obstacle. Infinity path.' + if id == scene_id: + obstacles = [ + Ellipse([1., 1.], motion_model=motion_model.Static(pos=[0., 0.])) + ] + reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + 3 * np.sin(4 * np.pi / 90 * s)] + for s in range(int(1.5 * 90))] + p0 = np.array(reference_path[0]) + theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + reference_path[1][0] - reference_path[0][0]) + xlim = [-10, 10] + ylim = [-10, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Cross obstacle and one circle. Infinity path.' + if id == scene_id: + obstacles = [ + StarshapedPolygon([[-1.5, -0.2], [1.5, -0.2], [1.5, 0.2], [-1.5, 0.2]]), + StarshapedPolygon([[-0.2, -1.5], [-0.2, 1.5], [0.2, 1.5], [0.2, -1.5]]), + Ellipse([1., 1.], motion_model=motion_model.Static(pos=[4., 3.5])) + ] + reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + 3 * np.sin(4 * np.pi / 90 * s)] + for s in range(90)] + p0 = np.array(reference_path[0]) + theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + reference_path[1][0] - reference_path[0][0]) + xlim = [-10, 10] + ylim = [-10, 10] + obstacles_to_plot = [StarshapedPrimitiveCombination(obstacles[:2], None, xr=[0, 0]), obstacles[-1]] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Two circle obstacles. Infinity path.' + if id == scene_id: + obstacles = [ + Ellipse([1., 1.], motion_model=motion_model.Static(pos=[0., 0.])), + Ellipse([1., 1.], motion_model=motion_model.Static(pos=[4., 3.5])), + ] + reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + 3 * np.sin(4 * np.pi / 90 * s)] + for s in range(2 * 90)] + p0 = np.array(reference_path[0]) + theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + reference_path[1][0] - reference_path[0][0]) + xlim = [-10, 10] + ylim = [-10, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Static crowd. Single attractor.' + if id == scene_id: + workspace = StarshapedPolygon([[-3.5, -2], [5.5, -2], [5.5, 2], [-3.5, 2]]) + obstacles = [ + Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 1.7])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([-.5, 0.9])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 0.1])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 1.6])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([4, 0.8])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([0.5, 2.8])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, 1.4])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1., -.4])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -1.2])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3., 2.2])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -2.2])) + ] + reference_path = [[5., 1]] + p0 = np.array([-3, 1]) + theta0 = 0 + xlim = [-4, 6] + ylim = [-4, 6] + + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Static crowd. Infinity path.' + if id == scene_id: + obstacles = [ + Ellipse([0.5, 0.5], motion_model=motion_model.Static([4.3, 3.2])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.2, 1.3])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.4, 2])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.4, 0.5])), + + # Ellipse([0.7, 0.7], motion_model=motion_model.Static([4.3, 3.2])), + # Ellipse([0.7, 0.7], motion_model=motion_model.Static([1.2, 1.3])), + # Ellipse([0.7, 0.7], motion_model=motion_model.Static([1.5, 2.5])), + # Ellipse([0.7, 0.7], motion_model=motion_model.Static([1.5, 0.])), + StarshapedPolygon([[0, 0], [3, 0], [3, 1], [0, 1]], motion_model=motion_model.Static([-4, -2], rot=-np.pi/4), is_convex=True), + StarshapedPolygon([[0, 0], [1, 0], [1, 3], [0, 3]], motion_model=motion_model.Static([-4, -2], rot=-np.pi/4), is_convex=True), + + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 1.7])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([-.5, 0.9])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 0.1])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 1.6])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([4, 0.8])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([0.5, 2.8])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, 1.4])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1., -.4])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -1.2])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([3., 2.2])), + # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -2.2])) + ] + obstacles_to_plot = obstacles[:-2] + [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-2:]]))] + reference_path = [[6 * np.cos(2 * np.pi * th / 36.5), + 3 * np.sin(4 * np.pi * th / 36.5)] + for th in np.linspace(0,36.5,100)] + p0 = np.array(reference_path[0]) #+ np.array([-2, 0]) + theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + reference_path[1][0] - reference_path[0][0]) + xlim = [-6.5, 6.5] + ylim = [-5, 5] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Dynamic crowd. Single attractor.' + if id == scene_id: + obstacles = [ + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, -5], x1_mag=-0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[4, -3], x1_mag=-0.5)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[7, -1], x1_mag=-0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[5, -2], x1_mag=-0.25, x2_mag=-0.2)), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[1, -5], x1_mag=0.3, x2_mag=0.5)), + Ellipse([0.4, 1], motion_model=motion_model.SinusVelocity(pos=[8, -3], x2_mag=2, x2_period=5)), + # Ellipse([2, 1], motion_model=motion_model.SinusVelocity(pos=[7, -2], x1_mag=2, x1_period=3)), + # Ellipse([0.4, 0.4], motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)), + ] + # reference_path = [[0., -3.], [11, -3]] + reference_path = [[11, -3]] + p0 = np.array([0., -3.]) + theta0 = 0 + xlim = [-1, 12] + ylim = [-8, 4] + + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Static mixed rectangle and circle scene. Single attractor.' + if id == scene_id: + ell_ax = [0.8, 0.8] + obstacles = [ + StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], motion_model=motion_model.Static([2, -3]), is_convex=True), + StarshapedPolygon([(3, 9), (3, 10), (6, 10), (6, 9)], motion_model=motion_model.Static([2, -3]), is_convex=True), + StarshapedPolygon([(3, 6), (3, 7), (0, 7), (0, 6)], motion_model=motion_model.Static([2, -3]), is_convex=True), + StarshapedPolygon([(8, 6), (9, 6), (9, 10), (8, 10)], motion_model=motion_model.Static([-1, -6]), is_convex=True), + StarshapedPolygon([(0, 1.2), (2.5, 1.2), (2.5, 2), (0, 2)], is_convex=True), + StarshapedPolygon([(0, 0), (2.5, 0), (2.5, 0.8), (0, 0.8)], is_convex=True), + StarshapedPolygon([(2, 0), (2.5, 0), (2.5, 2), (2, 2)], is_convex=True), + Ellipse([0.5, 2], n_pol=100, motion_model=motion_model.Static([2, 9], rot=np.pi/4)), + Ellipse([0.5, 2], n_pol=100, motion_model=motion_model.Static([2, 7], rot=-np.pi/4)), + Ellipse(ell_ax, motion_model=motion_model.Static([0.5, 6.5])), + Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 7.5])), + Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])), + ] + reference_path = [[9., 5.]] + p0 = np.array([1, 8.]) + theta0 = 0 + xlim = [-1, 12] + ylim = [-1, 12] + + workspace = StarshapedPolygon([[-6, -6], [6, -6], [6, 6], [-6, 6]]) + obstacles = [ + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5, -1.5])), + StarshapedPolygon([(0, 0), (1, 0), (1, 5), (0, 5)], motion_model=motion_model.Static([-2, -1]), + is_convex=True), + Ellipse([0.5, 1.5], motion_model=motion_model.Static([-3, 3], rot=-np.pi/4)), + Ellipse([0.5, 1.5], motion_model=motion_model.Static([-3, 0], rot=np.pi/4)), + StarshapedPolygon([(0, 0), (1, 0), (1, 4), (0, 4)], motion_model=motion_model.Static([1, -6]), + is_convex=True), + StarshapedPolygon([(0, 3), (4, 3), (4, 4), (0, 4)], motion_model=motion_model.Static([1, -6]), + is_convex=True), + StarshapedPolygon([(0, 0), (1, 0), (1, 4), (0, 4)], motion_model=motion_model.Static([0, 1]), + is_convex=True), + StarshapedPolygon([(3, 0), (4, 0), (4, 4), (3, 4)], motion_model=motion_model.Static([0, 1]), + is_convex=True), + StarshapedPolygon([(0, 0), (4, 0), (4, 1), (0, 1)], motion_model=motion_model.Static([0, 1]), + is_convex=True), + StarshapedPolygon([(1.5, 1), (1.5, -2), (2.5, -2), (2.5, 1)], motion_model=motion_model.Static([0, 1]), + is_convex=True), + StarshapedPolygon([(1.5, -2), (4, -2), (4, -1), (1.5, -1)], motion_model=motion_model.Static([0, 1]), + is_convex=True), + StarshapedPolygon([(0, 1.4), (3, 1.4), (3, 2.4), (0, 2.4)], motion_model=motion_model.Static([-4, -5]), + is_convex=True), + StarshapedPolygon([(0, 0), (3, 0), (3, 1), (0, 1)], motion_model=motion_model.Static([-4, -5]), + is_convex=True), + StarshapedPolygon([(0, 0), (1, 0), (1, 1.5), (0, 1.5)], motion_model=motion_model.Static([-2, -5]), + is_convex=True), + ] + xlim = [-7, 7] + ylim = [-7, 7] + reference_path = [[0., 0.]] + p0 = np.array([-2.5, 1.5]) + # p0 = np.array([2.4, -5.6]) + p0 = np.array([3, .5]) + p0 = np.array([2, 2.5]) + # p0 = np.array([-5, 1.5]) + # p0 = np.array([-2.8, -3.8]) + obstacles_to_plot = obstacles[:4] + [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[4:6]])), + Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[6:11]])), + Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[11:]]))] + + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Static mixed rectangle and circle scene. Given path.' + if id == scene_id: + ell_ax = [0.5, 0.5] + obstacles = [ + StarshapedPolygon([(7, 5), (6, 5), (6, 3), (7, 3)], is_convex=True), + StarshapedPolygon([(6, 3), (6, 7), (5, 7), (5, 3)], is_convex=True), + StarshapedPolygon([(7, 1), (8, 1), (8, 5), (7, 5)], is_convex=True), + StarshapedPolygon([(6, 7), (6, 8), (4, 8), (4, 7)], is_convex=True), + StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], is_convex=True), + StarshapedPolygon([(3, 9), (3, 10), (6, 10), (6, 9)], is_convex=True), + StarshapedPolygon([(8, 6), (9, 6), (9, 10), (8, 10)], is_convex=True), + Ellipse(ell_ax, motion_model=motion_model.Static([7.5, 8])), + Ellipse(ell_ax, motion_model=motion_model.Static([4.8, 5])), + Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])), + Ellipse(ell_ax, motion_model=motion_model.Static([3, 8.5])), + ] + reference_path = [[1., 9.], [1., 5.5], [3.5, 5.5], [3.5, 8.5], [7., 8.5], [7., 5.5], [9., 5.5], [9., 4]] + p0 = np.array(reference_path[0]) + theta0 = -np.pi / 2 + xlim = [-1, 11] + ylim = [-1, 11] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Static DSW equivalent scene. Single attractor.' + if id == scene_id: + workspace = StarshapedPolygon(shapely.ops.unary_union([ + shapely.geometry.Polygon([[-5, -6], [8, -6], [8, 1], [6, 1], [6, 6], [-5, 6], [-5, 0], [-3, -4]]), + shapely.geometry.Point([2.5, -6]).buffer(2)])) + # workspace = StarshapedPolygon([[-5, -6], [8, -6], [8, 1], [6, 1], [6, 6], [-5, 6], [-5, 0], [-3, -4]]) + # workspace = StarshapedPolygon([[-5, -8], [10, -8], [10, 6], [-5, 6]]) + + obstacles = [ + StarshapedPolygon([[0, 0], [3, 0], [3, 1], [0, 1]], motion_model=motion_model.Static([-3, 1], np.pi/4), is_convex=True), + StarshapedPolygon([[3, 0], [3, 3], [2, 3], [2, 0]], motion_model=motion_model.Static([-3, 1], np.pi/4), is_convex=True), + StarshapedPolygon([[-4, 0], [3, 0], [3, 1], [-4, 1]], motion_model=motion_model.Static([3, -5]), is_convex=True), + StarshapedPolygon([[0, 0], [0, 4], [-1, 4], [-1, 0]], motion_model=motion_model.Static([3, -5]), is_convex=True), + StarshapedPolygon([[0, 0], [4, 0], [4, 1], [0, 1]], motion_model=motion_model.Static([-1.5, 0]), is_convex=True), + StarshapedPolygon([[0, 0], [1, 0], [1, 4], [0, 4]], motion_model=motion_model.Static([0, -1.5]), is_convex=True), + StarshapedPolygon([[-.5, 0], [2.5, 0], [2.5, 1], [-.5, 1]], motion_model=motion_model.Static([6, -3.5]), is_convex=True), + StarshapedPolygon([[1.5, -2], [2.5, -2], [2.5, 3], [1.5, 3]], motion_model=motion_model.Static([6, -3.5]), is_convex=True), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 2.5])), + Ellipse([0.8, 1.3], motion_model=motion_model.Static([4, 2], np.pi / 10)), + Ellipse([0.8, 0.8], motion_model=motion_model.Static([3, 2.5])), + Ellipse([1, 1], motion_model=motion_model.Static([1.5, -3.5])), + Ellipse([0.8, 0.8], motion_model=motion_model.Static([-1.2, -1.8])), + Ellipse([1, 1], motion_model=motion_model.Static([5, -1])), + Ellipse([0.7, 0.7], motion_model=motion_model.Static([-2.8, -5.5])), + ] + reference_path = [[2.5, -7]] + p0 = np.array([-3., 3]) + theta0 = 0 + xlim = [-6, 9] + ylim = [-8, 7] + obstacles_to_plot = [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[:2]])), + StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[2:4]])), + StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[4:6]])), + StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[6:8]]))]\ + + obstacles[8:] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Dynamic crowd. Single attractor.' + if id == scene_id: + mean_vel = 0.2 + obstacles = [ + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[3, 5], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 4], x2_mag=-(mean_vel+0.1))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 9], x2_mag=-(mean_vel+0.3))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0, 6], x2_mag=-(mean_vel+0.1))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-4, 4], x2_mag=-(mean_vel+0.3))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 2], x2_mag=-(mean_vel+0.1))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.5, 3], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.5, 3], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.3, -1], x2_mag=-(mean_vel+0.3))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0, 0], x2_mag=-(mean_vel+0.1))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 4.5], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 7], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 10], x2_mag=-(mean_vel+0.1))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1, 11], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 2], x2_mag=-(mean_vel+0.4))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-0.5, 3], x2_mag=-(mean_vel+0.3))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1, 5], x2_mag=-(mean_vel+0.1))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-2.4, 7], x2_mag=-(mean_vel+0.2))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.5, 8], x2_mag=-(mean_vel+0.3))), + Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-3, 5], x2_mag=-(mean_vel+0.1))), + ] + # reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + # 3 * np.sin(4 * np.pi / 90 * s)] + # for s in range(8 * 90)] + # p0 = np.array(reference_path[0]) + # theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + # reference_path[1][0] - reference_path[0][0]) + reference_path = [[0, 10.]] + p0 = np.array([-4., -5.]) + # reference_path = [p0.tolist()] + reference_path + theta0 = np.pi / 2 + xlim = [-8, 8] + ylim = [-6, 12] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Corridor.' + if id == scene_id: + obstacles = [ + # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]), + StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]), + Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])), + # StarshapedPolygon(Ellipse([1, 1]).polygon(), motion_model=motion_model.Interval([-1, 4], [(9, (10, 4))])), + # Ellipse([1, 1], motion_model=motion_model.Interval([-2, 4], [(9, (11, 4))])), + ] + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + reference_path = [[0.5, 5.5]] + theta0 = np.pi + p0 = np.array([9, 4]) + xlim = [0, 10] + ylim = [0, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Dynamic crowd and static polygons. Infinity path.' + if id == scene_id: + + obstacles = [ + Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([4, 3], [4, 4], [4, -4], 0.2)), + Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([3, 0], [3, 3], [3, -3], 0.2)), + Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([-3, -3], [-3, -4], [-3, 4], 0.2)), + Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([2.5, 0], [4, 0], [-4, 0], 0.2)), + # Ellipse([0.5, 0.5], motion_model=motion_model.Waypoints(*zig_zag([4, 3], [4, 3], [4, -3]), vel=0.2)), + # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 0], x2_mag=-.1, x2_period=30)), + # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1, 0], x2_mag=.1, x2_period=20)), + # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 0], x2_mag=-.3, x2_period=40)), + # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-2, 0], x2_mag=.2, x2_period=30)), + # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0, 0], x2_mag=.3, x2_period=25)), + # StarshapedPolygon([[-3, -2], [-1, -2], [-1, -1.5], [-2.5, -1.5], [-2.5, 0], [-3, 0]]) + StarshapedPolygon([[-3, -2], [-1, -2], [-1, -1.5], [-3, -1.5]]), + StarshapedPolygon([[-3, -2], [-2.5, -2], [-2.5, 0], [-3, 0]]), + ] + reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + 3 * np.sin(4 * np.pi / 90 * s)] + for s in range(3 * 90)] + p0 = np.array(reference_path[0]) + theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + reference_path[1][0] - reference_path[0][0]) + xlim = [-10, 10] + ylim = [-10, 10] + obstacles_to_plot = obstacles[:-2] + [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-2:]]))] + + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Ellipse trap. Single attractor.' + if id == scene_id: + obstacles = [ + Ellipse([1, .5], motion_model=motion_model.Static([5, 5], np.pi/2)), + Ellipse([2, .5], motion_model=motion_model.Static([3, 6], 0)), + Ellipse([2, .5], motion_model=motion_model.Static([3, 4], 0)), + # StarshapedPolygon(Ellipse([1, 1]).polygon(), motion_model=motion_model.Interval([-1, 4], [(9, (10, 4))])), + # Ellipse([1, 1], motion_model=motion_model.Interval([-2, 4], [(9, (11, 4))])), + ] + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + reference_path = [[9, 5]] + theta0 = 0 + p0 = np.array([4, 5]) + xlim = [0, 10] + ylim = [0, 10] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Several boundaries. Waypoints.' + if id == scene_id: + workspace = [ + StarshapedPolygon([[-6, 3], [6, 3], [6, 6], [-6, 6]]), + StarshapedPolygon([[-6, -8], [-3, -8], [-3, 6], [-6, 6]]), + StarshapedPolygon([[-6, -8], [6, -8], [6, -5], [-6, -5]]), + StarshapedPolygon([[3, -8], [6, -8], [6, 0], [3, 0]]), + ] + ws_attractors = [ + [-4.5, 4.5], #[[-6., 3], [-3, 3]], + [-3, -6], + [5, -6], [5, -2]] + ws_attractors = [[-3.5, 4.5], [-3.5, -6], [5, -6], [5, -2]] + obstacles = [ + # Ellipse([0.7, 0.7], motion_model=zig_zag_motion_model([-3.5, -2], [-3.5, -4], [-3.5, 1], 0.2)), + Ellipse([0.7, 0.7], motion_model=motion_model.Waypoints([-3.5, -2], [[-3.5, -4], [-3.5, -0.5]], vel=0.2)), + StarshapedPolygon([[-5, -2], [-4, -2], [-4, 1], [-5, 1]]), + Ellipse([1, 1], motion_model=zig_zag_motion_model([0, -6], [-4, -6], [1, -6], 0.2)), + Ellipse([1, 1], motion_model=zig_zag_motion_model([-4, 3.6], [4, 3.6], [-4, 3.6], 0.2)), + Ellipse([.5, .5], motion_model=zig_zag_motion_model([-0.5, 4.5], [-4, 4.5], [3, 4.5], 0.2)), + # Ellipse([.5, .5], motion_model=zig_zag_motion_model([3, -6], [-3, -10], [3, -6], 0.2)), + # StarshapedPolygon([[-3, -2], [-2, -2], [-2, 1], [-3, 1]]), + ] + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + theta0 = np.pi + p0 = np.array([5, 4.5]) + reference_path = [ws_attractors[0]] + xlim = [-8, 8] + ylim = [-9, 7] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Several boundaries. Path.' + if id == scene_id: + workspace = [ + StarshapedPolygon([[-6, 3], [6, 3], [6, 6], [-6, 6]]), + StarshapedPolygon([[-6, -8], [-3, -8], [-3, 6], [-6, 6]]), + StarshapedPolygon([[-6, -8], [6, -8], [6, -5], [-6, -5]]), + StarshapedPolygon([[3, -8], [6, -8], [6, 0], [3, 0]]), + ] + ws_attractors = [ + [-4.5, 4.5], #[[-6., 3], [-3, 3]], + [-3, -6], + [5, -6], [5, -2]] + ws_attractors = [[-3.5, 4.5], [-3.5, -6], [5, -6], [5, -2]] + obstacles = [ + # Ellipse([0.7, 0.7], motion_model=zig_zag_motion_model([-3.5, -2], [-3.5, -4], [-3.5, 1], 0.2)), + Ellipse([0.7, 0.7], motion_model=motion_model.Waypoints([-3.5, -2], [[-3.5, -4], [-3.5, -0.5]], vel=0.2)), + StarshapedPolygon([[-5, -2], [-4, -2], [-4, 1], [-5, 1]]), + Ellipse([1, 1], motion_model=zig_zag_motion_model([0, -6], [-4, -6], [1, -6], 0.2)), + Ellipse([1, 1], motion_model=zig_zag_motion_model([-4, 3.6], [4, 3.6], [-4, 3.6], 0.2)), + Ellipse([.5, .5], motion_model=zig_zag_motion_model([-0.5, 4.5], [-4, 4.5], [3, 4.5], 0.2)), + # Ellipse([.5, .5], motion_model=zig_zag_motion_model([3, -6], [-3, -10], [3, -6], 0.2)), + # StarshapedPolygon([[-3, -2], [-2, -2], [-2, 1], [-3, 1]]), + ] + # obstacles_to_plot = obstacles[:-1] + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + theta0 = np.pi + p0 = np.array([5, 4.5]) + reference_path = [p0.tolist()] + ws_attractors + xlim = [-8, 8] + ylim = [-9, 7] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Enclosed corridor.' + if id == scene_id: + workspace = StarshapedPolygon([[1, 3], [9, 3], [9, 8], [1, 8]]) + obstacles = [ + # StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]), + # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + # StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]), + # Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])), + StarshapedPolygon([[3, 4], [8, 4], [8, 5.5], [3, 5.5]]), + Ellipse([0.5, 0.5], n_pol=80, motion_model=motion_model.Interval([2, 3.5], [(20, (10, 3.5))])), + Ellipse([0.5, 0.5], n_pol=80, motion_model=zig_zag_motion_model([6, 6.6], [6, 7.4], [6, 6.], 0.3)), + ] + + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + reference_path = [[2, 5.5]] + reference_path = [[2.5, 5]] + theta0 = np.pi / 1.2 + p0 = np.array([7, 3.4]) + xlim = [0.5, 9.5] + ylim = [2.5, 8.5] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'DSW example.' + if id == scene_id: + workspace = StarshapedPolygon([[0, 0], [17, 0], [17, 8], [11, 8], [11, 12], [5, 12], [5, 8], [0, 8]]) + obstacles = [ + # StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]), + # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + # StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]), + # Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])), + StarshapedPolygon([[7, 9], [12, 9], [12, 10], [7, 10]]), + StarshapedPolygon([[13, 3], [14, 3], [14, 7], [11, 7], [11, 6], [13, 6]]), + StarshapedPolygon([[4, -1], [8, -1], [8, 2], [6.5, 2], [6.5, 0.5], [4, 0.5]]), + StarshapedPolygon([[12, -1], [15, -1], [15, 1], [12, 1]]), + Ellipse([0.5, 1], motion_model=motion_model.Static([1.5, 2])), + Ellipse([1, 1], motion_model=motion_model.Static([7, 7])), + Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 1.5], np.pi/4)), + Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 2.5], -np.pi/4)), + StarshapedPolygon([[3, 2.5], [4, 2.5], [4, 6.5], [3, 6.5]]), + StarshapedPolygon([[3, 4], [9, 4], [9, 5], [3, 5]]), + ] + obstacles_to_plot = (obstacles[:-4] + + [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-4:-2]]))] + + [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-2:]]))]) + + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + reference_path = [[2, 5.5]] + reference_path = [[9, 11]] + theta0 = np.pi/2 + p0 = np.array([16, 2]) + # p0 = np.array([9, 8]) + xlim = [-1.5, 17.5] + ylim = [-1.5, 12.5] + + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Illustration of ModEnv changing obstacles.' + if id == scene_id: + obstacles = [ + StarshapedPolygon([[0, 0], [3, 0], [3, 3], [2, 3], [2, 1], [0, 1]]), + StarshapedPolygon([[0, 0], [1, 0], [1, 3], [0, 3]]), + Ellipse([0.5, 1], motion_model=motion_model.Static([3, -.5], 0 * np.pi / 2)), + Ellipse([.7, .7], motion_model=motion_model.Static([0, 3.2])) + ] + obstacles_to_plot = [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[:2]]))] + obstacles[2:] + reference_path = [[1.5, 1.6]] + theta0 = 0 + p0 = np.array([0, -1]) + xlim = [-1.5, 4] + ylim = [-2, 4.5] + + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'Dynamic crowd 2. Single attractor.' + if id == scene_id: + mean_vel = 0.3 + obstacles = [ + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[3, 5], x2_mag=-(mean_vel+0.2))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[2, 4], x2_mag=-(mean_vel+0.1))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[2, 9], x2_mag=-(mean_vel+0.3))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0, 6], x2_mag=-(mean_vel+0.1))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-4, 4], x2_mag=-(mean_vel+0.3))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0, 2], x2_mag=-(mean_vel+0.1))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.5, 3], x2_mag=-(mean_vel+0.2))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.5, 10], x2_mag=-(mean_vel+0.2))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.3, -1], x2_mag=-(mean_vel+0.3))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[2, 0], x2_mag=-(mean_vel+0.1))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0.5, 9], x2_mag=-(mean_vel+0.1))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-3, 7], x2_mag=-(mean_vel+0.2))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-0.5, 12], x2_mag=-(mean_vel+0.1))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-4, 12], x2_mag=-(mean_vel+0.2))), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[3, 13], x2_mag=-(mean_vel+0.1))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1, 11], x2_mag=-(mean_vel+0.2))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0.5, 2], x2_mag=-(mean_vel+0.4))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-0.5, 3], x2_mag=-(mean_vel+0.3))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1, 5], x2_mag=-(mean_vel+0.1))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-2.4, 7], x2_mag=-(mean_vel+0.2))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.5, 8], x2_mag=-(mean_vel+0.3))), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-3, 5], x2_mag=-(mean_vel+0.1))), + ] + # reference_path = [[6 * np.cos(2 * np.pi / 90 * s), + # 3 * np.sin(4 * np.pi / 90 * s)] + # for s in range(8 * 90)] + # p0 = np.array(reference_path[0]) + # theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1], + # reference_path[1][0] - reference_path[0][0]) + reference_path = [[0, 10.]] + p0 = np.array([-4., -5.]) + # reference_path = [p0.tolist()] + reference_path + theta0 = np.pi / 2 + xlim = [-8, 8] + ylim = [-6, 12] + + # ----------------------------------- # + scene_id += 1 + scene_description[scene_id] = 'MPC steps illustration.' + if id == scene_id: + workspace = StarshapedPolygon([[0, 0], [17, 0], [17, 8], [11, 8], [11, 12], [5, 12], [5, 8], [0, 8]]) + obstacles = [ + # StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]), + # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + # StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]), + # Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])), + # StarshapedPolygon([[7, 9], [12, 9], [12, 10], [7, 10]]), + # StarshapedPolygon([[4, -1], [8, -1], [8, 2], [6.5, 2], [6.5, 0.5], [4, 0.5]]), + # StarshapedPolygon([[12, -1], [15, -1], [15, 1], [12, 1]]), + Ellipse([0.5, 1], motion_model=motion_model.Static([1.5, 2])), + Ellipse([1, 1], motion_model=motion_model.Static([7, 7])), + Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 1.5], np.pi / 4)), + Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 2.5], -np.pi / 4)), + StarshapedPolygon([[3, 2.5], [4, 2.5], [4, 6.5], [3, 6.5]]), + StarshapedPolygon([[3, 4], [9, 4], [9, 5], [3, 5]]), + StarshapedPolygon([[13, 3], [14, 3], [14, 7], [13, 7]]), + StarshapedPolygon([[13, 7], [16, 7], [16, 6], [13, 6]]), + StarshapedPolygon([[13, 4], [16, 4], [16, 3], [13, 3]]) + ] + obstacles_to_plot = (obstacles[:-5] + + [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-5:-3]]))] + + [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-3:]]))]) + + # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]] + # self.p0 = np.array(self.reference_path[0]) + reference_path = [[2, 5.5]] + reference_path = [[9, 11]] + theta0 = np.pi / 2 * 0.2 + p0 = np.array([14.5, 5]) + # p0 = np.array([9, 8]) + xlim = [-1.5, 17.5] + ylim = [-1.5, 12.5] + + while id is None or not (1 <= id <= scene_id): + print("Select scene ID\n -------------") + for i, description in scene_description.items(): + print(str(i) + ": " + description) + try: + id = int(input("-------------\nScene ID: ")) + except: + pass + # if not valid_id(id): + # print("Invalid scene id: " + str(id) + ". Valid range [1, " + str(scene_id) + "].\n") + + return scene_config(id) + + if obstacles_to_plot is None: + obstacles_to_plot = obstacles + return p0, theta0, reference_path, ws_attractors, obstacles, workspace, obstacles_to_plot, xlim, ylim, id + + +# +# def scene_description(): +# return scene_config() + +class Scene: + + def __init__(self, id=None, robot_radius=0.): + self.id = id + self.p0, self.theta0, self.reference_path, self.ws_attractors, self.obstacles, self.workspace, \ + self._obstacles_to_plot, self.xlim, self.ylim, self.id = scene_config(id) + # Compute all polygon + [o.polygon() for o in self.obstacles] + [o.kernel() for o in self.obstacles] + [o.is_convex() for o in self.obstacles] + self.robot_radius = robot_radius + # if robot_radius > 0: + # self.obstacles = [o.dilated_obstacle(padding=robot_radius, id="duplicate") for o in self.obstacles] + self._static_obstacle = [o._motion_model is None or o._motion_model.__class__.__name__ == "Static" for o in self.obstacles] + self.is_static = all(self._static_obstacle) + + def step(self, dt, robot_pos): + if self.is_static: + return + for i, o in enumerate(self.obstacles): + if self._static_obstacle[i]: + continue + prev_pos, prev_rot = o.pos().copy(), o.rot() + o.move(dt) + # if not o.exterior_point(robot_pos): + # NOTE: Not well implemented. E.g. with hardcoded dt + if o.polygon().distance(shapely.geometry.Point(robot_pos)) < max(abs(o._motion_model.lin_vel()))*0.2: + print("[Scene]: Obstacle " + str(o) + " stands still to avoid moving into robot.") + o._motion_model.set_pos(prev_pos) + o._motion_model.set_rot(prev_rot) + for i in range(len(self.obstacles)): + if self._static_obstacle[i]: + continue + if self.obstacles[i]._motion_model is not None: + self._obstacles_to_plot[i]._motion_model.set_pos(self.obstacles[i].pos()) + self._obstacles_to_plot[i]._motion_model.set_rot(self.obstacles[i].rot()) + + def init_plot(self, ax=None, workspace_color='k', workspace_alpha=0.6, obstacle_color='lightgrey', obstacle_edge_color='k', obstacle_alpha=1, show_obs_name=False, draw_p0=1, + draw_ref=1, reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14): + if ax is None: + _, ax = plt.subplots(subplot_kw={'aspect': 'equal'}) + ax.set_xlim(self.xlim) + ax.set_ylim(self.ylim) + ax.set_aspect('equal') + line_handles = [] + if draw_p0: + ax.plot(*self.p0, 'ks', markersize=10) + if draw_ref: + if len(self.reference_path) > 1: + ax.plot([r[0] for r in self.reference_path], [r[1] for r in self.reference_path], color=reference_color, + alpha=reference_alpha, zorder=-2) + else: + if isinstance(self.workspace, list): + [ax.plot(*attr, color=reference_color, alpha=reference_alpha, + marker=reference_marker, + markersize=reference_markersize) for attr in self.ws_attractors] + else: + ax.plot(*self.reference_path[-1], color=reference_color, alpha=reference_alpha, marker=reference_marker, + markersize=reference_markersize) + + if self.workspace is not None: + draw_shapely_polygon(pol=shapely.geometry.box(self.xlim[0], self.ylim[0], self.xlim[1], self.ylim[1]), ax=ax, fc=obstacle_color, alpha=obstacle_alpha, ec='None', zorder=-10) + if isinstance(self.workspace, list): + [b.draw(ax=ax, fc='w', ec='None', show_reference=False, zorder=-9) for b in self.workspace] + else: + self.workspace.draw(ax=ax, fc='w', ec=obstacle_edge_color, show_reference=False, zorder=-9) + + for o in self._obstacles_to_plot: + lh, _ = o.init_plot(ax=ax, fc=obstacle_color, alpha=obstacle_alpha, ec=obstacle_edge_color, show_name=show_obs_name, show_reference=False) + line_handles.append(lh) + + return line_handles, ax + + def update_plot(self, line_handles): + for i, o in enumerate(self._obstacles_to_plot): + o.update_plot(line_handles[i]) + + def draw(self, ax=None, workspace_color='k', workspace_alpha=0.6, obstacle_color='lightgrey', obstacle_edge_color='k', obstacle_alpha=1, show_obs_name=False, draw_p0=1, + draw_ref=1, reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14): + lh, ax = self.init_plot(ax, workspace_color, workspace_alpha, obstacle_color, obstacle_edge_color, obstacle_alpha, show_obs_name, draw_p0, + draw_ref, reference_color, reference_alpha, reference_marker, reference_markersize) + self.update_plot(lh) + return lh, ax diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py new file mode 100644 index 0000000..261a8ac --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py @@ -0,0 +1 @@ +from .dmp import DMPController, DMP \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py new file mode 100644 index 0000000..15a2d7a --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py @@ -0,0 +1,175 @@ +import numpy as np +import shapely +from starworlds.starshaped_hull import cluster_and_starify +from starworlds.utils.misc import tic, toc +from motion_control.soads import f_nominal as soads_f + +class DMP: + + def __init__(self, parameters, reference_path, path_speed): + self.K = parameters['K'] + self.D = parameters['D'] + self.alpha_s = None + self.n_bfs = parameters['n_bfs'] + self.n = 2 # Hardcoded 2D path + self.goal = np.array(reference_path.coords[-1]) + self.p0 = np.array(reference_path.coords[0]) + self.centers = None + self.widths = None + self.w = None + self.fit(reference_path, path_speed) + self.z0 = np.concatenate((self.p0, 0 * self.p0)) + + def p(self, z): + assert len(z) == 2*self.n + return z[:self.n] + + def v(self, z, obstacles=None, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False, + convergence_tolerance=1e-4): + assert len(z) == 2*self.n + if obstacles is None and workspace is None: + return z[self.n:] + else: + return soads_f(z[:self.n], z[self.n:], obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect, + convergence_tolerance) + + def f(self, z, t, obstacles=None, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False, + convergence_tolerance=1e-4): + s = np.exp(-self.alpha_s * t) + psi = self.basis_fcn(s) + fe = self.w.dot(psi) / np.maximum(np.sum(psi), 1e-8) * (self.goal - self.p0) * s + return self.K * (self.goal - self.p(z)) - self.D * self.v(z, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect, + convergence_tolerance) + fe + + def dz(self, z, t, obstacles=None, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False, + convergence_tolerance=1e-4): + # return np.concatenate((self.v(z, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect, + # convergence_tolerance), self.f(z, t))) + return np.concatenate((self.v(z, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect, + convergence_tolerance), self.f(z, t, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect, + convergence_tolerance))) + + def basis_fcn(self, s, i=None): + if i is not None: + return np.exp(-1 / (2 * self.widths[i] ** 2) * (s - self.centers[i]) ** 2) + if i is None: + return np.exp(-1 / (2 * self.widths ** 2) * (s - self.centers) ** 2) + + def fit(self, reference_path, path_speed): + delta_p = int(reference_path.length / path_speed) + t_demo = np.linspace(0, reference_path.length, 10000) + self.alpha_s = 1. / t_demo[-1] + + p_demo = np.array([reference_path.interpolate(s).coords[0] for s in t_demo]).T + + # Set basis functions + t_centers = np.linspace(0, reference_path.length, self.n_bfs, endpoint=True) + self.centers = np.exp(-self.alpha_s * t_centers) + widths = np.abs((np.diff(self.centers))) + self.widths = np.concatenate((widths, [widths[-1]])) + + # Calculate derivatives + dp_demo = (p_demo[:, 1:] - p_demo[:, :-1]) / (t_demo[1:] - t_demo[:-1]) + dp_demo = np.concatenate((dp_demo, np.zeros((self.n, 1))), axis=1) + ddp_demo = (dp_demo[:, 1:] - dp_demo[:, :-1]) / (t_demo[1:] - t_demo[:-1]) + ddp_demo = np.concatenate((ddp_demo, np.zeros((self.n, 1))), axis=1) + + # Compute weights + x_seq = np.exp(-self.alpha_s * t_demo) + self.w = np.zeros((self.n, self.n_bfs)) + for i in range(self.n): + if abs(self.goal[i] - self.p0[i]) < 1e-5: + continue + f_gain = x_seq * (self.goal[i] - self.p0[i]) + f_target = ddp_demo[i, :] - self.K * (self.goal[i] - p_demo[i, :]) + self.D * dp_demo[i, :] + for j in range(self.n_bfs): + psi_j = self.basis_fcn(x_seq, j) + num = f_gain.dot((psi_j * f_target).T) + den = f_gain.dot((psi_j * f_gain).T) + if abs(den) < 1e-14: + continue + self.w[i, j] = num / den + + +class DMPController: + + def __init__(self, params, robot, reference_path, verbose=False): + if not robot.__class__.__name__ == 'Omnidirectional': + raise NotImplementedError("DMPController only implemented for Omnidirectional robots.") + self.params = params + self.robot = robot + self.obstacle_clusters = None + self.obstacles_star = [] + self.dp_prev = None + self.verbose = verbose + self.reference_path = shapely.geometry.LineString(reference_path) + self.theta_g = self.reference_path.length + self.dmp = DMP(params, self.reference_path, robot.vmax) + self.z = self.dmp.z0.copy() + self.u = None + self.theta = 0 + self.timing = {'workspace': 0, 'target': 0, 'mpc': 0} + + def compute_u(self, x): + return self.v_ref #+ 1 * (self.p_ref - self.robot.h(x)) + + def update_policy(self, x, obstacles, workspace=None): + p = self.robot.h(x) + + # if self.params['starify']: + # self.obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles, p, pg, + # self.params['hull_epsilon'], + # max_compute_time= self.params['max_compute_time'], + # workspace=workspace, + # previous_clusters=self.obstacle_clusters, + # # dx_prev=self.dp_prev, + # make_convex=self.params['make_convex'], + # verbose=self.verbose) + # self.timing['obstacle'] = sum(obstacle_timing) + # self.obstacles_star = [cl.cluster_obstacle for cl in self.obstacle_clusters] + # else: + # self.timing['obstacle'] += 0 + # self.obstacles_star = obstacles + + # t0 = tic() + # dist_to_goal = np.linalg.norm(p - pg) + + dtheta_dt = 0.2 + + # obstacles=None + self.v_ref = self.dmp.v(self.z, obstacles, workspace, unit_magnitude=False, crep=self.params['crep'], + reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'], + adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'], + convergence_tolerance=self.params['convergence_tolerance']) * dtheta_dt + self.p_ref = self.dmp.p(self.z) + + dv = (self.v_ref - self.z[2:]) / self.params['dt'] + + dz = np.concatenate((self.v_ref, dv)) + dz = self.dmp.dz(self.z, self.theta, obstacles, workspace, unit_magnitude=False, crep=self.params['crep'], + reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'], + adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'], + convergence_tolerance=self.params['convergence_tolerance']) + + # Target state integration + # dz = np.concatenate((self.dmp.v(self.z), self.dmp.f(self.z, self.theta))) + self.z += dz * dtheta_dt * self.params['dt'] + + self.theta += dtheta_dt * self.params['dt'] + + + + # p_next = p + dp * self.params['dt'] + # + # while not all([o.exterior_point(p_next) for o in self.obstacles_star]) and (workspace is None or workspace.interior_point(p_next)): + # dp *= self.params['dp_decay_rate'] + # p_next = p + dp * self.params['dt'] + # # Additional compute time check + # if toc(t0) > self.params['max_compute_time']: + # if self.verbose or True: + # print("[Max compute time in soads when adjusting for collision. ") + # dp *= 0 + # break + # self.timing['control'] = toc(t0) + # + # self.dp_prev = dp diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py new file mode 100644 index 0000000..987aeaa --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py @@ -0,0 +1,2 @@ +from .mpc import Mpc, pol2pos +from .motion_controller import MotionController \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py new file mode 100644 index 0000000..f3b6efc --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py @@ -0,0 +1,142 @@ +from motion_control.pfmpc_artificial_reference import Mpc, pol2pos +from starworlds.utils.misc import tic, toc +import numpy as np +import shapely + + +class MotionController: + def __init__(self, params, robot, reference_path, verbosity=0): + self.params = params + self.robot = robot + self.mpc = Mpc(params, robot) + self.verbosity = verbosity + self.reference_path = shapely.geometry.Point(reference_path[0]) if len(reference_path) == 1 else shapely.geometry.LineString(reference_path) # Waypoints + self.theta_g = self.reference_path.length + self.rhrp_L = self.params['N'] * self.params['dt'] * self.mpc.build_params['w_max'] + self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps']) + self.reset() + + def reset(self): + self.mpc.reset() + self.theta = 0 + self.rhrp_path = None + # self.rhrp_L = self.params['N'] * self.mpc.build_params['dp_max'] + # self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps']) + # self.receding_ds = self.params['normalized_reference_stepsize'] * self.mpc.build_params['dp_max'] + # self.rhrp_s = np.arange(0, self.rhrp_L + self.receding_ds, self.receding_ds) + self.path_pol = None + self.epsilon = None + self.solution = None + self.sol_feasible = None + self.u_prev = [0] * self.robot.nu + self.timing = {'workspace': 0, 'target': 0, 'mpc': 0} + + def extract_obs_par(self, obstacles): + n_ell_par = 3 * self.mpc.build_params['max_No_ell'] * (1 + self.mpc.build_params['N_obs_predict']) + n_pol_par = self.mpc.build_params['max_No_pol'] * self.mpc.build_params['max_No_vert'] * 2 + obs_par = [0] * (n_ell_par + n_pol_par) + obs_par[1:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params['max_No_ell'] + obs_par[2:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params['max_No_ell'] + obs_par_padded = obs_par.copy() + n_ell, n_pol = 0, 0 + + for o in obstacles: + if hasattr(o, "_a"): + # print(o.pos()) + j = n_ell * 3 * (1 + self.mpc.build_params['N_obs_predict']) + obs_par[j] = 1 # Include obstacle + obs_par[j + 1:j + 3] = o._a # Ellipse axes + # Ugly coding for prediction + mm = o._motion_model + pos, rot, t = mm.pos().copy(), mm.rot(), mm._t + if hasattr(mm, '_wp_idx'): + wp_idx = mm._wp_idx + for k in range(self.mpc.build_params['N_obs_predict']): + obs_par[j + 3 + 3 * k:j + 5 + 3 * k] = mm.pos() # Ellipse position + obs_par[j + 5 + 3 * k] = mm.rot() # Ellipse orientation + mm.move(None, self.mpc.build_params['dt']) + mm.set_pos(pos), mm.set_rot(rot) + mm._t = t + if hasattr(mm, '_wp_idx'): + mm._wp_idx = wp_idx + + obs_par_padded[j:j+3*(self.mpc.build_params['N_obs_predict']+1)] = obs_par[j:j+3*(self.mpc.build_params['N_obs_predict']+1)] + obs_par_padded[j + 1:j + 3] = o._a + self.params['obstacle_padding'] + + n_ell += 1 + + if hasattr(o, "vertices"): + idx = n_ell_par + n_pol * self.mpc.build_params['max_No_vert'] * 2 + vertices = shapely.ops.orient(o.polygon()).exterior.coords[:-1] + for i in range(self.mpc.build_params['max_No_vert']): + obs_par[idx+i*2:idx+(i+1)*2] = vertices[i % len(vertices)] + vertices = shapely.ops.orient(o.polygon().buffer(self.params['obstacle_padding'], quad_segs=1, cap_style=3, join_style=2)).exterior.coords[:-1] + for i in range(self.mpc.build_params['max_No_vert']): + obs_par_padded[idx+i*2:idx+(i+1)*2] = vertices[i % len(vertices)] + n_pol += 1 + + # for i in range(self.mpc.build_params['max_No_ell']): + # j = i * 3 * (1 + self.mpc.build_params['N_obs_predict']) + # include_obs = obs_par[j] + # ell_axs = obs_par[j + 1:j + 3] + # ell_pos = obs_par[j + 3 + 3 * k:j + 5 + 3 * k] + # ell_rot = obs_par[j + 5 + 3 * k] + # print(include_obs,ell_axs,ell_pos,ell_rot) + # + # for i in range(self.mpc.build_params['max_No_pol']): + # j = n_ell_par + i * self.mpc.build_params['max_No_vert'] * 2 + # print(obs_par[j : j + 2 * self.mpc.build_params['max_No_vert']]) + return obs_par, obs_par_padded + + def compute_u(self, x): + return self.u_prev + + def update_policy(self, x, obstacles, workspace=None): + p = self.robot.h(x) + + # Extract receding path from global target path + t0 = tic() + rhrp_path_sh = shapely.ops.substring(self.reference_path, start_dist=self.theta, end_dist=self.theta + self.rhrp_L) + if rhrp_path_sh.length > 0: + self.rhrp_path = np.array([rhrp_path_sh.interpolate(s).coords[0] for s in self.rhrp_s]) + else: + self.rhrp_path = np.tile(rhrp_path_sh.coords[0], (len(self.rhrp_s), 1)) + + # Approximate target with polynomials + self.path_pol = np.polyfit(self.rhrp_s, self.rhrp_path[:, 0], self.params['n_pol']).tolist() + \ + np.polyfit(self.rhrp_s, self.rhrp_path[:, 1], self.params['n_pol']).tolist() + # Force init position to be correct + self.path_pol[self.params['n_pol']] = self.rhrp_path[0, 0] + self.path_pol[-1] = self.rhrp_path[0, 1] + # Compute polyfit approximation error + self.epsilon = max(np.linalg.norm(self.rhrp_path - np.array([pol2pos(self.path_pol, s, self.mpc.build_params['n_pol']) for s in self.rhrp_s]), axis=1)) + self.timing['target'] = toc(t0) + + t0 = tic() + # Extract obstacle parameters (Assumes all ellipses) + obs_par, obs_par_padded = self.extract_obs_par(obstacles) + # Compute MPC solution + if self.solution is not None: + init_guess = self.solution.copy() + init_guess[:3] = x + else: + init_guess = None + solution_data = self.mpc.run(x.tolist(), self.path_pol, self.params, obs_par_padded, init_guess) + # solution_data = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, 1, 0.1, self.solution) + if solution_data is None: + self.sol_feasible, self.mpc_exit_status = False, "None" + else: + self.solution, self.mpc_exit_status = solution_data.solution, solution_data.exit_status + self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, obs_par, self.params, d=self.verbosity > 0) + # self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, 1, 0.1, d=self.verbosity > 0) + + if self.sol_feasible: + xa0, u, ua, w = self.mpc.sol2xa0uuaw(self.solution) + self.u_prev = u[:self.robot.nu] + p_ref_dist = rhrp_path_sh.distance(shapely.geometry.Point(p)) + if self.theta_g > 0 and p_ref_dist < 1: + self.theta = min(self.theta + w[0] * self.params['dt'], self.theta_g) + else: + self.u_prev = [0] * self.robot.nu + + self.timing['mpc'] = toc(t0) diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py new file mode 100644 index 0000000..ff960bb --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py @@ -0,0 +1,489 @@ +import casadi.casadi as cs +import opengen as og +import numpy as np +import os, sys +import yaml + + +class NoSolutionError(Exception): + '''raise this when there's no solution to the mpc problem''' + pass + +def pol_eval(pol_coeff, x, n_pol): + return sum([pol_coeff[i] * x ** (n_pol - i) for i in range(n_pol+1)]) + + +def pol2pos(pol_coeff, s, n_pol): + return [pol_eval(pol_coeff[:n_pol+1], s, n_pol), pol_eval(pol_coeff[n_pol+1:], s, n_pol)] + +# Returns >0 for interior points, <0 for exterior points, =0 for boundary points +def ellipse_function(p, pos, rot, ax): + x_dist = p[0] - pos[0] + y_dist = p[1] - pos[1] + s, c = cs.sin(rot), cs.cos(rot) + normalized_center_dist = ((x_dist * c + y_dist * s) ** 2) / (ax[0] ** 2) + ((x_dist * s + y_dist * c) ** 2) / (ax[1] ** 2) + return 1 - normalized_center_dist + +# Returns >0 for interior points, otherwise 0 +def convex_polygon_function(p, vertices, N): + hp_vals = [] + for i in range(N-1): + idc1, idc2 = i*2, (i+1) % N * 2 + x1, y1 = vertices[idc1], vertices[idc1+1] + x2, y2 = vertices[idc2], vertices[idc2+1] + # x2, y2 = vertices[(i + 1) % N * 2], vertices[(i + 1) % N + 1] + dx = x2 - x1 + dy = y2 - y1 + Ai = cs.vertcat(dy, -dx) + bi = x1 * dy - y1 * dx + # print(i, [x1,y1],[x2, y2], bi-cs.dot(Ai, p)) + hp_vals += [bi-cs.dot(Ai, p)] + return cs.vertcat(*hp_vals) + +class Mpc: + + def __init__(self, params, robot): + self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build') + # Load parameters + self.build_params = None + self.robot = robot + self.set_build_params(params) + rebuild, self.build_name = self.get_build_version() + self.generate_sol_fun() + # Build if different from latest build + if rebuild: + self.build() + else: + print("Found MPC build: {}".format(self.build_name)) + + sys.path.insert(1, os.path.join(self.build_dir, self.build_name)) + optimizer = __import__(self.build_name) + self.solver = optimizer.solver() + self.reset() + + def reset(self): + self.sol_prev = None + + def set_build_params(self, params): + self.build_params = { + 'mode': params['build_mode'], + 'N': params['N'], + 'dt': params['dt'], + 'solver_tol': params['solver_tol'], + 'solver_max_time': params['solver_max_time'], + 'solver_max_inner_iterations': params['solver_max_inner_iterations'], + 'solver_max_outer_iterations': params['solver_max_outer_iterations'], + 'x_min': self.robot.x_min, + 'x_max': self.robot.x_max, + 'u_min': self.robot.u_min, + 'u_max': self.robot.u_max, + 'nx': self.robot.nx, + 'nu': self.robot.nu, + 'np': 2, + 'robot_model': self.robot.__class__.__name__, + 'n_pol': params['n_pol'], + 'integration_method': params['integration_method'], + 'w_max': self.robot.vmax, + 'max_No_ell': params['max_No_ell'], + 'max_No_pol': params['max_No_pol'], + 'max_No_vert': params['max_No_vert'], + 'N_obs_predict': params['N_obs_predict'], + 'dlp_con': params['dlp_con'], + 'xaeN_con': params['xaeN_con'], + } + + def get_build_version(self): + builds = [name for name in os.listdir(self.build_dir) + if os.path.isdir(os.path.join(self.build_dir, name))] + for build_ver in builds: + if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')): + continue + with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file: + build_ver_params = yaml.load(file, Loader=yaml.FullLoader) + if self.build_params == build_ver_params: + return False, build_ver + ver = 0 + while 'ver' + str(ver) in os.listdir(self.build_dir): + ver += 1 + return True, 'ver' + str(ver) + + def cs_obstacle_evaluation(self, obs_par, x, k): + n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict']) + obs_val = [] + for i in range(self.build_params['max_No_ell']): + j = i * 3 * (1 + self.build_params['N_obs_predict']) + time_idx = min(k, self.build_params['N_obs_predict']-1) + include_obs = obs_par[j] + ell_axs = obs_par[j + 1:j + 3] + ell_pos = obs_par[j + 3 + 3 * time_idx:j + 5 + 3 * time_idx] + ell_rot = obs_par[j + 5 + 3 * time_idx] + ell_val = ellipse_function(self.robot.h(x), ell_pos, ell_rot, ell_axs) + obs_val += [cs.fmax(include_obs * ell_val, 0)] + + for i in range(self.build_params['max_No_pol']): + j = n_ell_par + i * self.build_params['max_No_vert'] * 2 + hp_vals = convex_polygon_function(self.robot.h(x), obs_par[j : j + 2 * self.build_params['max_No_vert']], self.build_params['max_No_vert']) + obs_val += [cs.fmax(cs.mmin(hp_vals),0)] + return cs.vertcat(*obs_val) + + def is_feasible(self, sol, x0, path_pol, obs_par, params, d=False): + con = self.sol2con(sol, x0, path_pol, params) + sol_min, sol_max = self.sol_bounds() + con_min, con_max = self.con_bounds() + + obs_val = self.sol2obsval(sol, x0, obs_par) + + eps = 1.e-4 + + def in_range(val, min, max): + return ((np.array(val) >= np.array(min)) & (np.array(val) <= np.array(max))).all() + + sol_ok = True + + if not in_range(sol, sol_min - eps, sol_max + eps): + sol_ok = False + if not in_range(con, con_min - eps, con_max + eps): + sol_ok = False + if not in_range(obs_val, 0 - eps, 0 + eps): + sol_ok = False + + return sol_ok + + # if not in_range(u, u_min, u_max): + # sol_ok = False + # if d: + # print("[MPC]: Bad u.") + # print(u) + # if not in_range(w, w_min, w_max): + # sol_ok = False + # if d: + # print("[MPC]: Bad w. Not in [{:.4f}, {:.4f}]".format(0, self.build_params['dp_max'])) + # print(w) + # if not in_range(x, x_min, x_max): + # sol_ok = False + # if d: + # print("[MPC]: Bad x") + # print(x) + # if not in_range(s, s_min, s_max): + # sol_ok = False + # if d: + # print("[MPC]: Bad sN {:.4f} > {:.4f}".format(s[-1], s_max[0])) + # return sol_ok + + def error(self, x, s, path_pol): + p_ref = cs.vertcat(*pol2pos(path_pol, s, self.build_params['n_pol'])) + return cs.norm_2(p_ref - self.robot.h(x)) + + def base_solution(self, x0, path_pol, w0): + u = [0] * (self.build_params['N'] * self.build_params['nu']) + xa0 = x0.copy() + w = [0] * (self.build_params['N']) + w[0] = w0 + + if self.build_params['robot_model'] == 'Unicycle': + p_ref = pol2pos(path_pol, w0*self.build_params['dt'], self.build_params['n_pol']) + k1, k2 = 10, 2 + # k1, k2 = 0.15, 0.3 + + x = x0.copy() + for i in range(self.build_params['N']): + e = np.array(x[:2]) - np.array(p_ref) + th = x[2] + + r_th = np.arctan2(e[1], e[0]) + np.pi + # E Theta in (-pi, pi] + e_th = r_th - th + e_th0 = e_th + while e_th > np.pi: + e_th -= 2 * np.pi + while e_th <= -np.pi: + e_th += 2 * np.pi + + ui = [ + -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)), + k2 * (e_th) + ] + + if self.robot.u_min[0] == 0 and ui[0] < 0: + ui[0] = 0 + u0_sig = 1 + else: + u0_sig = ui[0] / self.robot.u_max[0] if ui[0] >= 0 else ui[0] / self.robot.u_min[0] + u1_sig = ui[1] / self.robot.u_max[1] if ui[1] >= 0 else ui[1] / self.robot.u_min[1] + + sig = max(u0_sig, u1_sig, 1) + if sig > 1: + if u0_sig > u1_sig: + ui[0] = self.robot.u_max[0] if ui[0] > 0 else self.robot.u_min[0] + ui[1] /= sig + else: + ui[0] /= sig + ui[1] = self.robot.u_max[1] if ui[1] > 0 else self.robot.u_min[1] + u[i * self.build_params['nu']:(i+1) * self.build_params['nu']] = ui + # print(e, e_th, ui) + x, _ = self.robot.move(x, ui, self.build_params['dt']) + + ua = u + + return xa0 + u + ua + w + + def discrete_integration(self, f, x, u): + dt = self.build_params['dt'] + if self.build_params['integration_method'] == 'euler': + return x + f(x, u) * dt + if self.build_params['integration_method'] == 'RK4': + k1 = f(x, u) + k2 = f(x + dt / 2 * k1, u) + k3 = f(x + dt / 2 * k2, u) + k4 = f(x + dt * k3, u) + return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) + raise NotImplemented + + def generate_sol_fun(self): + # Initialize + sol_sym = cs.SX.sym('sol', self.build_params['nx'] + (2 * self.build_params['nu'] + 1) * self.build_params['N']) + x0_sym = cs.SX.sym('x0', self.build_params['nx']) + path_pol_sym = cs.SX.sym('path_pol', self.build_params['np'] * (self.build_params['n_pol'] + 1)) + + n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict']) + n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2 + obs_par_sym = cs.SX.sym('obs_par', n_ell_par + n_pol_par) + # obs_par_sym = cs.SX.sym('obs_par', self.build_params['max_No_ell'] * self.build_params['N_obs_predict'] * 6 + self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2) + cost_params = {'Q': self.build_params['nx'], 'R': self.build_params['nu'], 'K': 1, + 'S': self.build_params['nu'], 'T': 1, 'mu': 1} + cost_params_sym = cs.SX.sym("cost_params", sum(list(cost_params.values()))) + # Exchange parameter dimension with SX variable + p_idx = 0 + for key, dim in cost_params.items(): + cost_params[key] = cost_params_sym[p_idx:p_idx + dim] + p_idx += dim + # Initialize + xa0, u, ua, w = self.sol2xa0uuaw(sol_sym) + u_k, ua_k, w_k = u[:self.build_params['nu']], ua[:self.build_params['nu']], w[0] + x_k, xa_k, s_k = x0_sym, xa0, 0 + ea_k = self.error(xa_k, s_k, path_pol_sym) + obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, 0) + obs_val_a_k = self.cs_obstacle_evaluation(obs_par_sym, xa_k, 0) + # lp_k = cost_params['K'] * ea_k ** 2 + cs.dot(cost_params['S'], ua_k * ua_k) + cost_params['T'] * (w_k - self.build_params['dp_max']) ** 2 + lp_k = cost_params['K'] * ea_k ** 2 + cost_params['T'] * (w_k - self.build_params['w_max']) ** 2 + x, s, ea, xa, obs_val, obs_val_a = x_k, s_k, ea_k, xa_k, obs_val_k, obs_val_a_k + dlp = [] + # Loop over time steps + cs_f = lambda x,u: cs.vertcat(*self.robot.f(x,u)) + cs_fs = lambda s,w: w + for k in range(self.build_params['N']): + # Current control variables + u_k = u[k * self.build_params['nu']:(k + 1) * self.build_params['nu']] + ua_k = ua[k * self.build_params['nu']:(k + 1) * self.build_params['nu']] + w_k = w[k] + # Integrate one step + x_k = self.discrete_integration(cs_f, x_k, u_k) + s_k = self.discrete_integration(cs_fs, s_k, w_k) + xa_k = self.discrete_integration(cs_f, xa_k, ua_k) + lp_prev_k = lp_k + # Extract error and obstacle values + ea_k = self.error(xa_k, s_k, path_pol_sym) + obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, k+1) + obs_val_a_k = self.cs_obstacle_evaluation(obs_par_sym, xa_k, k+1) + + # lp_k = cost_params['K'] * ea_k ** 2 + cs.dot(cost_params['S'], ua_k * ua_k) + cost_params['T'] * (w_k - self.build_params['dp_max']) ** 2 + lp_k = cost_params['K'] * ea_k ** 2 + cost_params['T'] * (w_k - self.build_params['w_max']) ** 2 + dlp_k = lp_k - lp_prev_k + + # Store current state + x = cs.vertcat(x, x_k) + xa = cs.vertcat(xa, xa_k) + s = cs.vertcat(s, s_k) + ea = cs.vertcat(ea, ea_k) + obs_val = cs.vertcat(obs_val, obs_val_k) + obs_val_a = cs.vertcat(obs_val_a, obs_val_a_k) + dlp = cs.vertcat(dlp, dlp_k) + + # Define constraint vector + con = cs.vertcat(s, x) + if self.build_params['xaeN_con']: + con = cs.vertcat(con, x_k-xa_k) + if self.build_params['dlp_con']: + con = cs.vertcat(con, dlp) + # con = cs.vertcat(s, x, x_k-xa_k, dlp) + # con = cs.vertcat(s, x, x_k-xa_k) + + u_target = cs.SX([self.build_params['w_max'], 0]) + u_err = cs.repmat(u_target,self.build_params['N']) - ua + xae = x - xa + uae = u - ua + we = w - self.build_params['w_max'] + # Define costs + Q = cs.repmat(cost_params['Q'], self.build_params['N'] + 1) + R = cs.repmat(cost_params['R'], self.build_params['N']) + K = cs.repmat(cost_params['K'], self.build_params['N'] + 1) + S = cs.repmat(cost_params['S'], self.build_params['N']) + T = cs.repmat(cost_params['T'], self.build_params['N']) + # Note s_cost is normalized for easier tuning + la_cost = cs.dot(Q, xae * xae) + cs.dot(R, uae * uae) + lo_cost = cs.dot(K, ea * ea) + cs.dot(S, u_err * u_err) + lw_cost = cs.dot(T, we ** 2) + obs_cost = 0.5 * cost_params['mu'] * (cs.sum1(obs_val * obs_val) + cs.sum1(obs_val_a * obs_val_a)) + # Define constraints + self.cs_sol2state = cs.Function('cs_sol2state', [sol_sym, x0_sym, path_pol_sym], [x, s, xa], ['sol', 'x0', 'path_pol'], ['x', 's', 'xa']) + self.cs_sol2cost = cs.Function('cs_sol2cost', [sol_sym, x0_sym, path_pol_sym, obs_par_sym, cost_params_sym], [la_cost, lo_cost, lw_cost, obs_cost], ['sol', 'x0', 'path_pol', 'obs_par', 'cost_params'], ['la_cost', 'lo_cost', 'lw_cost', 'obs_cost']) + self.cs_sol2con = cs.Function('cs_sol2con', [sol_sym, x0_sym, path_pol_sym, cost_params_sym], [con], ['sol', 'x0', 'path_pol', 'cost_params'], ['con']) + self.cs_sol2obsval = cs.Function('cs_sol2obsval', [sol_sym, x0_sym, obs_par_sym], [obs_val], ['sol', 'x0', 'obs_par'], ['obs_val']) + + def sol_bounds(self): + xa0_min = np.tile(-np.inf, self.build_params['nx']) + u_min = np.tile(self.build_params['u_min'], self.build_params['N']) + ua_min = np.tile(-np.inf, self.build_params['nu'] * self.build_params['N']) + w_min = np.zeros(self.build_params['N']) + xa0_max = np.tile(np.inf, self.build_params['nx']) + u_max = np.tile(self.build_params['u_max'], self.build_params['N']) + ua_max = np.tile(np.inf, self.build_params['nu'] * self.build_params['N']) + w_max = np.tile(self.build_params['w_max'], self.build_params['N']) + return np.concatenate((xa0_min, u_min, ua_min, w_min)), np.concatenate((xa0_max, u_max, ua_max, w_max)) + + def con_bounds(self): + s_min = np.zeros(self.build_params['N'] + 1) + x_min = np.tile(self.build_params['x_min'], self.build_params['N'] + 1) + xaeN_min = np.tile(-0.1, self.build_params['nx']) + dlp_min = np.tile(-np.inf, self.build_params['N']) + s_max = np.tile(self.build_params['w_max'] * self.build_params['dt'] * self.build_params['N'], self.build_params['N'] + 1) + x_max = np.tile(self.build_params['x_max'], self.build_params['N'] + 1) + xaeN_max = np.tile(0.1, self.build_params['nx']) + dlp_max = np.tile(0, self.build_params['N']) + con_min, con_max = np.concatenate((s_min, x_min)), np.concatenate((s_max, x_max)) + if self.build_params['xaeN_con']: + con_min, con_max = np.concatenate((con_min, xaeN_min)), np.concatenate((con_max, xaeN_max)) + if self.build_params['dlp_con']: + con_min, con_max = np.concatenate((con_min, dlp_min)), np.concatenate((con_max, dlp_max)) + return con_min, con_max + + def sol2state(self, sol, x0, path_pol): + x, s, xa = self.cs_sol2state(sol, x0, path_pol) + return np.array(x).flatten(), np.array(s).flatten(), np.array(xa).flatten() + + def sol2con(self, sol, x0, path_pol, params): + cost_params = params['Q'] + params['R'] + [params['K']] + params['S'] + [params['T'], params['mu']] + return np.array(self.cs_sol2con(sol, x0, path_pol, cost_params)).flatten() + + def sol2obsval(self, sol, x0, obs_par): + return np.array(self.cs_sol2obsval(sol, x0, obs_par)).flatten() + + def sol2cost(self, sol, x0, path_pol, obs_par, params): + cost_params = params['Q'] + params['R'] + [params['K']] + params['S'] + [params['T'], params['mu']] + la_cost, lo_cost, lw_cost, obs_cost = self.cs_sol2cost(sol, x0, path_pol, obs_par, cost_params) + return {'la_cost': float(la_cost), 'lo_cost': float(lo_cost), 'lw_cost': float(lw_cost), 'obs_cost': float(obs_cost)} + + def sol2xa0uuaw(self, sol): + xa0_len, u_len, ua_len, w_len = self.build_params['nx'], self.build_params['nu'] * self.build_params['N'], \ + self.build_params['nu'] * self.build_params['N'], self.build_params['N'] + u_idx = xa0_len + ua_idx = u_idx + u_len + w_idx = ua_idx + ua_len + xa0 = sol[:u_idx] + u = sol[u_idx:ua_idx] + ua = sol[ua_idx:w_idx] + w = sol[w_idx:] + return xa0, u, ua, w + + def build(self): + # Build parametric optimizer + # ------------------------------------ + + n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict']) + n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2 + + params = {'x0': self.build_params['nx'], 'path_pol': self.build_params['np'] * (self.build_params['n_pol'] + 1), + 'Q': self.build_params['nx'], 'R': self.build_params['nu'], 'K': 1, + 'S': self.build_params['nu'], 'T': 1, 'mu': 1, + 'obs_par': n_ell_par + n_pol_par} + par_dim = sum(list(params.values())) + # Exchange parameter dimension with value + par = cs.SX.sym("par", par_dim) # Parameters + p_idx = 0 + for key, dim in params.items(): + params[key] = par[p_idx:p_idx + dim] + p_idx += dim + + # Define solution vector + sol = cs.SX.sym('sol', self.build_params['nx'] + (2 * self.build_params['nu'] + 1) * self.build_params['N']) + + # Cost + cost_params = cs.vertcat(params['Q'], params['R'], params['K'], params['S'], params['T'], params['mu']) + la_cost, lo_cost, lw_cost, obs_cost = self.cs_sol2cost(sol, params['x0'], params['path_pol'], params['obs_par'], cost_params) + cost = la_cost + lo_cost + lw_cost + obs_cost + + # Constraints + sol_min, sol_max = self.sol_bounds() + con_min, con_max = self.con_bounds() + sol_bounds = og.constraints.Rectangle(sol_min.tolist(), sol_max.tolist()) + agl_con = self.cs_sol2con(sol, params['x0'], params['path_pol'], cost_params) + agl_con_bounds = og.constraints.Rectangle(con_min.tolist(), con_max.tolist()) + # Setup builder + problem = og.builder.Problem(sol, par, cost) \ + .with_constraints(sol_bounds) \ + .with_aug_lagrangian_constraints(agl_con, agl_con_bounds) + build_config = og.config.BuildConfiguration() \ + .with_build_directory(self.build_dir) \ + .with_build_mode(self.build_params['mode']) \ + .with_build_python_bindings() + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(self.build_name) + solver_config = og.config.SolverConfiguration() \ + .with_tolerance(self.build_params['solver_tol']) \ + .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \ + .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \ + .with_max_outer_iterations(self.build_params['solver_max_outer_iterations']) + builder = og.builder.OpEnOptimizerBuilder(problem, + meta, + build_config, + solver_config) \ + .with_verbosity_level(1) + builder.build() + print('') + + # Save build params + with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file: + yaml.dump(self.build_params, file) + + + def run(self, x0, path_pol, params, obs_par, init_guess): + p = x0 + path_pol + params['Q'] + params['R'] + [params['K']] + params['S'] + [params['T'], params['mu']] + obs_par + if init_guess is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, 2*e_max, lam_rho): + # Use base solution as initial guess + init_guess = self.base_solution(x0, path_pol, 0.1) + + # Run solver + solution_data = self.solver.run(p=p, initial_guess=init_guess) + + return solution_data + + base_sol = [0] * (self.build_params['nx'] + (2 * self.build_params['nu'] + 1) * self.build_params['N']) + base_sol[:self.build_params['nx']] = x0 + base_sol[-self.build_params['N']] = self.build_params['dp_max'] + + + if self.sol_prev is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, params): + # Use base solution as initial guess + self.sol_prev = base_sol + + # Run solver + solution_data = self.solver.run(p=p, initial_guess=self.sol_prev) + + if solution_data is None: + sol, exit_status = base_sol, "TrivialSolution" + self.sol_prev = None + else: + sol, exit_status = solution_data.solution, solution_data.exit_status + # self.sol_prev = sol + x, s, xa = self.sol2state(sol, x0, path_pol) + + xa0, u, ua, w = self.sol2xa0uuaw(sol) + + self.sol_prev = xa[self.build_params['nx']:2*self.build_params['nx']].tolist() + \ + u[self.build_params['nu']:] + [0] * self.build_params['nu'] + \ + ua[self.build_params['nu']:] + [0] * self.build_params['nu'] + \ + w[1:] + [0] + + sol_feasible = self.is_feasible(sol, x0, path_pol, params, d=verbosity>0) + + return sol, sol_feasible, exit_status \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py new file mode 100644 index 0000000..04c4fb7 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py @@ -0,0 +1,5 @@ +from .workspace_modification import workspace_modification, rho_environment, extract_r0 +from .path_generator import path_generator +from .mpc import Mpc, pol2pos +# from .tunnel_mpc_controller import TunnelMpcController +from .motion_controller import MotionController, ControlMode \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py new file mode 100644 index 0000000..36f7fd8 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py @@ -0,0 +1,447 @@ +import matplotlib.pyplot as plt + +from motion_control.pfmpc_ds import workspace_modification, path_generator, Mpc, pol2pos, rho_environment, extract_r0 +from starworlds.utils.misc import tic, toc +from starworlds.obstacles import Ellipse, StarshapedPolygon, motion_model +import numpy as np +import shapely +import warnings +from enum import Enum + +class ControlMode(Enum): + SBC = 1 + MPC = 2 + + class InvalidModeError(Exception): + pass + + +class MotionController: + # reference_path: list of waypoints (i.e. list of lists) + def __init__(self, params, robot, reference_path, verbosity=0): + self.params = params + self.robot = robot + self.mpc = Mpc(params, robot) + self.rhrp_L = self.params['N'] * self.params['dt'] * self.mpc.build_params['w_max'] + self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps']) + self.set_reference_path(reference_path) + self.verbosity = verbosity + # print(self.params['cs'] * self.mpc.build_params['w_max'], self.params['ce'] * self.params['rho0']) + self.reset() + + def reset(self): + self.mpc.reset() + self.mode = ControlMode.SBC + self.obstacle_clusters = None + self.obstacles_rho = [] + self.workspace_rho = None + self.obstacles_star = [] + self.r_plus = None + self.theta = 0 + self.ds_generated_path = None + self.rg = None + self.pg_buffer = None + self.pg = None + self.rhrp_path = None + self.path_pol = None + self.rho = None + self.epsilon = None + self.solution = None + self.sol_feasible = None + self.mpc_exit_status = None + if self.params['lambda'] * self.params['rho0'] > self.mpc.build_params['w_max'] * self.params['dt']: + print("Poor lamda, rho0 selection. lambda*rho ({:.2f}) > w_max*dt ({:.2f})".format(self.params['lambda'] * self.params['rho0'], self.mpc.build_params['w_max'] * self.params['dt'])) + # else: + # print("Ok lamda, rho0 selection. {:.2f} <= {:.2f}".format(self.params['lambda'] * self.params['rho0'], + # self.mpc.build_params['dp_max'])) + self.u_prev = [0] * self.robot.nu + self.timing = {'workspace': 0, 'target': 0, 'mpc': 0, 'workspace_detailed': [0] * 4} + + def set_reference_path(self, reference_path): + self.reference_path = shapely.geometry.Point(reference_path[0]) if len(reference_path) == 1 else shapely.geometry.LineString(reference_path) + self.theta_g = self.reference_path.length + + # Extract path rs -> reference_path(theta, theta+L) + def nominal_rhrp(self, r0, free_rho_sh): + # Find receding global reference + # nominal_rhrp_0 = np.array(self.reference_path.interpolate(self.theta).coords[0]) + nominal_rhrp_sh = shapely.ops.substring(self.reference_path, start_dist=self.theta, end_dist=self.theta + self.params['nominal_rhrp_horizon']) + init_path = shapely.geometry.LineString([r0, nominal_rhrp_sh.coords[0]]) + if nominal_rhrp_sh.geom_type == 'Point': + nominal_rhrp_sh = init_path + else: + nominal_rhrp_sh = shapely.ops.linemerge([init_path, nominal_rhrp_sh]) + nominal_rhrp_sh = shapely.ops.substring(nominal_rhrp_sh, start_dist=0, end_dist=self.params['nominal_rhrp_horizon']) + path_collision_check = nominal_rhrp_sh if nominal_rhrp_sh.length > init_path.length else init_path + collision_free = path_collision_check.within(free_rho_sh) + return nominal_rhrp_sh, collision_free + + def ref_kappa(self, p, r0, r_lambda_rho, rho): + if np.linalg.norm(p-r_lambda_rho) < rho: + return r_lambda_rho + # Intersection point of B(r0,rho) and B(rlr,rho) + rc = 0.5 * (r0 + r_lambda_rho) + nc = np.array([rc[1] - r0[1], r0[0] - rc[0]]) + nc /= np.linalg.norm(nc) + d = np.sqrt(rho ** 2 - np.linalg.norm(rc - r0) ** 2) + i1 = rc + nc * d + i2 = rc - nc * d + # Line coefficients for l[r0, r_lambda_rho] + a = r0[1] - r_lambda_rho[1] + b = r_lambda_rho[0] - r0[0] + c = - (r0[1] * b + r0[0] * a) + # Line coefficients for li + i_scale = 0.01 + ai = i1[0] - p[0] + bi = i1[1] - p[1] + ci = 0.5 * (p.dot(p) - i1.dot(i1) + i_scale * (p.dot(i2) - p.dot(i1) - i1.dot(i2) + i1.dot(i1))) + # Return intersection of lines + return np.array([(b * ci - bi * c) / (a * bi - ai * b), (c * ai - ci * a) / (a * bi - ai * b)]) + + def unicycle_sbc(self, x, p_ref): + k1, k2 = 2, 2 + # k1, k2 = 1.25, 0.3 + e = np.array(x[:2]) - np.array(p_ref) + th = x[2] + + r_th = np.arctan2(e[1], e[0]) + np.pi + # E Theta in (-pi, pi] + e_th = r_th - th + e_th0 = e_th + while e_th > np.pi: + e_th -= 2 * np.pi + while e_th <= -np.pi: + e_th += 2 * np.pi + + u = [ + -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)), + k2 * (e_th) + ] + + e_norm = np.linalg.norm(e) + if e_norm < abs(u[0]) * self.params['dt']: + u[0] = np.sign(u[0]) * e_norm / self.params['dt'] + + if self.robot.u_min[0] == 0 and u[0] < 0: + u[0] = 0 + u0_sig = 1 + else: + u0_sig = u[0] / self.robot.u_max[0] if u[0] >= 0 else u[0] / self.robot.u_min[0] + u1_sig = u[1] / self.robot.u_max[1] if u[1] >= 0 else u[1] / self.robot.u_min[1] + + sig = max(u0_sig, u1_sig, 1) + if sig > 1: + if u0_sig > u1_sig: + u[0] = self.robot.u_max[0] if u[0] > 0 else self.robot.u_min[0] + u[1] /= sig + else: + u[0] /= sig + u[1] = self.robot.u_max[1] if u[1] > 0 else self.robot.u_min[1] + + return u + + # Control law (Assumes Unicycle robot) + ref_dist = np.linalg.norm(x[:2] - p_ref) + if ref_dist > 1e-5: + theta_ref = np.arctan2(p_ref[1] - x[1], p_ref[0] - x[0]) + theta_diff = float(theta_ref - x[-1]) + if theta_diff > np.pi: + theta_diff -= 2 * np.pi + if theta_diff < -np.pi: + theta_diff += 2 * np.pi + if abs(theta_diff) < 1e-2: # Less than 0.57 degree error + # Linear velocity + u[0] = min(self.robot.u_max[0], ref_dist / self.params['dt']) + else: + # Angular velocity + if theta_diff > 0: + u[1] = min(self.robot.u_max[1], theta_diff / self.params['dt']) + else: + u[1] = max(self.robot.u_min[1], theta_diff / self.params['dt']) + return u + + def r_plus_dist(self): + return np.linalg.norm(np.array(self.reference_path.interpolate(self.theta).coords[0]) - self.r_plus) + + def theta_plus(self, s): + return max(self.theta, min(self.theta + s - self.r_plus_dist(), self.theta_g)) + + def compute_u(self, x): + if self.mode == ControlMode.MPC: + u, w = self.mpc.sol2uds(self.solution) + return u[:self.robot.nu] + else: + # r_kappa = self.ref_kappa(self.robot.h(x), self.rhrp_path[0, :], + # pol2pos(self.path_pol, self.lam_rho, self.mpc.build_params['n_pol']), self.rho) + r_kappa = self.rhrp_path[0, :] + if self.robot.__class__.__name__ == 'Unicycle': + return self.unicycle_sbc(x, r_kappa) + elif self.robot.__class__.__name__ == 'Omnidirectional': + k = 100 + u = [k*(r_kappa[0]-x[0]), k*(r_kappa[1]-x[1])] + for i in range(2): + scale = 1 + if u[i] > self.robot.u_max[i]: + scale = self.robot.u_max[i] / u[i] + elif u[i] < self.robot.u_min[i]: + scale = self.robot.u_min[i] / u[i] + u = [scale * u[0], scale * u[1]] + # + # u0_sig = u[0] / self.robot.u_max[0] + # u1_sig = u[1] / self.robot.u_max[1] + # sig = max(u0_sig, u1_sig, 1) + # if sig > 1: + # if u0_sig > u1_sig: + # u[0] = self.robot.u_max[0] + # u[1] /= sig + # else: + # u[0] /= sig + # u[1] = self.robot.u_max[1] + return u + else: + print("No SBC for this robot model!!!!") + + def update_policy(self, x, obstacles, workspace=None): + if workspace is None: + workspace = Ellipse([1.e10, 1.e10]) + p = self.robot.h(x) + + obstacles_local = obstacles.copy() + # Adjust for robot radius + if self.robot.radius > 0: + obstacles_local, workspace, _, _ = rho_environment(workspace, obstacles_local, self.robot.radius) + + # Adjust for moving obstacles + if self.params['velocity_obstacle']: + for i, o in enumerate(obstacles_local): + if not (o._motion_model is None or o._motion_model.__class__.__name__ == "Static"): + o_pol = o.polygon() + xvel, yvel = o._motion_model.lin_vel() + dil = StarshapedPolygon(shapely.MultiPolygon([o_pol, shapely.affinity.translate(o_pol, xoff=xvel*self.params['dt'], yoff=yvel*self.params['dt'])]).convex_hull) + # dil = o.dilated_obstacle(padding=max(abs(o._motion_model.lin_vel())) * self.params['dt'], id="duplicate") + if dil.polygon().distance(shapely.geometry.Point(p)) > max(abs(o._motion_model.lin_vel()))*self.params['dt']: + obstacles_local[i] = dil + else: + print("Not dillated obstacle.") + # dil = o.dilated_obstacle(padding=max(abs(o._motion_model.lin_vel())) * self.params['dt'], id="duplicate") + # _, ax = o.draw() + # ax.set_xlim([-1, 12]) + # ax.set_ylim([-8, 4]) + # obstacles_local[i].draw(ax=ax, fc='r', alpha=0.4, zorder=-2) + # dil.draw(ax=ax, fc='b', alpha=0.4, zorder=-3) + # plt.show() + + if self.params['workspace_horizon'] > 0: + obstacle_detection_region = shapely.geometry.Point(p).buffer(self.params['workspace_horizon']) + obstacles_filtered = [] + for o in obstacles_local: + if obstacle_detection_region.intersects(o.polygon()): + obstacles_filtered += [o] + obstacles_local = obstacles_filtered + else: + obstacles_filtered = [] + for o in obstacles_local: + if workspace.polygon().intersects(o.polygon()): + obstacles_filtered += [o] + obstacles_local = obstacles_filtered + + # Initialize rs to robot position + if self.r_plus is None: + self.r_plus = p + + self.timing['workspace'] = 0 + self.timing['target'] = 0 + self.timing['mpc'] = 0 + self.timing['workspace_detailed'] = None + + pg_prev = self.pg + self.rg = None + self.pg = None + self.rho = self.params['rho0'] + ref_end = np.array(self.reference_path.coords[-1]) + close_to_convergence = (np.linalg.norm(self.r_plus - ref_end) < self.rho) and (self.theta >= self.theta_g) + if close_to_convergence: + self.mode = ControlMode.SBC + for i in range(len(self.rhrp_s)): + self.rhrp_path[i, :] = ref_end + else: + ds_path_generation = True + if self.theta_g > 0: + # Extract receding path from global target path + t0 = tic() + obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh = rho_environment(workspace, obstacles_local, self.rho) + self.timing['workspace'] = toc(t0) + + t0 = tic() + nominal_rhrp_sh, nominal_rhrp_collision_free = self.nominal_rhrp(self.r_plus, free_rho_sh) + if nominal_rhrp_collision_free: + ds_path_generation = False + if nominal_rhrp_sh.length > 0: + self.rhrp_path = np.array([nominal_rhrp_sh.interpolate(s).coords[0] for s in self.rhrp_s]) + else: + self.rhrp_path = np.tile(nominal_rhrp_sh.coords[0], (len(self.rhrp_s), 1)) + self.obstacles_rho = obstacles_rho + self.workspace_rho = workspace_rho + self.obstacles_star = [] + self.obstacle_clusters = None + self.pg = self.rhrp_path[-1, :] + rhrp_path_length = self.rhrp_L + self.timing['target'] = toc(t0) + + if ds_path_generation: + # Find attractor for DS dynamics + if self.theta_g == 0: + self.pg = np.array(self.reference_path.coords[0]) + else: + t0 = tic() + self.theta = self.theta_plus(self.params['nominal_rhrp_horizon']) + for theta in np.arange(self.theta_plus(self.params['nominal_rhrp_horizon']), self.theta_g, np.diff(self.rhrp_s[:2])): + # if self.reference_path.interpolate(theta).disjoint(obstacles_rho_sh): + if self.reference_path.interpolate(theta).within(free_rho_sh): + self.theta = theta + break + self.pg = np.array(self.reference_path.interpolate(self.theta).coords[0]) + print(self.theta_g, theta, self.reference_path.interpolate(theta).within(free_rho_sh)) + self.timing['workspace'] += toc(t0) + + pg_buffer_thresh = 0.5 + # new_pg_buffer = not self.ds_generated_path or np.linalg.norm(self.pg_buffer - self.pg) > pg_buffer_thresh + # if new_pg_buffer: + # self.pg_buffer = self.pg + + buffer_active = self.params['buffer'] and (self.ds_generated_path and np.linalg.norm(pg_prev - self.pg) < pg_buffer_thresh) + buffer_path = self.rhrp_path if buffer_active else None + if buffer_active: + self.pg_buffer = self.pg + + local_pars = self.params.copy() + local_pars['max_rhrp_compute_time'] -= self.timing['target'] + + self.rhrp_path, rhrp_path_length, self.obstacle_clusters, self.obstacles_rho, self.workspace_rho, \ + self.obstacles_star, self.rho, self.rg, workspace_exitflag, workspace_timing, target_timing, self.timing['workspace_detailed'] = \ + ds_path_gen(obstacles_local, workspace, p, self.pg, self.r_plus, self.params['rho0'], self.params['hull_epsilon'], self.rhrp_s, + self.obstacle_clusters, buffer_path, local_pars, self.verbosity) + + # Update O+ + t0 = tic() + obstacles_rho_sh = shapely.ops.unary_union([o.polygon() for o in self.obstacles_rho]) + C_rho0 = self.workspace_rho.polygon().difference(obstacles_rho_sh).buffer(self.params['rho0']) + env_DSW = workspace_exitflag != 0 + if not (env_DSW and (shapely.geometry.Point(p).within(C_rho0) or self.rho == self.params['rho0'])): + self.obstacle_clusters = None + self.timing['workspace'] += toc(t0) + + self.timing['workspace'] += workspace_timing + self.timing['target'] += target_timing + + self.ds_generated_path = ds_path_generation + + t0 = tic() + # Approximate target with polynomials + self.path_pol = np.polyfit(self.rhrp_s, self.rhrp_path[:, 0], self.params['n_pol']).tolist() + \ + np.polyfit(self.rhrp_s, self.rhrp_path[:, 1], self.params['n_pol']).tolist() + # Force init position to be correct + self.path_pol[self.params['n_pol']] = self.rhrp_path[0, 0] + self.path_pol[-1] = self.rhrp_path[0, 1] + # Compute polyfit approximation error + self.epsilon = max(np.linalg.norm(self.rhrp_path - np.array([pol2pos(self.path_pol, s, self.mpc.build_params['n_pol']) for s in self.rhrp_s]), axis=1)) + self.timing['target'] += toc(t0) + + t0 = tic() + self.lam_rho = self.params['lambda'] * self.rho # Parameter for tracking error constraint + + # Compute MPC solution + e_max = self.rho - self.epsilon + solution_data = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, e_max, self.lam_rho, self.solution, verbosity=self.verbosity) + if solution_data is None: + self.solution, self.mpc_exit_status, self.sol_feasible = None, "BaseSolution", False + else: + self.solution, self.mpc_exit_status = solution_data.solution, solution_data.exit_status + self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, e_max, self.lam_rho, d=self.verbosity > 0) + # self.solution, self.sol_feasible, self.mpc_exit_status = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, + # e_max, self.lam_rho, verbosity=self.verbosity) + self.timing['mpc'] = toc(t0) + + if (np.linalg.norm(self.rhrp_path[-1, :]-self.pg) <= 0.1 * self.rhrp_L or rhrp_path_length > 0.1 * self.rhrp_L) and self.sol_feasible: + self.mode = ControlMode.MPC + else: + if self.verbosity > 0: + print("[Motion Controller]: MPC solution not feasible. Using default control law.") + self.mode = ControlMode.SBC + + if self.mode == ControlMode.SBC: + # r_kappa = self.ref_kappa(p, self.rhrp_path[0, :], pol2pos(self.path_pol, self.lam_rho, self.mpc.build_params['n_pol']), self.rho) + # r_kappa = self.rhrp_path[0, :] + # u = self.unicycle_sbc(x, r_kappa) + # if self.verbosity > 0: + # print("[Motion Controller]: MPC solution not feasible. Using default control law. u = " + str(u)) + self.r_plus = self.rhrp_path[0, :] + else: + u, w = self.mpc.sol2uds(self.solution) + # u = u[:self.robot.nu] + # Update rs and theta + + # if np.linalg.norm(self.rhrp_path[0, :] - np.array(self.reference_path.interpolate(self.theta).coords[0])) < self.rho: + if self.theta_g > 0 and self.r_plus_dist() < self.rho: + self.theta = min(self.theta + w[0] * self.params['dt'], self.theta_g) + + self.r_plus = np.array(pol2pos(self.path_pol, w[0] * self.params['dt'], self.mpc.build_params['n_pol'])) + # if update_theta: + # if self.theta < self.theta_g and np.linalg.norm(self.rhrp_path[0, :] - np.array(self.reference_path.interpolate(self.theta).coords[0])) < self.rho: + # self.theta += w[0] * self.params['dt'] + # print(np.linalg.norm(self.r_plus-self.rhrp_path[0, :]) ) + + + # return self.sppc(x, pol2pos(self.path_pol, self.lam_rho, self.mpc.build_params['n_pol'])) + # return self.sppc(x, self.reference_path.coords[-1]) + + # self.sol_feasible = False + + self.u_prev = self.compute_u(x) + # return np.array(u) + + +def ds_path_gen(obstacles, workspace, p, pg, r_plus, rho0, hull_epsilon, rhrp_s, + previous_obstacle_clusters=None, buffer_path=None, params=None, verbosity=0): + # Update obstacles + obstacle_clusters, r0, rg, rho, obstacles_rho, workspace_rho, workspace_timing, workspace_timing_detailed, workspace_exitflag = \ + workspace_modification(obstacles, workspace, p, pg, r_plus, rho0, hull_epsilon, previous_obstacle_clusters, params, verbosity) + obstacles_star = [o.cluster_obstacle for o in obstacle_clusters] + # if exit_flag == 0: + # obstacle_clusters = None + + # Make sure all polygon representations are computed + [o._compute_polygon_representation() for o in obstacles_star] + + # Buffer target path + init_path, init_s = None, None + if buffer_path is not None: + # Shift path to start closest to current r_plus + init_path = buffer_path[np.argmin(np.linalg.norm(r0 - buffer_path, axis=1)):, :] + if np.linalg.norm(r0 - init_path[0, :]) > 1e-6: + init_path = np.vstack((r0, init_path)) + + # ||self.r_plus - p|| < rho from construction + for r in init_path: + # NOTE: Assumes previous path not outside workspace due to static workspace + if not all([o.exterior_point(r) for o in obstacles_star]) or not workspace_rho.interior_point(r): + # if not all([o.exterior_point(r) for o in obstacles_star]): + if verbosity > 1: + print("[Path Generator]: No reuse of previous path. Path not collision-free.") + init_path = None + break + + if init_path is not None: + # Cut off stand still padding in init path + init_path_stepsize = np.linalg.norm(np.diff(init_path, axis=0), axis=1) + init_s = np.hstack((0, np.cumsum(init_path_stepsize))) + init_path_mask = [True] + (init_path_stepsize > 1e-8).tolist() + init_path = init_path[init_path_mask, :] + init_s = init_s[init_path_mask] + + + # Update target path + rhrp_path, rhrp_path_length, path_timing = \ + path_generator(r0, rg, obstacles_star, workspace_rho, rhrp_s, init_path, init_s, params, verbosity) + + return rhrp_path, rhrp_path_length, obstacle_clusters, obstacles_rho, workspace_rho, obstacles_star, rho, rg, workspace_exitflag, workspace_timing, path_timing, workspace_timing_detailed \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py new file mode 100644 index 0000000..3503887 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py @@ -0,0 +1,364 @@ +import casadi.casadi as cs +import opengen as og +import numpy as np +import os, sys +import yaml + + +class NoSolutionError(Exception): + '''raise this when there's no solution to the mpc problem''' + pass + +def pol_eval(pol_coeff, x, n_pol): + return sum([pol_coeff[i] * x ** (n_pol - i) for i in range(n_pol+1)]) + + +def pol2pos(pol_coeff, s, n_pol): + return [pol_eval(pol_coeff[:n_pol+1], s, n_pol), pol_eval(pol_coeff[n_pol+1:], s, n_pol)] + + +class Mpc: + + def __init__(self, params, robot): + self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build') + # Load parameters + self.build_params = None + self.robot = robot + self.set_build_params(params) + rebuild, self.build_name = self.get_build_version() + self.generate_sol_fun() + # Build if different from latest build + if rebuild: + self.build() + else: + print("Found MPC build: {}".format(self.build_name)) + + sys.path.insert(1, os.path.join(self.build_dir, self.build_name)) + optimizer = __import__(self.build_name) + self.solver = optimizer.solver() + self.reset() + + def reset(self): + self.sol_prev = None + + def set_build_params(self, params): + self.build_params = { + 'mode': params['build_mode'], + 'N': params['N'], + 'dt': params['dt'], + 'solver_tol': params['solver_tol'], + 'solver_max_time': params['solver_max_time'], + 'solver_max_inner_iterations': params['solver_max_inner_iterations'], + 'solver_max_outer_iterations': params['solver_max_outer_iterations'], + 'x_min': self.robot.x_min, + 'x_max': self.robot.x_max, + 'u_min': self.robot.u_min, + 'u_max': self.robot.u_max, + 'nx': self.robot.nx, + 'nu': self.robot.nu, + 'np': 2, + 'robot_model': self.robot.__class__.__name__, + 'n_pol': params['n_pol'], + 'integration_method': params['integration_method'], + 'w_max': self.robot.vmax + } + + def get_build_version(self): + builds = [name for name in os.listdir(self.build_dir) + if os.path.isdir(os.path.join(self.build_dir, name))] + for build_ver in builds: + if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')): + continue + with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file: + build_ver_params = yaml.load(file, Loader=yaml.FullLoader) + if self.build_params == build_ver_params: + return False, build_ver + ver = 0 + while 'ver' + str(ver) in os.listdir(self.build_dir): + ver += 1 + return True, 'ver' + str(ver) + + def is_feasible(self, sol, x0, path_pol, e_max, lam_rho, d=False): + u, w = self.sol2uds(sol) + x, s, e = self.sol2state(sol, x0, path_pol) + u_min, w_min, s1_lamrho_min, sN_min, x_min, e_max_min = self.min_bounds() + u_max, w_max, s1_lamrho_max, sN_max, x_max, e_max_max = self.max_bounds() + + eps = 1e-3 + + sol_ok = True + if not all((u_min <= u) & (u <= u_max)): + sol_ok = False + if d: + print("[MPC]: Bad u") + print(u) + if not all((w_min <= w) & (w <= w_max)): + sol_ok = False + if d: + print("[MPC]: Bad w") + print(w) + if not all((x_min <= x) & (x <= x_max)): + sol_ok = False + if d: + print("[MPC]: Bad x") + print(x) + if s1_lamrho_min - eps > s[1] - lam_rho: + sol_ok = False + if d: + print("[MPC]: Bad s1 {:.4f} < {:.4f}".format(s[1], lam_rho)) + if not ((sN_min <= s[-1]) and (s[-1] <= sN_max + 0.1)): + sol_ok = False + if d: + print("[MPC]: Bad sN {:.4f} > {:.4f}".format(s[-1], sN_max)) + if not all((e_max_min <= e - e_max) & (e - e_max <= e_max_max)): + sol_ok = False + if d: + print("[MPC]: Bad e (e_max: {:.4f})".format(e_max)) + print(e) + return sol_ok + + def error(self, x, s, path_pol): + p_ref = cs.vertcat(*pol2pos(path_pol, s, self.build_params['n_pol'])) + return cs.norm_2(p_ref - self.robot.h(x)) + + def base_solution(self, x0, path_pol, lam_rho): + u = [0] * (self.build_params['N'] * self.build_params['nu']) + w = [0] * (self.build_params['N']) + w[0] = lam_rho / self.build_params['dt'] + + if self.build_params['robot_model'] == 'Unicycle': + p_ref = pol2pos(path_pol, lam_rho, self.build_params['n_pol']) + k1, k2 = 2, 2 + # k1, k2 = 1.25, 0.3 + + x = x0.copy() + for i in range(self.build_params['N']): + e = np.array(x[:2]) - np.array(p_ref) + th = x[2] + + r_th = np.arctan2(e[1], e[0]) + np.pi + # E Theta in (-pi, pi] + e_th = r_th - th + e_th0 = e_th + while e_th > np.pi: + e_th -= 2 * np.pi + while e_th <= -np.pi: + e_th += 2 * np.pi + + ui = [ + -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)), + k2 * (e_th) + ] + + if self.robot.u_min[0] == 0 and ui[0] < 0: + ui[0] = 0 + u0_sig = 1 + else: + u0_sig = ui[0] / self.robot.u_max[0] if ui[0] >= 0 else ui[0] / self.robot.u_min[0] + u1_sig = ui[1] / self.robot.u_max[1] if ui[1] >= 0 else ui[1] / self.robot.u_min[1] + + sig = max(u0_sig, u1_sig, 1) + if sig > 1: + if u0_sig > u1_sig: + ui[0] = self.robot.u_max[0] if ui[0] > 0 else self.robot.u_min[0] + ui[1] /= sig + else: + ui[0] /= sig + ui[1] = self.robot.u_max[1] if ui[1] > 0 else self.robot.u_min[1] + u[i * self.build_params['nu']:(i+1) * self.build_params['nu']] = ui + + x, _ = self.robot.move(x, ui, self.build_params['dt']) + + return u + w + + def discrete_integration(self, f, x, u): + dt = self.build_params['dt'] + if self.build_params['integration_method'] == 'euler': + return x + f(x, u) * dt + if self.build_params['integration_method'] == 'RK4': + k1 = f(x, u) + k2 = f(x + dt / 2 * k1, u) + k3 = f(x + dt / 2 * k2, u) + k4 = f(x + dt * k3, u) + return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) + raise NotImplemented + + def generate_sol_fun(self): + # Initialize + sol_sym = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N']) + x0_sym = cs.SX.sym('x0', self.build_params['nx']) + path_pol_sym = cs.SX.sym('path_pol', self.build_params['np'] * (self.build_params['n_pol'] + 1)) + cost_params = {'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], + 'u_prev': self.build_params['nu']} + cost_params_sym = cs.SX.sym("cost_params", sum(list(cost_params.values()))) + # Exchange parameter dimension with SX variable + p_idx = 0 + for key, dim in cost_params.items(): + cost_params[key] = cost_params_sym[p_idx:p_idx + dim] + p_idx += dim + # Initialize + u, w = self.sol2uds(sol_sym) + x_k, s_k = x0_sym, 0 + e_k = self.error(x_k, s_k, path_pol_sym) + x, s, e = x_k, s_k, e_k + # Loop over time steps + for k in range(self.build_params['N']): + # Current control variables + u_k = u[k * self.build_params['nu']:(k + 1) * self.build_params['nu']] + # Integrate one step + x_k = self.discrete_integration(lambda x, u: cs.vertcat(*self.robot.f(x,u)), x_k, u_k) + s_k = self.discrete_integration(lambda s, w: w, s_k, w[k]) + e_k = self.error(x_k, s_k, path_pol_sym) + # Store current state + x = cs.vertcat(x, x_k) + s = cs.vertcat(s, s_k) + e = cs.vertcat(e, e_k) + # Define costs + u_target = cs.SX([self.build_params['w_max'], 0]) + u_err = cs.repmat(u_target,self.build_params['N']) - u + du = u - cs.vertcat(cost_params['u_prev'], u[:-self.build_params['nu']]) + R = cs.repmat(cost_params['R'], self.build_params['N']) + DR = cs.repmat(cost_params['DR'], self.build_params['N']) + w_cost = cost_params['cs'] * cs.sum1(self.build_params['w_max'] - w) + e_cost = cost_params['ce'] * cs.sum1(e) + # u_cost = cs.dot(R, u * u) + u_cost = cs.dot(R, u_err * u_err) + du_cost = cs.dot(DR, du * du) + # Define constraints + self.cs_sol2state = cs.Function('cs_sol2state', [sol_sym, x0_sym, path_pol_sym], [x, s, e], ['sol', 'x0', 'path_pol'], ['x', 's', 'e']) + self.cs_sol2cost = cs.Function('cs_sol2cost', [sol_sym, x0_sym, path_pol_sym, cost_params_sym], [w_cost, e_cost, u_cost, du_cost], ['sol', 'x0', 'path_pol', 'cost_params'], ['s_cost', 'e_cost', 'u_cost', 'du_cost']) + + def min_bounds(self): + u_min = np.tile(self.build_params['u_min'], self.build_params['N']) + w_min = np.zeros(self.build_params['N']) + s1_lamrho_min = 0 + sN_min = 0 + x_min = np.tile(self.build_params['x_min'], self.build_params['N'] + 1) + e_max_min = np.tile(-np.inf, self.build_params['N'] + 1) + return u_min, w_min, s1_lamrho_min, sN_min, x_min, e_max_min + + def max_bounds(self): + u_max = np.tile(self.build_params['u_max'], self.build_params['N']) + # ds_max = np.tile(np.inf, self.build_params['N']) + w_max = np.tile(self.build_params['w_max'], self.build_params['N']) + s1_lamrho_max = np.inf + sN_max = self.build_params['w_max'] * self.build_params['N'] * self.build_params['dt'] + x_max = np.tile(self.build_params['x_max'], self.build_params['N'] + 1) + e_max_max = np.zeros(self.build_params['N'] + 1) + return u_max, w_max, s1_lamrho_max, sN_max, x_max, e_max_max + + def sol2state(self, sol, x0, path_pol): + x, s, e = self.cs_sol2state(sol, x0, path_pol) + # cs.MX to [float] + return np.array(x).flatten(), np.array(s).flatten(), np.array(e).flatten() + + def sol2cost(self, sol, x0, path_pol, params, u_prev): + cost_params = [params['cs'], params['ce'], params['R'][0], 0, params['DR'][0], 0] + u_prev + cost_params[2:6] = [params['R'][0], 0, params['DR'][0], 0] + s_cost, e_cost, u1_cost, ud1_cost = self.cs_sol2cost(sol, x0, path_pol, cost_params) + cost_params[2:6] = [0, params['R'][1], 0, params['DR'][1]] + _, _, u2_cost, ud2_cost = self.cs_sol2cost(sol, x0, path_pol, cost_params) + return {'s': float(s_cost), 'e': float(e_cost), 'u': float(u1_cost + u2_cost), 'ud': float(ud1_cost + ud2_cost), + 'u1': float(u1_cost), 'ud1': float(ud1_cost), 'u2': float(u2_cost), 'ud2': float(ud2_cost)} + + def sol2uds(self, sol): + u = sol[:self.build_params['nu'] * self.build_params['N']] + ds = sol[self.build_params['nu'] * self.build_params['N']:] + return u, ds + + def build(self): + # Build parametric optimizer + # ------------------------------------ + params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'], + 'path_pol': self.build_params['np'] * (self.build_params['n_pol'] + 1), + 'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], 'e_max': 1, + 'lam_rho': 1} + par_dim = sum(list(params.values())) + + # Exchange parameter dimension with value + par = cs.SX.sym("par", par_dim) # Parameters + p_idx = 0 + for key, dim in params.items(): + params[key] = par[p_idx:p_idx + dim] + p_idx += dim + + # Initialize + sol = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N']) + + # Cost + cost_params = cs.vertcat(params['cs'], params['ce'], params['R'], params['DR'], params['u_prev']) + s_cost, e_cost, u_cost, du_cost = self.cs_sol2cost(sol, params['x0'], params['path_pol'], cost_params) + cost = s_cost + e_cost + u_cost + du_cost + + # Constraints + x, s, e = self.cs_sol2state(sol, params['x0'], params['path_pol']) + u_min, w_min, s1_lamrho_min, sN_min, x_min, e_max_min = self.min_bounds() + u_max, w_max, s1_lamrho_max, sN_max, x_max, e_max_max = self.max_bounds() + sol_bounds = og.constraints.Rectangle(u_min.tolist() + w_min.tolist(), u_max.tolist() + w_max.tolist()) + agl_con = cs.vertcat(s[1] - params['lam_rho'], + s, + x, + e - params['e_max']) + agl_con_bounds = og.constraints.Rectangle([s1_lamrho_min] + [sN_min] * (self.build_params['N'] + 1) + + x_min.tolist() + e_max_min.tolist(), + [s1_lamrho_max] + [sN_max] * (self.build_params['N'] + 1) + + x_max.tolist() + e_max_max.tolist()) + + # Setup builder + problem = og.builder.Problem(sol, par, cost) \ + .with_constraints(sol_bounds) \ + .with_aug_lagrangian_constraints(agl_con, agl_con_bounds) + build_config = og.config.BuildConfiguration() \ + .with_build_directory(self.build_dir) \ + .with_build_mode(self.build_params['mode']) \ + .with_build_python_bindings() + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(self.build_name) + solver_config = og.config.SolverConfiguration() \ + .with_tolerance(self.build_params['solver_tol']) \ + .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \ + .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \ + .with_max_outer_iterations(self.build_params['solver_max_outer_iterations']) + builder = og.builder.OpEnOptimizerBuilder(problem, + meta, + build_config, + solver_config) \ + .with_verbosity_level(1) + builder.build() + print('') + + # Save build params + with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file: + yaml.dump(self.build_params, file) + + def shift_sol(self, sol): + return sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \ + sol[self.build_params['nu'] * (self.build_params['N'] - 1) : self.build_params['nu'] * self.build_params['N']] \ + + sol[self.build_params['nu'] * self.build_params['N']:] + + def run(self, x0, u_prev, path_pol, params, e_max, lam_rho, init_guess=None, verbosity=0): + + p = x0 + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + params['DR'] + \ + [0.9*e_max, 1.1*lam_rho] + + + if init_guess is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, 2*e_max, lam_rho): + # Use base solution as initial guess + init_guess = self.base_solution(x0, path_pol, lam_rho) + # Run solver + return self.solver.run(p=p, initial_guess=init_guess) + solution_data = self.solver.run(p=p, initial_guess=init_guess) + + if solution_data is None or not self.is_feasible(solution_data.solution, x0, path_pol, e_max, lam_rho, d=verbosity>0): + sol, exit_status = self.base_solution(x0, path_pol, lam_rho), "BaseSolution" + else: + sol = solution_data.solution + exit_status = solution_data.exit_status + + # self.sol_prev = sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \ + # sol[self.build_params['nu'] * (self.build_params['N'] - 1):self.build_params['nu'] * + # self.build_params['N']] \ + # + sol[self.build_params['nu'] * self.build_params['N']:] + sol_feasible = self.is_feasible(sol, x0, path_pol, e_max, lam_rho, d=verbosity > 0) + + return sol, sol_feasible, exit_status diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py new file mode 100644 index 0000000..d019f27 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py @@ -0,0 +1,110 @@ +import numpy as np +from starworlds.utils.misc import tic, toc +from motion_control.soads.soads import f as soads_f + + +def path_generator(r0, rg, obstacles, workspace, path_s, init_path=None, init_s=None, params=None, verbosity=0): + par = {'ds_decay_rate': 0.5, 'ds_increase_rate': 2., 'max_nr_steps': 1000, 'convergence_tolerance': 1e-5, + 'reactivity': 1., 'crep': 1., 'tail_effect': False, 'max_rhrp_compute_time': np.inf} + for k in par.keys(): + if k in params: + par[k] = params[k] + + # Assumes equidistant path_s + path_ds = path_s[1] - path_s[0] + + t0 = tic() + + # Initialize + ds = path_ds + s = np.zeros(par['max_nr_steps']) + r = np.zeros((par['max_nr_steps'], r0.size)) + if init_path is not None: + i = init_path.shape[0] + r[:i, :] = init_path + s[:i] = init_s + else: + i = 1 + r[0, :] = r0 + + + L = path_s[-1] + + while True: + dist_to_goal = np.linalg.norm(r[i - 1, :] - rg) + # Check exit conditions + if dist_to_goal < par['convergence_tolerance']: + if verbosity > 2: + print("[Path Generator]: Path converged. " + str( + int(100 * (s[i - 1] / L))) + "% of path completed.") + break + if s[i - 1] >= 0.5 * L: + if verbosity > 2: + print("[Path Generator]: Completed path length. " + str( + int(100 * (s[i - 1] / L))) + "% of path completed.") + break + if toc(t0) > par['max_rhrp_compute_time']: + if verbosity > 0: + print("[Path Generator]: Max compute time in path integrator. " + str( + int(100 * (s[i - 1] / L))) + "% of path completed.") + break + if i >= par['max_nr_steps']: + if verbosity > 0: + print("[Path Generator]: Max steps taken in path integrator. " + str( + int(100 * (s[i - 1] / L))) + "% of path completed.") + break + + # Movement using SOADS dynamics + dr = min(1, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, workspace=workspace, adapt_obstacle_velocity=False, + unit_magnitude=True, crep=par['crep'], + reactivity=par['reactivity'], tail_effect=par['tail_effect'], + convergence_tolerance=par['convergence_tolerance']) + r[i, :] = r[i - 1, :] + dr * ds + ri_collision = False + + def collision_free(p): + return all([o.exterior_point(p) for o in obstacles]) and workspace.interior_point(p) + + # Adjust for collisions that may arise due to crep!=1 + if not collision_free(r[i, :]): + dr = min(1, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, workspace=workspace, + adapt_obstacle_velocity=False, + unit_magnitude=True, crep=1, + reactivity=1, convergence_tolerance=par['convergence_tolerance']) + r[i, :] = r[i - 1, :] + dr * ds + + # while any([o.interior_point(r[i, :]) for o in obstacles]): + while not collision_free(r[i, :]): + if verbosity > 2: + print("[Path Generator]: Path inside obstacle. Reducing integration step from {:5f} to {:5f}.".format(ds, ds*par['ds_decay_rate'])) + ds *= par['ds_decay_rate'] + r[i, :] = r[i - 1, :] + dr * ds + # if ds < 0.01: + # import matplotlib.pyplot as plt + # plt.gca().quiver(*r[i-1, :], *dr, color='g') + # for o in obstacles: + # plt.gca().quiver(*o.boundary_mapping(r[i-1, :]), *o.normal(r[i-1, :])) + # plt.show() + + # Additional compute time check + if toc(t0) > par['max_rhrp_compute_time']: + ri_collision = True + break + if ri_collision: + continue + + # Update travelled distance + s[i] = s[i - 1] + ds + # Try to increase step rate again + ds = min(par['ds_increase_rate'] * ds, path_ds) + # Increase iteration counter + i += 1 + + r = r[:i, :] + s = s[:i] + + # Evenly spaced path + path = np.vstack((np.interp(path_s, s, r[:, 0]), np.interp(path_s, s, r[:, 1]))).T + + compute_time = toc(t0) + return path, s[-1], compute_time diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py new file mode 100644 index 0000000..24c8a4b --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py @@ -0,0 +1,139 @@ +import numpy as np +from starworlds.starshaped_hull import cluster_and_starify, ObstacleCluster +from starworlds.utils.misc import tic, toc +import shapely + +def rho_environment(workspace, obstacles, rho): + obstacles_rho = [o.dilated_obstacle(padding=rho, id="duplicate") for o in obstacles] + workspace_rho = workspace.dilated_obstacle(padding=-rho, id="duplicate") + obstacles_rho_sh = shapely.ops.unary_union([o.polygon() for o in obstacles_rho]) + free_rho_sh = workspace_rho.polygon().difference(obstacles_rho_sh) + return obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh + + +def extract_r0(r_plus, p, rho, free_rho_sh, workspace_rho, obstacles_rho): + initial_reference_set = shapely.geometry.Point(p).buffer(rho).intersection(free_rho_sh.buffer(-1e-2)) + if initial_reference_set.is_empty: + if rho < 1e-5: + print(rho, free_rho_sh.exterior.distance(shapely.geometry.Point(p))) + import matplotlib.pyplot as plt + from starworlds.utils.misc import draw_shapely_polygon + _, ax = plt.subplots() + draw_shapely_polygon(free_rho_sh.buffer(-1e-2), ax=ax, fc='g') + ax.plot(*r_plus, 'rs') + ax.plot(*p, 'ks') + ax.plot(*shapely.geometry.Point(p).buffer(rho).exterior.xy, 'b--') + for o in obstacles_rho: + o.draw(ax=ax, fc='r', alpha=0.3) + # ax.plot(*o.polygon().exterior.xy, 'k--') + # for o in obstacles: + # o.draw(ax=ax, fc='lightgrey') + plt.show() + + return None + r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(r_plus)) + r0 = np.array(r0_sh.coords[0]) + if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]): + # initial_reference_set = initial_reference_set.buffer(-rho_buffer) + r0_sh = initial_reference_set.centroid + r0 = np.array(r0_sh.coords[0]) + if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]): + return None + return r0 + +def workspace_modification(obstacles, workspace, p, pg, r_plus, rho0, hull_epsilon, previous_obstacle_clusters=None, params=None, verbosity=0): + par = {'max_obs_compute_time': np.inf, 'gamma': 0.5, 'make_convex': 1, 'iterative_rho_reduction': 1, + 'use_previous_workspace': 1} + for k in par.keys(): + if k in params: + par[k] = params[k] + + p_sh = shapely.geometry.Point(p) + # Clearance variable initialization + t_init = tic() + rho = rho0 + + # Pad obstacles with rho + obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh = rho_environment(workspace, obstacles, rho) + r0 = extract_r0(r_plus, p, rho, free_rho_sh, workspace_rho, obstacles_rho) + while r0 is None: + if par['iterative_rho_reduction'] or rho < rho0: + rho *= par['gamma'] + else: + print(min([o.polygon().distance(p_sh) for o in obstacles] + [workspace_rho.polygon().exterior.distance(p_sh)])) + obstacles_dist = min([o.polygon().distance(p_sh) for o in obstacles] + [workspace_rho.polygon().exterior.distance(p_sh)]) + rho = 0.8 * obstacles_dist + obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh = rho_environment(workspace, obstacles, rho) + r0 = extract_r0(r_plus, p, rho, free_rho_sh, workspace_rho, obstacles_rho) + if verbosity > 1: + print("[Workspace modification]: Reducing rho to " + str(rho)) + + + + # r0 = extract_r0(initial_reference_set, r_plus, workspace_rho, obstacles_rho) + + # initial_reference_set = initial_reference_set.buffer(-0.2 * initial_reference_set.minimum_clearance) + + # Initial reference position selection + # r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(r_plus)) + # r0 = np.array(r0_sh.coords[0]) + # if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]): + # # initial_reference_set = initial_reference_set.buffer(-rho_buffer) + # r0_sh = initial_reference_set.centroid + # r0 = np.array(r0_sh.coords[0]) + # + # if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]): + # if verbosity > 1: + # print("[Workspace Modification]: r0 not valid. Reducing rho to " + str(rho * par['gamma'])) + + + + # Goal reference position selection + rg_sh = shapely.geometry.Point(pg) + rg = pg + rho_buffer = 0 + while not all([o.exterior_point(rg) for o in obstacles_rho] + [workspace_rho.interior_point(rg)]): + rg_sh, _ = shapely.ops.nearest_points(workspace_rho.polygon().buffer(-rho_buffer).difference(obstacles_rho_sh.buffer(rho_buffer)), shapely.geometry.Point(pg)) + # while not all([o.exterior_point(rg) for o in obstacles_rho]): + # rg_sh, _ = shapely.ops.nearest_points(shapely.geometry.box(-1e6,-1e6,1e6,1e6).difference(obstacles_rho_sh.buffer(rho_buffer)), shapely.geometry.Point(pg)) + rg = np.array(rg_sh.coords[0]) + rho_buffer += 1e-2 + + # TODO: Check more thoroughly + if par['use_previous_workspace'] and previous_obstacle_clusters is not None: + obstacles_star_sh = shapely.ops.unary_union([o.polygon() for o in previous_obstacle_clusters]).buffer(1e-4) + if obstacles_star_sh.disjoint(shapely.geometry.Point(r0)) and obstacles_star_sh.disjoint(rg_sh) and obstacles_rho_sh.within(obstacles_star_sh): + if verbosity > 2: + print("[Workspace modification]: Reuse workspace from previous time step.") + obstacle_clusters = previous_obstacle_clusters + exit_flag = 10 + compute_time = toc(t_init) + obstacle_timing = None + return obstacle_clusters, r0, rg, rho, obstacles_rho, workspace_rho, compute_time, obstacle_timing, exit_flag + else: + if verbosity > 2: + print("[Workspace modification]: No reuse workspace from previous time step.") + print(obstacles_star_sh.disjoint(shapely.geometry.Point(r0)), obstacles_star_sh.disjoint(rg_sh), obstacles_rho_sh.within(obstacles_star_sh)) + else: + if par['use_previous_workspace'] and verbosity > 2: + print("[Workspace modification]: No reuse workspace from previous time step.") + # import matplotlib.pyplot as plt + # from starworlds.utils.misc import draw_shapely_polygon + # _,ax = plt.subplots() + # ax.plot([obstacles_rho_sh.bounds[0], obstacles_rho_sh.bounds[2]], + # [obstacles_rho_sh.bounds[1], obstacles_rho_sh.bounds[3]], alpha=0) + # draw_shapely_polygon(obstacles_star_sh, ax=ax, fc='g', alpha=0.7) + # draw_shapely_polygon(obstacles_rho_sh.difference(obstacles_star_sh), ax=ax, fc='r', alpha=1, ec='k', linestyle='--') + # plt.show() + + + # Apply cluster and starify + obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles_rho, r0, rg, hull_epsilon, + workspace=workspace_rho, + max_compute_time=par['max_obs_compute_time']-toc(t_init), + previous_clusters=previous_obstacle_clusters, + make_convex=par['make_convex'], verbose=verbosity, + timing_verbose=0) + + compute_time = toc(t_init) + return obstacle_clusters, r0, rg, rho, obstacles_rho, workspace_rho, compute_time, obstacle_timing, exit_flag \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py new file mode 100644 index 0000000..987aeaa --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py @@ -0,0 +1,2 @@ +from .mpc import Mpc, pol2pos +from .motion_controller import MotionController \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py new file mode 100644 index 0000000..f51976a --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py @@ -0,0 +1,138 @@ +from motion_control.pfmpc_obstacle_constraints import Mpc, pol2pos +from starworlds.utils.misc import tic, toc +import numpy as np +import shapely + + +class MotionController: + def __init__(self, params, robot, reference_path, verbosity=0): + self.params = params + self.robot = robot + self.mpc = Mpc(params, robot) + self.verbosity = verbosity + self.reference_path = shapely.geometry.Point(reference_path[0]) if len(reference_path) == 1 else shapely.geometry.LineString(reference_path) # Waypoints + self.theta_g = self.reference_path.length + self.rhrp_L = self.params['N'] * self.params['dt'] * self.mpc.build_params['w_max'] + self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps']) + self.reset() + + def reset(self): + self.mpc.reset() + self.r_plus = None + self.theta = 0 + self.rhrp_path = None + # self.rhrp_L = self.params['N'] * self.mpc.build_params['dp_max'] + # self.rhrp_s = np.arange(0, self.rhrp_L + self.rhrp_ds, self.rhrp_ds) + self.path_pol = None + self.epsilon = None + self.solution = None + self.sol_feasible = None + self.u_prev = [0] * self.robot.nu + self.timing = {'workspace': 0, 'target': 0, 'mpc': 0} + + def extract_obs_par(self, obstacles): + n_ell_par = 3 * self.mpc.build_params['max_No_ell'] * (1 + self.mpc.build_params['N_obs_predict']) + n_pol_par = self.mpc.build_params['max_No_pol'] * self.mpc.build_params['max_No_vert'] * 2 + obs_par = [0] * (n_ell_par + n_pol_par) + obs_par[1:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params[ + 'max_No_ell'] + obs_par[2:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params[ + 'max_No_ell'] + obs_par_padded = obs_par.copy() + n_ell, n_pol = 0, 0 + + for o in obstacles: + if hasattr(o, "_a"): + # print(o.pos()) + j = n_ell * 3 * (1 + self.mpc.build_params['N_obs_predict']) + obs_par[j] = 1 # Include obstacle + obs_par[j + 1:j + 3] = o._a # Ellipse axes + mm = o._motion_model + pos, rot, t = mm.pos().copy(), mm.rot(), mm._t + if hasattr(mm, '_wp_idx'): + wp_idx = mm._wp_idx + for k in range(self.mpc.build_params['N_obs_predict']): + obs_par[j + 3 + 3 * k:j + 5 + 3 * k] = mm.pos() # Ellipse position + obs_par[j + 5 + 3 * k] = mm.rot() # Ellipse orientation + mm.move(None, self.mpc.build_params['dt']) + mm.set_pos(pos), mm.set_rot(rot) + mm._t = t + if hasattr(mm, '_wp_idx'): + mm._wp_idx = wp_idx + + obs_par_padded[j:j + 3 * (self.mpc.build_params['N_obs_predict'] + 1)] = obs_par[j:j + 3 * ( + self.mpc.build_params['N_obs_predict'] + 1)] + obs_par_padded[j + 1:j + 3] = o._a + self.params['obstacle_padding'] + + n_ell += 1 + + if hasattr(o, "vertices"): + idx = n_ell_par + n_pol * self.mpc.build_params['max_No_vert'] * 2 + vertices = shapely.ops.orient(o.polygon()).exterior.coords[:-1] + for i in range(self.mpc.build_params['max_No_vert']): + obs_par[idx + i * 2:idx + (i + 1) * 2] = vertices[i % len(vertices)] + vertices = shapely.ops.orient( + o.polygon().buffer(self.params['obstacle_padding'], quad_segs=1, cap_style=3, + join_style=2)).exterior.coords[:-1] + for i in range(self.mpc.build_params['max_No_vert']): + obs_par_padded[idx + i * 2:idx + (i + 1) * 2] = vertices[i % len(vertices)] + n_pol += 1 + return obs_par, obs_par_padded + + def compute_u(self, x): + return self.u_prev + + def update_policy(self, x, obstacles, workspace=None): + p = self.robot.h(x) + + # Initialize r_plus to robot position + if self.r_plus is None: + self.r_plus = p + + # Extract receding path from global target path + t0 = tic() + rhrp_path_sh = shapely.ops.substring(self.reference_path, start_dist=self.theta, end_dist=self.theta + self.rhrp_L) + if rhrp_path_sh.length > 0: + self.rhrp_path = np.array([rhrp_path_sh.interpolate(s).coords[0] for s in self.rhrp_s]) + else: + self.rhrp_path = np.tile(rhrp_path_sh.coords[0], (len(self.rhrp_s), 1)) + + # Approximate target with polynomials + self.path_pol = np.polyfit(self.rhrp_s, self.rhrp_path[:, 0], self.params['n_pol']).tolist() + \ + np.polyfit(self.rhrp_s, self.rhrp_path[:, 1], self.params['n_pol']).tolist() + # Force init position to be correct + self.path_pol[self.params['n_pol']] = self.rhrp_path[0, 0] + self.path_pol[-1] = self.rhrp_path[0, 1] + # Compute polyfit approximation error + self.epsilon = max(np.linalg.norm(self.rhrp_path - np.array([pol2pos(self.path_pol, s, self.mpc.build_params['n_pol']) for s in self.rhrp_s]), axis=1)) + self.timing['target'] = toc(t0) + + t0 = tic() + # Extract obstacle parameters (Assumes all ellipses) + obs_par, obs_par_padded = self.extract_obs_par(obstacles) + # Compute MPC solution + if self.solution is not None: + init_guess = self.solution.copy() + else: + init_guess = None + solution_data = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, obs_par_padded, init_guess) + if solution_data is None: + self.sol_feasible, self.mpc_exit_status = False, "None" + else: + self.solution, self.mpc_exit_status = solution_data.solution, solution_data.exit_status + self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, obs_par, d=self.verbosity > 0) + + if self.sol_feasible: + u, w = self.mpc.sol2uds(self.solution) + self.u_prev = u[:self.robot.nu] + + p_ref_dist = rhrp_path_sh.distance(shapely.geometry.Point(p)) + if self.theta_g > 0 and p_ref_dist < 1: + self.theta = min(self.theta + w[0] * self.params['dt'], self.theta_g) + else: + self.u_prev = [0] * self.robot.nu + + self.timing['mpc'] = toc(t0) + + + diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py new file mode 100644 index 0000000..917d47a --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py @@ -0,0 +1,398 @@ +import casadi.casadi as cs +import opengen as og +import numpy as np +import os, sys +import yaml + + +class NoSolutionError(Exception): + '''raise this when there's no solution to the mpc problem''' + pass + +def pol_eval(pol_coeff, x, n_pol): + return sum([pol_coeff[i] * x ** (n_pol - i) for i in range(n_pol+1)]) + + +def pol2pos(pol_coeff, s, n_pol): + return [pol_eval(pol_coeff[:n_pol+1], s, n_pol), pol_eval(pol_coeff[n_pol+1:], s, n_pol)] + +# Returns >0 for interior points, <0 for exterior points, =0 for boundary points +def ellipse_function(p, pos, rot, ax): + x_dist = p[0] - pos[0] + y_dist = p[1] - pos[1] + s, c = cs.sin(rot), cs.cos(rot) + normalized_center_dist = ((x_dist * c + y_dist * s) ** 2) / (ax[0] ** 2) + ((x_dist * s + y_dist * c) ** 2) / (ax[1] ** 2) + return 1 - normalized_center_dist + +# Returns >0 for interior points, otherwise 0 +def convex_polygon_function(p, vertices, N): + hp_vals = [] + for i in range(N-1): + idc1, idc2 = i*2, (i+1) % N * 2 + x1, y1 = vertices[idc1], vertices[idc1+1] + x2, y2 = vertices[idc2], vertices[idc2+1] + dx = x2 - x1 + dy = y2 - y1 + Ai = cs.vertcat(dy, -dx) + bi = x1 * dy - y1 * dx + hp_vals += [bi-cs.dot(Ai, p)] + return cs.vertcat(*hp_vals) + +class Mpc: + + def __init__(self, params, robot): + self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build') + # Load parameters + self.build_params = None + self.robot = robot + self.set_build_params(params) + rebuild, self.build_name = self.get_build_version() + self.generate_sol_fun() + # Build if different from latest build + if rebuild: + self.build() + else: + print("Found MPC build: {}".format(self.build_name)) + + sys.path.insert(1, os.path.join(self.build_dir, self.build_name)) + optimizer = __import__(self.build_name) + self.solver = optimizer.solver() + self.reset() + + def reset(self): + self.sol_prev = None + + def set_build_params(self, params): + self.build_params = { + 'mode': params['build_mode'], + 'N': params['N'], + 'dt': params['dt'], + 'solver_tol': params['solver_tol'], + 'solver_max_time': params['solver_max_time'], + 'solver_max_inner_iterations': params['solver_max_inner_iterations'], + 'solver_max_outer_iterations': params['solver_max_outer_iterations'], + 'x_min': self.robot.x_min, + 'x_max': self.robot.x_max, + 'u_min': self.robot.u_min, + 'u_max': self.robot.u_max, + 'nx': self.robot.nx, + 'nu': self.robot.nu, + 'np': 2, + 'robot_model': self.robot.__class__.__name__, + 'n_pol': params['n_pol'], + 'integration_method': params['integration_method'], + 'w_max': self.robot.vmax, + 'max_No_ell': params['max_No_ell'], + 'max_No_pol': params['max_No_pol'], + 'max_No_vert': params['max_No_vert'], + 'N_obs_predict': params['N_obs_predict'], + } + + def get_build_version(self): + builds = [name for name in os.listdir(self.build_dir) + if os.path.isdir(os.path.join(self.build_dir, name))] + for build_ver in builds: + if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')): + continue + with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file: + build_ver_params = yaml.load(file, Loader=yaml.FullLoader) + if self.build_params == build_ver_params: + return False, build_ver + ver = 0 + while 'ver' + str(ver) in os.listdir(self.build_dir): + ver += 1 + return True, 'ver' + str(ver) + + def cs_obstacle_evaluation(self, obs_par, x, k): + n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict']) + obs_val = [] + for i in range(self.build_params['max_No_ell']): + j = i * 3 * (1 + self.build_params['N_obs_predict']) + time_idx = min(k, self.build_params['N_obs_predict']-1) + include_obs = obs_par[j] + ell_axs = obs_par[j + 1:j + 3] + ell_pos = obs_par[j + 3 + 3 * time_idx:j + 5 + 3 * time_idx] + ell_rot = obs_par[j + 5 + 3 * time_idx] + ell_val = ellipse_function(self.robot.h(x), ell_pos, ell_rot, ell_axs) + obs_val += [cs.fmax(include_obs * ell_val, 0)] + + for i in range(self.build_params['max_No_pol']): + j = n_ell_par + i * self.build_params['max_No_vert'] * 2 + hp_vals = convex_polygon_function(self.robot.h(x), obs_par[j : j + 2 * self.build_params['max_No_vert']], self.build_params['max_No_vert']) + obs_val += [cs.fmax(cs.mmin(hp_vals),0)] + return cs.vertcat(*obs_val) + + def is_feasible(self, sol, x0, path_pol, obs_par, d=False): + u, w = self.sol2uds(sol) + x, s, e = self.sol2state(sol, x0, path_pol) + u_min, w_min, sN_min, x_min, obs_val_min = self.min_bounds() + u_max, w_max, sN_max, x_max, obs_val_max = self.max_bounds() + obs_val = self.sol2obsval(sol, x0, obs_par) + + eps_val = 1e-8 + def in_range(val, min, max): + return ((val >= min - eps_val) & (val <= max + eps_val)).all() + + sol_ok = True + if not in_range(u, u_min, u_max): + sol_ok = False + if d: + print("[MPC]: Bad u") + print(u) + if not in_range(w, w_min, w_max): + sol_ok = False + if d: + print("[MPC]: Bad w") + print(w) + if not in_range(x, x_min, x_max): + sol_ok = False + if d: + print("[MPC]: Bad x") + print(x) + if not in_range(s[-1], sN_min, sN_max): + sol_ok = False + if d: + print("[MPC]: Bad sN {:.4f} > {:.4f}".format(s[-1], sN_max)) + if not in_range(obs_val, 0, 0): + sol_ok = False + return sol_ok + + def error(self, x, s, path_pol): + p_ref = cs.vertcat(*pol2pos(path_pol, s, self.build_params['n_pol'])) + return cs.norm_2(p_ref - self.robot.h(x)) + + def base_solution(self, x0, path_pol, w0): + u = [0] * (self.build_params['N'] * self.build_params['nu']) + w = [0] * (self.build_params['N']) + w[0] = w0 + + if self.build_params['robot_model'] == 'Unicycle': + p_ref = pol2pos(path_pol, w0*self.build_params['dt'], self.build_params['n_pol']) + k1, k2 = 10, 2 + # k1, k2 = 0.15, 0.3 + + x = x0.copy() + for i in range(self.build_params['N']): + e = np.array(x[:2]) - np.array(p_ref) + th = x[2] + + r_th = np.arctan2(e[1], e[0]) + np.pi + # E Theta in (-pi, pi] + e_th = r_th - th + e_th0 = e_th + while e_th > np.pi: + e_th -= 2 * np.pi + while e_th <= -np.pi: + e_th += 2 * np.pi + + ui = [ + -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)), + k2 * (e_th) + ] + + if self.robot.u_min[0] == 0 and ui[0] < 0: + ui[0] = 0 + u0_sig = 1 + else: + u0_sig = ui[0] / self.robot.u_max[0] if ui[0] >= 0 else ui[0] / self.robot.u_min[0] + u1_sig = ui[1] / self.robot.u_max[1] if ui[1] >= 0 else ui[1] / self.robot.u_min[1] + + sig = max(u0_sig, u1_sig, 1) + if sig > 1: + if u0_sig > u1_sig: + ui[0] = self.robot.u_max[0] if ui[0] > 0 else self.robot.u_min[0] + ui[1] /= sig + else: + ui[0] /= sig + ui[1] = self.robot.u_max[1] if ui[1] > 0 else self.robot.u_min[1] + u[i * self.build_params['nu']:(i+1) * self.build_params['nu']] = ui + + x, _ = self.robot.move(x, ui, self.build_params['dt']) + + return u + w + + def discrete_integration(self, f, x, u): + dt = self.build_params['dt'] + if self.build_params['integration_method'] == 'euler': + return x + f(x, u) * dt + if self.build_params['integration_method'] == 'RK4': + k1 = f(x, u) + k2 = f(x + dt / 2 * k1, u) + k3 = f(x + dt / 2 * k2, u) + k4 = f(x + dt * k3, u) + return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) + raise NotImplemented + + def generate_sol_fun(self): + # Initialize + sol_sym = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N']) + x0_sym = cs.SX.sym('x0', self.build_params['nx']) + path_pol_sym = cs.SX.sym('path_pol', self.build_params['np'] * (self.build_params['n_pol'] + 1)) + n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict']) + n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2 + obs_par_sym = cs.SX.sym('obs_par', n_ell_par + n_pol_par) + cost_params = {'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], 'obs_pen': 1, + 'u_prev': self.build_params['nu']} + cost_params_sym = cs.SX.sym("cost_params", sum(list(cost_params.values()))) + # Exchange parameter dimension with SX variable + p_idx = 0 + for key, dim in cost_params.items(): + cost_params[key] = cost_params_sym[p_idx:p_idx + dim] + p_idx += dim + # Initialize + u, w = self.sol2uds(sol_sym) + x_k, s_k = x0_sym, 0 + e_k = self.error(x_k, s_k, path_pol_sym) + obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, 0) + x, s, e, obs_val = x_k, s_k, e_k, obs_val_k + # Loop over time steps + for k in range(self.build_params['N']): + # Current control variables + u_k = u[k * self.build_params['nu']:(k + 1) * self.build_params['nu']] + # Integrate one step + x_k = self.discrete_integration(lambda x, u: cs.vertcat(*self.robot.f(x,u)), x_k, u_k) + s_k = self.discrete_integration(lambda s, w: w, s_k, w[k]) + # Extract error and obstacle values + e_k = self.error(x_k, s_k, path_pol_sym) + obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, k+1) + # Store current state + x = cs.vertcat(x, x_k) + s = cs.vertcat(s, s_k) + e = cs.vertcat(e, e_k) + obs_val = cs.vertcat(obs_val, obs_val_k) + # Define costs + u_target = cs.SX([self.build_params['w_max'], 0]) + du = u - cs.vertcat(cost_params['u_prev'], u[:-self.build_params['nu']]) + u_err = cs.repmat(u_target,self.build_params['N']) - u + R = cs.repmat(cost_params['R'], self.build_params['N']) + DR = cs.repmat(cost_params['DR'], self.build_params['N']) + w_cost = cost_params['cs'] * cs.sum1(self.build_params['w_max'] - w) + e_cost = cost_params['ce'] * cs.sum1(e) + u_cost = cs.dot(R, u_err * u_err) + du_cost = cs.dot(DR, du * du) + obs_cost = cost_params['obs_pen'] * cs.sum1(obs_val * obs_val) + # Define constraints + self.cs_sol2state = cs.Function('cs_sol2state', [sol_sym, x0_sym, path_pol_sym], [x, s, e], ['sol', 'x0', 'path_pol'], ['x', 's', 'e']) + self.cs_sol2cost = cs.Function('cs_sol2cost', [sol_sym, x0_sym, path_pol_sym, obs_par_sym, cost_params_sym], [w_cost, e_cost, u_cost, du_cost, obs_cost], ['sol', 'x0', 'path_pol', 'obs_par', 'cost_params'], ['s_cost', 'e_cost', 'u_cost', 'du_cost', 'obs_cost']) + self.cs_sol2obsval = cs.Function('cs_sol2obsval', [sol_sym, x0_sym, obs_par_sym], [obs_val], ['sol', 'x0', 'obs_par'], ['obs_val']) + + def min_bounds(self): + u_min = np.tile(self.build_params['u_min'], self.build_params['N']) + w_min = np.zeros(self.build_params['N']) + sN_min = 0 + x_min = np.tile(self.build_params['x_min'], self.build_params['N'] + 1) + obs_val_min = np.tile(-np.inf, self.build_params['max_No_ell'] * (self.build_params['N'] + 1)) + return u_min, w_min, sN_min, x_min, obs_val_min + + def max_bounds(self): + u_max = np.tile(self.build_params['u_max'], self.build_params['N']) + w_max = np.tile(self.build_params['w_max'], self.build_params['N']) + sN_max = self.build_params['w_max'] * self.build_params['N'] * self.build_params['dt'] + x_max = np.tile(self.build_params['x_max'], self.build_params['N'] + 1) + obs_val_max = np.zeros(self.build_params['max_No_ell'] * (self.build_params['N'] + 1)) + return u_max, w_max, sN_max, x_max, obs_val_max + + def sol2state(self, sol, x0, path_pol): + x, s, e = self.cs_sol2state(sol, x0, path_pol) + # cs.MX to [float] + return np.array(x).flatten(), np.array(s).flatten(), np.array(e).flatten() + + def sol2obsval(self, sol, x0, obs_par): + return np.array(self.cs_sol2obsval(sol, x0, obs_par)).flatten() + + def sol2cost(self, sol, x0, path_pol, obs_par, params, u_prev): + cost_params = [params['cs'], params['ce'], params['R'][0], 0, params['DR'][0], 0, params['obs_pen']] + u_prev + cost_params[2:6] = [params['R'][0], 0, params['DR'][0], 0] + s_cost, e_cost, u1_cost, ud1_cost, obs_cost = self.cs_sol2cost(sol, x0, path_pol, obs_par, cost_params) + cost_params[2:6] = [0, params['R'][1], 0, params['DR'][1]] + _, _, u2_cost, ud2_cost, _ = self.cs_sol2cost(sol, x0, path_pol, obs_par, cost_params) + return {'s': float(s_cost), 'e': float(e_cost), 'u': float(u1_cost + u2_cost), 'ud': float(ud1_cost + ud2_cost), + 'u1': float(u1_cost), 'ud1': float(ud1_cost), 'u2': float(u2_cost), 'ud2': float(ud2_cost), 'obs': float(obs_cost)} + + def sol2uds(self, sol): + u = sol[:self.build_params['nu'] * self.build_params['N']] + ds = sol[self.build_params['nu'] * self.build_params['N']:] + return u, ds + + def build(self): + # Build parametric optimizer + # ------------------------------------ + n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict']) + n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2 + + params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'], + 'path_pol': self.build_params['np'] * (self.build_params['n_pol'] + 1), + 'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], 'obs_pen': 1, + 'obs_par': n_ell_par + n_pol_par} + par_dim = sum(list(params.values())) + + # Exchange parameter dimension with value + par = cs.SX.sym("par", par_dim) # Parameters + p_idx = 0 + for key, dim in params.items(): + params[key] = par[p_idx:p_idx + dim] + p_idx += dim + + # Initialize + sol = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N']) + + # Cost + cost_params = cs.vertcat(params['cs'], params['ce'], params['R'], params['DR'], params['obs_pen'], params['u_prev']) + s_cost, e_cost, u_cost, du_cost, obs_cost = self.cs_sol2cost(sol, params['x0'], params['path_pol'], params['obs_par'], cost_params) + cost = s_cost + e_cost + u_cost + du_cost + obs_cost + + # Constraints + x, s, e = self.cs_sol2state(sol, params['x0'], params['path_pol']) + obs_val = self.cs_sol2obsval(sol, params['x0'], params['obs_par']) + u_min, w_min, sN_min, x_min, obs_val_min = self.min_bounds() + u_max, w_max, sN_max, x_max, obs_val_max = self.max_bounds() + sol_bounds = og.constraints.Rectangle(u_min.tolist() + w_min.tolist(), u_max.tolist() + w_max.tolist()) + agl_con = cs.vertcat(s, + x, + obs_val) + agl_con_bounds = og.constraints.Rectangle([sN_min] * (self.build_params['N'] + 1) + + x_min.tolist() + obs_val_min.tolist(), + [sN_max] * (self.build_params['N'] + 1) + + x_max.tolist() + obs_val_max.tolist()) + + # Setup builder + problem = og.builder.Problem(sol, par, cost) \ + .with_constraints(sol_bounds) \ + .with_aug_lagrangian_constraints(agl_con, agl_con_bounds) + build_config = og.config.BuildConfiguration() \ + .with_build_directory(self.build_dir) \ + .with_build_mode(self.build_params['mode']) \ + .with_build_python_bindings() + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(self.build_name) + solver_config = og.config.SolverConfiguration() \ + .with_tolerance(self.build_params['solver_tol']) \ + .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \ + .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \ + .with_max_outer_iterations(self.build_params['solver_max_outer_iterations']) + builder = og.builder.OpEnOptimizerBuilder(problem, + meta, + build_config, + solver_config) \ + .with_verbosity_level(1) + builder.build() + print('') + + # Save build params + with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file: + yaml.dump(self.build_params, file) + + def shift_sol(self, sol): + return sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \ + sol[self.build_params['nu'] * (self.build_params['N'] - 1) : self.build_params['nu'] * self.build_params['N']] \ + + sol[self.build_params['nu'] * self.build_params['N']:] + + def run(self, x0, u_prev, path_pol, params, obs_par, init_guess=None): + p = x0 + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + params['DR'] + [params['obs_pen']] + obs_par + + if init_guess is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, 2*e_max, lam_rho): + # Use base solution as initial guess + init_guess = self.base_solution(x0, path_pol, 0.1) + # Run solver + return self.solver.run(p=p, initial_guess=init_guess) + diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py new file mode 100644 index 0000000..e6fff9e --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py @@ -0,0 +1 @@ +from .soads import f, SoadsController, draw_vector_field, compute_weights, f_nominal, rollout \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py new file mode 100644 index 0000000..6a1c11a --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py @@ -0,0 +1,386 @@ +import numpy as np +import shapely +from starworlds.starshaped_hull import cluster_and_starify +from starworlds.utils.misc import tic, toc + +class SoadsController: + + def __init__(self, params, robot, verbose=False): + if not robot.__class__.__name__ == 'Omnidirectional': + raise NotImplementedError("SoadsController only implemented for Omnidirectional robots.") + self.params = params + self.robot = robot + self.obstacle_clusters = None + self.obstacles_star = [] + self.dp_prev = None + self.verbose = verbose + self.timing = {'obstacle': 0, 'control': 0} + + def compute_u(self, x, pg, obstacles, workspace=None): + p = self.robot.h(x) + + if self.params['starify']: + self.obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles, p, pg, + self.params['hull_epsilon'], + max_compute_time= self.params['max_compute_time'], + workspace=workspace, + previous_clusters=self.obstacle_clusters, + # dx_prev=self.dp_prev, + make_convex=self.params['make_convex'], + verbose=self.verbose) + self.timing['obstacle'] = sum(obstacle_timing) + self.obstacles_star = [cl.cluster_obstacle for cl in self.obstacle_clusters] + + # import matplotlib.pyplot as plt + # tmp_h = [] + # ax = plt.gca() + # for o in self.obstacle_clusters: + # for k in o.kernel_points: + # tmp_h += ax.plot(*k, 'y*') + # while not plt.waitforbuttonpress(): pass + # [h.remove() for h in tmp_h] + + + # print(obstacle_timing) + + # self.obstacles_star = [] + # for cl in self.obstacle_clusters: + # cl_obstacles = cl.obstacles + # for i in range(len(cl_obstacles)): + # cl_obstacles[i].set_xr(cl.cluster_obstacle.xr()) + # self.obstacles_star += cl_obstacles + # print(obstacle_timing, n_iter) + # print("(Cl/Adm/Hull) calculation ({:.2f}/{:.2f}/{:.2f}) [{:.0f}]".format(*obstacle_timing[:-1], n_iter)) + + else: + self.timing['obstacle'] += 0 + self.obstacles_star = obstacles + + # if any([o.interior_point(p) for o in self.obstacles_star]): + # import matplotlib.pyplot as plt + # from obstacles.tools import draw_shapely_polygon, is_cw, is_ccw, is_collinear, RayCone + # from starshaped_hull import admissible_kernel + # print("p in obstacles") + # for cl in self.obstacle_clusters: + # _, ax = plt.subplots() + # # cl.cluster_obstacle.draw(ax=ax, fc='r', alpha=0.5) + # for o in cl.obstacles: + # o.draw(ax=ax, show_name=1) + # # if o.id() == 1: + # # ad_ker = admissible_kernel(o, p) + # # # tp1, tp2 = o.tangent_points(p) + # # # if is_ccw(p, tp1, tp2): + # # # print('ccw') + # # # elif is_cw(p, tp1, tp2): + # # # print('cw') + # # # elif is_collinear(p, tp1, tp2): + # # # print('collinear') + # # # ax.plot(*tp1, 'rx') + # # # ax.plot(*tp2, 'rs') + # # draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.3) + # + # ax.plot(*p, 'o') + # ax.set_xlim([0, 13]) + # ax.set_ylim([-1, 11]) + # adm_ker_robot = RayCone.intersection_same_apex([admissible_kernel(o, p) for o in cl.obstacles], debug_info={'o': obstacles, 'x': p, 'xlim': [0, 13], 'ylim': [-1, 11]}) + # if cl.admissible_kernel is not None: + # # draw_shapely_polygon(cl.admissible_kernel, ax=ax, fc='y', alpha=0.3) + # draw_shapely_polygon(adm_ker_robot.polygon(), ax=ax, fc='y', alpha=0.3) + # plt.show() + + t0 = tic() + dist_to_goal = np.linalg.norm(p - pg) + if self.params['lin_vel'] > 0: + lin_vel = min(self.params['lin_vel'], dist_to_goal / self.params['dt']) + unit_mag = True + else: + lin_vel = 1 + unit_mag = False + dp = lin_vel * f(p, pg, self.obstacles_star, workspace=workspace, unit_magnitude=unit_mag, crep=self.params['crep'], + reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'], + adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'], + convergence_tolerance=self.params['convergence_tolerance']) + + p_next = p + dp * self.params['dt'] + + while not all([o.exterior_point(p_next) for o in self.obstacles_star]) and (workspace is None or workspace.interior_point(p_next)): + dp *= self.params['dp_decay_rate'] + p_next = p + dp * self.params['dt'] + # Additional compute time check + if toc(t0) > self.params['max_compute_time']: + if self.verbose or True: + print("[Max compute time in soads when adjusting for collision. ") + dp *= 0 + break + self.timing['control'] = toc(t0) + + self.dp_prev = dp + + return dp + +def f_nominal(r, f_nominal, obstacles, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False, + convergence_tolerance=1e-4, d=False): + No = len(obstacles) + + if No == 0 and workspace is None: + return f_nominal + + ef = [-f_nominal[1], f_nominal[0]] + Rf = np.vstack((f_nominal, ef)).T + + r_sh = shapely.geometry.Point(r) + + mu = [obs.reference_direction(r) for obs in obstacles] + normal = [obs.normal(r) for obs in obstacles] + gamma = [obs.distance_function(r) for obs in obstacles] + dist = [obs.polygon().distance(r_sh) for obs in obstacles] + + workspace_avoidance = False + if workspace is not None: + workspace_avoidance = np.linalg.norm( + workspace.xr() - r) > 1e-4 # workspace.polygon().exterior.distance(r_sh) < 1 + + if workspace_avoidance: + mu += [workspace.reference_direction(r)] + normal += [workspace.normal(r)] + gamma += [1 / workspace.distance_function(r)] + dist += [workspace.polygon().exterior.distance(r_sh)] + No += 1 + + # Compute weights + # w = compute_weights(gamma) + w = compute_weights(dist, gamma_lowerlimit=0) + + # Compute obstacle velocities + xd_o = np.zeros((2, No)) + if adapt_obstacle_velocity: + for i, obs in enumerate(obstacles): + xd_o[:, i] = obs.vel_intertial_frame(r) + + kappa = 0. + f_mag = 0. + for i in range(No): + # Compute basis matrix + E = np.zeros((2, 2)) + E[:, 0] = mu[i] + E[:, 1] = [-normal[i][1], normal[i][0]] + # Compute eigenvalues + D = np.zeros((2, 2)) + if workspace_avoidance and i == No - 1: + inactive_repulsion = normal[i].dot(f_nominal) < 0 + # inactive_repulsion = False + D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity)) + D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity) + else: + inactive_repulsion = mu[i].dot(f_nominal) > 0 and normal[i].dot(f_nominal) > 0 + # inactive_repulsion = False + D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity)) + D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity) + # Compute modulation + M = E.dot(D).dot(np.linalg.inv(E)) + # f_i = M.dot(f_nominal) + f_i = M.dot(f_nominal - xd_o[:, i]) + xd_o[:, i] + # Compute contribution to velocity magnitude + f_i_abs = np.linalg.norm(f_i) + f_mag += w[i] * f_i_abs + # Compute contribution to velocity direction + nu_i = f_i / f_i_abs + nu_i_hat = Rf.T.dot(nu_i) + kappa_i = np.arccos(np.clip(nu_i_hat[0], -1, 1)) * np.sign(nu_i_hat[1]) + kappa += w[i] * kappa_i + kappa_norm = abs(kappa) + f_o = Rf.dot([np.cos(kappa_norm), np.sin(kappa_norm) / kappa_norm * kappa]) if kappa_norm > 0. else f_nominal + + if unit_magnitude: + f_mag = 1. + + # f_mag = np.linalg.norm(f_nominal) + + return f_mag * f_o + +# TODO: Rename to something with linear attractor +def f(r, rg, obstacles, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False, + convergence_tolerance=1e-4, d=False): + goal_vector = rg - r + goal_dist = np.linalg.norm(goal_vector) + if goal_dist < convergence_tolerance: + return 0 * r + + No = len(obstacles) + fa = goal_vector / goal_dist # Attractor dynamics + + + if No == 0 and workspace is None: + return fa + + return f_nominal(r, fa, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect, + convergence_tolerance, d) + + ef = [-fa[1], fa[0]] + Rf = np.vstack((fa, ef)).T + + for o in obstacles: + if o.boundary_mapping(r) is None: + import matplotlib.pyplot as plt + o.draw(ax=plt.gca(), fc='r', alpha=0.8) + hs = plt.plot(*zip(o.xr(),r), 'b-') + hs += plt.plot(*zip(o.xr(), o.xr()+1.1*o.enclosing_ball_diameter*o.reference_direction(r)), 'r--') + while not plt.waitforbuttonpress(): pass + [h.remove() for h in hs] + + r_sh = shapely.geometry.Point(r) + + mu = [obs.reference_direction(r) for obs in obstacles] + normal = [obs.normal(r) for obs in obstacles] + gamma = [obs.distance_function(r) for obs in obstacles] + dist = [obs.polygon().distance(r_sh) for obs in obstacles] + + workspace_avoidance = False + if workspace is not None: + workspace_avoidance = np.linalg.norm(workspace.xr()-r) > 1e-4# workspace.polygon().exterior.distance(r_sh) < 1 + + if workspace_avoidance: + mu += [workspace.reference_direction(r)] + normal += [workspace.normal(r)] + gamma += [1 / workspace.distance_function(r)] + dist += [workspace.polygon().exterior.distance(r_sh)] + No += 1 + + # Compute weights + # w = compute_weights(gamma) + w = compute_weights(dist, gamma_lowerlimit=0) + + # Compute obstacle velocities + xd_o = np.zeros((2, No)) + if adapt_obstacle_velocity: + for i, obs in enumerate(obstacles): + xd_o[:, i] = obs.vel_intertial_frame(r) + + kappa = 0. + f_mag = 0. + for i in range(No): + # Compute basis matrix + E = np.zeros((2, 2)) + E[:, 0] = mu[i] + E[:, 1] = [-normal[i][1], normal[i][0]] + # Compute eigenvalues + D = np.zeros((2, 2)) + if workspace_avoidance and i == No-1: + inactive_repulsion = normal[i].dot(fa) < 0 + D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity)) + D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity) + else: + inactive_repulsion = mu[i].dot(fa) > 0 and normal[i].dot(fa) > 0 + D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity)) + D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity) + # Compute modulation + M = E.dot(D).dot(np.linalg.inv(E)) + # f_i = M.dot(fa) + f_i = M.dot(fa - xd_o[:, i]) + xd_o[:, i] + # Compute contribution to velocity magnitude + f_i_abs = np.linalg.norm(f_i) + f_mag += w[i] * f_i_abs + # Compute contribution to velocity direction + nu_i = f_i / f_i_abs + nu_i_hat = Rf.T.dot(nu_i) + kappa_i = np.arccos(np.clip(nu_i_hat[0], -1, 1)) * np.sign(nu_i_hat[1]) + kappa += w[i] * kappa_i + kappa_norm = abs(kappa) + f_o = Rf.dot([np.cos(kappa_norm), np.sin(kappa_norm) / kappa_norm * kappa]) if kappa_norm > 0. else fa + + if unit_magnitude: + f_mag = 1. + return f_mag * f_o + + +def compute_weights( + gamma, + gamma_lowerlimit=1, + gamma_upperlimit=500, + weightPow=2 +): + """Compute weights based on a distance measure (with no upper limit)""" + gamma = np.array(gamma) + n_points = gamma.shape[0] + + collision_points = gamma <= gamma_lowerlimit + + if np.any(collision_points): # at least one + if np.sum(collision_points) == 1: + return collision_points * 1.0 + else: + # TODO: continuous weighting function + w = collision_points * 1.0 / np.sum(collision_points) + return w + + gamma = gamma - gamma_lowerlimit + + w = (1 / gamma) ** weightPow + if np.sum(w) == 0: + return w + w = w / np.sum(w) # Normalization + return w + + +def rollout(x, xg, obstacles, step_size=0.01, max_nr_steps=10000, max_distance=np.inf, convergence_threshold=0.05, crep=1., reactivity=1.): + xs = np.zeros((2, max_nr_steps)) + xs[:, 0] = x + dist_travelled = 0 + for k in range(1, max_nr_steps): + dist_to_goal = np.linalg.norm(xs[:, k-1]-xg) + if dist_to_goal < convergence_threshold or dist_travelled > max_distance: + return xs[:, :k] + else: + step_mag = min(step_size, dist_to_goal) + step_dir = f(xs[:, k-1], xg, obstacles, unit_magnitude=1, crep=crep, reactivity=reactivity) + xs[:, k] = xs[:, k-1] + step_mag * step_dir + while any([o.interior_point(xs[:, k]) for o in obstacles]): + step_mag /= 2. + xs[:, k] = xs[:, k-1] + step_mag * step_dir + dist_travelled += step_mag + return xs + + +def draw_streamlines(xg, obstacles, ax, init_pos_list, step_size=0.01, max_nr_steps=10000, max_distance=np.inf, + convergence_threshold=0.05, crep=1., reactivity=1., + arrow_step=0.5, color='k', linewidth=1, **kwargs): + for x0 in init_pos_list: + xs = rollout(x0, xg, obstacles, step_size, max_nr_steps, max_distance, convergence_threshold, crep, reactivity) + travelled_dist = np.cumsum(np.linalg.norm(np.diff(xs, axis=1), axis=0)) + travelled_dist = np.insert(travelled_dist, 0, 0) + arrow_dist = np.arange(0, travelled_dist[-1], arrow_step) + arrows_X = np.interp(arrow_dist, travelled_dist, xs[0, :]) + arrows_Y = np.interp(arrow_dist, travelled_dist, xs[1, :]) + arrows_U = np.zeros_like(arrows_X) + arrows_V = np.zeros_like(arrows_X) + # ax.plot(arrows_X, arrows_Y, 'k*') + for j in range(len(arrow_dist)): + arrows_U[j], arrows_V[j] = f(np.array([arrows_X[j], arrows_Y[j]]), xg, obstacles, unit_magnitude=1, + crep=crep, reactivity=reactivity) + ax.quiver(arrows_X, arrows_Y, arrows_U, arrows_V, color=color, scale=50, width=0.005, scale_units='width', headlength=2, headaxislength=2) + # ax.plot(xs[0, :], xs[1, :], color=color, linewidth=linewidth, **kwargs) + +def draw_vector_field(xg, obstacles, ax, workspace=None, xlim=None, ylim=None, n=50, crep=1., reactivity=1., **kwargs): + if xlim is None: + xlim = ax.get_xlim() + if ylim is None: + ylim = ax.get_ylim() + + def in_boundary(x): + return True if workspace is None else workspace.interior_point(x) + + Y, X = np.mgrid[ylim[0]:ylim[1]:n*1j, xlim[0]:xlim[1]:n*1j] + U = np.zeros(X.shape) + V = np.zeros(X.shape) + mask = np.zeros(U.shape, dtype=bool) + for i in range(X.shape[1]): + for j in range(X.shape[0]): + x = np.array([X[i, j], Y[i, j]]) + if in_boundary(x) and np.all([o.exterior_point(x) for o in obstacles]): + U[i, j], V[i, j] = f(x, xg, obstacles, workspace=workspace, unit_magnitude=1, crep=crep, reactivity=reactivity) + else: + mask[i, j] = True + U[i, j] = np.nan + U = np.ma.array(U, mask=mask) + # return ax.quiver(X, Y, U, V) + return ax.streamplot(X, Y, U, V, **kwargs) \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/__init__.py b/python/ur_simple_control/path_generation/star_navigation/robot/__init__.py new file mode 100644 index 0000000..a0f0b8a --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/robot/__init__.py @@ -0,0 +1,7 @@ +""" +The :mod:`robot` module implements mobile robots for obstacle avoidance. +""" +from .mobile_robot import MobileRobot +from .unicycle import Unicycle +from .bicycle import Bicycle +from .omnidirectional import Omnidirectional \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py b/python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py new file mode 100644 index 0000000..a2f4ec6 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py @@ -0,0 +1,55 @@ +import numpy as np +from robot import MobileRobot + + +class Bicycle(MobileRobot): + + def __init__(self, width, vel_min=None, vel_max=None, steer_max=None, name='robot'): + self.vmax = vel_max[0] + x_min = [-np.inf] * 3 + [-steer_max] + x_max = [np.inf] * 3 + [steer_max] + super().__init__(nu=2, nx=4, width=width, name=name, u_min=vel_min, u_max=vel_max, x_min=x_min, x_max=x_max) + + # [x,y of backwheel, orientation, steering angle] + # def f(self, x, u): + # return [u[0] * np.cos(x[2]), + # u[0] * np.sin(x[2]), + # u[0] * np.tan(u[1]) / self.width] + def f(self, x, u): + return [u[0] * np.cos(x[2]), + u[0] * np.sin(x[2]), + u[0] * np.tan(u[1]) / self.width, + u[1]] + # def f(self, x, u): + # return [u[0] * np.cos(x[2]) * np.cos(x[3]), + # u[0] * np.sin(x[2]) * np.cos(x[3]), + # u[0] * np.sin(x[3]) / self.width, + # u[1]] + + def h(self, x): + return [x[0] + self.width/2 * np.cos(x[2]), + x[1] + self.width/2 * np.sin(x[2])] + + def vel_min(self): + return self.u_min + + def vel_max(self): + return self.u_max + + def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10, **kwargs): + h = super(Bicycle, self).init_plot(ax=ax, color=color, alpha=alpha) + h += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color) + h += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w') + h += ax.plot(0, 0, color=color, alpha=alpha) + return h + + def update_plot(self, x, handles): + super(Bicycle, self).update_plot(x, handles) + handles[1].set_data(self.h(x)) + handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2))) + handles[1].set_markersize(handles[1].get_markersize()) + handles[2].set_data(x[0], x[1]) + handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2))) + handles[2].set_markersize(handles[2].get_markersize()) + handles[3].set_data([x[0], x[0] + self.width * np.cos(x[2])], + [x[1], x[1] + self.width * np.sin(x[2])]) \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py b/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py new file mode 100644 index 0000000..0177489 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py @@ -0,0 +1,74 @@ +import matplotlib.pyplot as plt +import matplotlib.patches as patches +from abc import ABC, abstractmethod +import numpy as np + + +class MobileRobot(ABC): + def __init__(self, nu, nx, radius, name, u_min=None, u_max=None, x_min=None, x_max=None, + integration_method='euler'): + self.nx = nx + self.nu = nu + self.radius = radius + self.name = name + def valid_u_bound(bound): return bound is not None and len(bound) == self.nu + def valid_q_bound(bound): return bound is not None and len(bound) == self.nx + self.u_min = u_min if valid_u_bound(u_min) else [-np.inf] * self.nu + self.u_max = u_max if valid_u_bound(u_max) else [np.inf] * self.nu + self.x_min = x_min if valid_q_bound(x_min) else [-np.inf] * self.nx + self.x_max = x_max if valid_q_bound(x_max) else [np.inf] * self.nx + self.integration_method = integration_method + + @abstractmethod + def f(self, x, u): + """ Forward dynamics ? """ + pass + + @abstractmethod + def h(self, q): + """ Forward kinematics """ + pass + + @abstractmethod + def vel_min(self): + pass + + @abstractmethod + def vel_max(self): + pass + + def move(self, x, u, dt): + u_sat = np.clip(u, self.u_min, self.u_max) + if self.integration_method == 'euler': + x_next = x + np.array(self.f(x, u_sat)) * dt + elif self.integration_method == 'RK4': + k1 = np.array(self.f(x, u_sat)) + k2 = np.array(self.f(x + dt / 2 * k1, u_sat)) + k3 = np.array(self.f(x + dt / 2 * k2, u_sat)) + k4 = np.array(self.f(x + dt * k3, u_sat)) + x_next = x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) + else: + print("Not a valid integration method") + x_next = np.clip(x_next, self.x_min, self.x_max) + return x_next, u_sat + +# def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10, **kwargs): +# if ax is None: +# _, ax = plt.subplots(subplot_kw={'aspect': 'equal'}) +# if self.radius == 0: +# handles = plt.plot(0, 0, '+', color=color, alpha=alpha, markersize=markersize, label=self.name, **kwargs) +# else: +# handles = [patches.Circle((0, 0), self.radius, color=color, alpha=alpha, label=self.name, **kwargs)] +# ax.add_patch(handles[0]) +# return handles, ax +# +# def update_plot(self, x, handles): +# if self.radius == 0: +# handles[0].set_data(*self.h(x)) +# else: +# handles[0].set(center=self.h(x)) +# +# def draw(self, x, **kwargs): +# handles, ax = self.init_plot(**kwargs) +# self.update_plot(x, handles) +# return handles, ax diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py b/python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py new file mode 100644 index 0000000..f476fac --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py @@ -0,0 +1,23 @@ +import numpy as np +from robot import MobileRobot + + +class Omnidirectional(MobileRobot): + + def __init__(self, radius, vel_max, name='robot'): + vel_max = vel_max * np.ones(2) + self.vmax = float(np.linalg.norm(vel_max)) + super().__init__(nu=2, nx=2, radius=radius, name=name, u_min=(-vel_max).tolist(), u_max=(vel_max).tolist()) + + def f(self, x, u): + return [u[0], + u[1]] + + def h(self, x): + return x[:2] + + def vel_min(self): + return self.u_min + + def vel_max(self): + return self.u_max diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py b/python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py new file mode 100644 index 0000000..5ae1226 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py @@ -0,0 +1,68 @@ +import numpy as np +from robot import MobileRobot +import matplotlib.pyplot as plt + + +def get_triangle_vertices(center, orientation, side_length): + # calculate the distance from center to vertex + dist = side_length / (2 * np.sin(np.radians(60))) + + # calculate the x and y offsets from center to vertex + x_offset = dist * np.cos(orientation) + y_offset = dist * np.sin(orientation) + + # calculate the three vertices + vertex1 = (center[0] + x_offset, center[1] + y_offset) + vertex2 = (center[0] + x_offset * np.cos(np.radians(120)) - y_offset * np.sin(np.radians(120)), + center[1] + x_offset * np.sin(np.radians(120)) + y_offset * np.cos(np.radians(120))) + vertex3 = (center[0] + x_offset * np.cos(np.radians(240)) - y_offset * np.sin(np.radians(240)), + center[1] + x_offset * np.sin(np.radians(240)) + y_offset * np.cos(np.radians(240))) + + return (vertex1, vertex2, vertex3) + +class Unicycle(MobileRobot): + + def __init__(self, width, vel_min=None, vel_max=None, name='robot'): + self.vmax = vel_max[0] + super().__init__(nu=2, nx=3, width=width, name=name, u_min=vel_min, u_max=vel_max) + + def f(self, x, u): + return [u[0] * np.cos(x[2]), # vx + u[0] * np.sin(x[2]), # vy + u[1]] # wz + + + def move(self, x, u, dt): + u_sat = np.clip(u, self.u_min, self.u_max) + x_next = x + np.array(self.f(x, u_sat)) * dt + x_next = np.clip(x_next, self.x_min, self.x_max) + while x_next[2] > 2*np.pi: + x_next[2] -= 2 * np.pi + while x_next[2] <= -0*np.pi: + x_next[2] += 2 * np.pi + return x_next, u_sat + + + def h(self, x): + return x[:2] # x, y + + def vel_min(self): + return self.u_min + + def vel_max(self): + return self.u_max + + def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10): + handles, ax = super(Unicycle, self).init_plot(ax=ax, color=color, alpha=alpha) + handles += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color) + handles += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w') + return handles, ax + + def update_plot(self, x, handles): + super(Unicycle, self).update_plot(x, handles) + handles[1].set_data([x[0]], [x[1]]) + handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2))) + handles[1].set_markersize(handles[1].get_markersize()) + handles[2].set_data([x[0]], [x[1]]) + handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2))) + handles[2].set_markersize(handles[2].get_markersize()) diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py b/python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py new file mode 100644 index 0000000..993f33d --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py @@ -0,0 +1,135 @@ +import numpy as np +import matplotlib.pyplot as plt +from config.load_config import load_config +from visualization import PFMPCGUI + +make_video = 0 + +ctrl_param_file = 'pfmpc_ds_params.yaml' +robot_type_id = 3 +scene_id = 13 +verbosity = 0 + +scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity) + +workspaces = scene.workspace if isinstance(scene.workspace, list) else [scene.workspace] + + +# Settings +T_max = 200 +theta_max = 100 +dt = 0.01 +ctrl_sim_dt_ratio = int(controller.params['dt'] / dt) +show_gui = True + + +if show_gui: + gui = PFMPCGUI(robot, scene, x0, scene.xlim, scene.ylim, + controller=controller, robot_alpha=1., robot_color='k', + obstacles_star_alpha=0.2, obstacles_star_show_reference=0, + obstacles_star_color='b', + # workspace_rho_color='None', + # show_obs_name=1, + reference_color='y', + reference_markersize=10, + pg_markersize=10, pg_color='b', pg_marker='*', + theta_pos_color='g', theta_pos_marker='*', theta_pos_markersize=10, + s1_pos_color='g', s1_pos_marker='+', s1_pos_markersize=0, + mpc_path_linestyle='-', mpc_path_linewidth=4, mpc_path_color='None', + mpc_tunnel_color='r', + travelled_path_linestyle='--', travelled_path_linewidth=3, + receding_path_color='g', + receding_path_linewidth=2, + show_time=1, show_timing=1, show_axis=0 + ) + + +fig, ax = plt.subplots() +linestyles = ['-', '--', ':', '-.'] +rows = [] +data = [] + +x0_0s = [-4, -2, 0, 2] +for i, x0_0 in enumerate(x0_0s): + x0[:2] = [x0_0, -5.] + + + + # Initialize + k = 0 + x = x0 + active_ws_idx = 0 + converged = False + collision = False + timing = {'workspace': [], 'target': [], 'mpc': []} + controller.reset() + + + while k*dt <= T_max and controller.theta <= theta_max and not converged and not collision: + p = robot.h(x) + if active_ws_idx + 1 < len(workspaces): + ws_smaller = workspaces[active_ws_idx + 1].dilated_obstacle(-0.1, id='temp') + if controller.rhrp_path is not None and all([ws_smaller.interior_point(r) for r in controller.rhrp_path]): + active_ws_idx += 1 + if len(scene.reference_path) == 1: + controller.set_reference_path([scene.ws_attractors[active_ws_idx]]) + + # Move obstacles + scene.step(dt, p) + + # Control update + if k % ctrl_sim_dt_ratio == 0: + if k*dt % 2 == 0: + print(k*dt) + # Compute mpc + controller.update_policy(x, scene.obstacles, workspace=workspaces[active_ws_idx]) + # Update timing + close_to_convergence = (np.linalg.norm(controller.r_plus - controller.pg) < controller.rho) and (controller.theta >= controller.theta_g) + if not close_to_convergence: + for key in timing.keys(): + timing[key] += [controller.timing[key]] + + if show_gui: + gui.update(x, k*dt, controller) + plt.pause(0.005) + + # Robot integration + u = controller.compute_u(x) + x, _ = robot.move(x, u, dt) + k += 1 + + # Convergence and collision check + converged = controller.theta >= controller.theta_g and np.linalg.norm(robot.h(x)-scene.reference_path[-1]) < controller.params['convergence_tolerance'] + collision = any([o.interior_point(robot.h(x)) for o in scene.obstacles]) + if collision: + print("Collision") + + + ax.plot(timing['workspace'],'b', ls=linestyles[i]) + ax.plot(timing['target'],'r', ls=linestyles[i]) + ax.plot(timing['mpc'],'g', ls=linestyles[i]) + ax.legend(['Workspace modification', 'Target path generation', 'MPC']) + + for k, v in timing.items(): + rows += [k] + data += [[np.min(v), np.max(v), np.mean(v)]] + rows += [""] + +# Plot bars and create text labels for the table +columns = ['min', 'max', 'mean'] +cell_text = [] +for row in range(len(rows)): + if row % 2 == 0: + cell_text.append(["", "", ""]) + cell_text.append(["{:.2f}".format(t) for t in data[row]]) +# Add a table at the bottom of the axes +the_table = plt.table(cellText=cell_text, + rowLabels=rows, + colLabels=columns, + loc='bottom') + +fig.tight_layout() +plt.xticks([]) +plt.title('Computation time') +plt.show() + diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py b/python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py new file mode 100755 index 0000000..85a71d3 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py @@ -0,0 +1,304 @@ +import numpy as np +import matplotlib.pyplot as plt +from config.load_config import load_config +from visualization import PFMPCGUI, VideoWriter, StarobsGUI +from starworlds.utils.misc import tic, toc +from motion_control.pfmpc_ds.motion_controller import ControlMode +from motion_control.soads import draw_vector_field + +make_video = 0 + +# ctrl_param_file = 'tunnel_mpc_params.yaml' +ctrl_param_file = 'pfmpc_ds_params.yaml' +# ctrl_param_file = 'pfmpc_obstacle_constraints_params.yaml' +# ctrl_param_file = 'pfmpc_artificial_reference_params.yaml' +# ctrl_param_file = 'dmp_ctrl_params.yaml' +robot_type_id = 2 +scene_id = None +scene_id = 17 +verbosity = 0 +show_mpc_solution = 0 +show_mpc_cost = 0 +show_u_history = 1 +show_timing = 1 + +scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity) + +# x0[:2] = [0., -5.] +# x0s_10 = [[-2.5, 1.5, 0.], [2.4, -3.5, 0.], [3, .5, 0.], [2, 2.5, 0.], [-5, -5.5, 0.], [-2.8, -3.8, 0.]] +# x0 = np.array(x0s_10[4]) + +if hasattr(controller, 'mpc'): + gui = PFMPCGUI(robot, scene, x0, scene.xlim, scene.ylim, + show_mpc_solution=show_mpc_solution, show_mpc_cost=show_mpc_cost, show_u_history=show_u_history, + controller=controller, robot_alpha=1., robot_color='k', + obstacles_star_alpha=0.2, obstacles_star_show_reference=0, + obstacles_star_color='b', + workspace_rho_color='b', + # show_obs_name=1, + reference_color='y', + reference_markersize=10, + pg_markersize=10, pg_color='b', pg_marker='*', + theta_pos_color='g', theta_pos_marker='*', theta_pos_markersize=10, + s1_pos_color='g', s1_pos_marker='+', s1_pos_markersize=0, + mpc_path_linestyle=':', mpc_path_linewidth=4, mpc_path_color='None', + mpc_tunnel_color='None', + travelled_path_linestyle='-', travelled_path_linewidth=3, + receding_path_color='g', + receding_path_linewidth=2, + indicate_sbc=1, + show_time=1, show_timing=show_timing, show_axis=1, + ) +else: + gui = StarobsGUI(robot, scene, x0, scene.xlim, scene.ylim) + +workspaces = scene.workspace if isinstance(scene.workspace, list) else [scene.workspace] + +# Initialize +T_max = 50 +theta_max = 100 +dt = 0.01 +dt_gui = 0.1 +ctrl_sim_dt_ratio = int(controller.params['dt'] / dt) +gui_sim_dt_ratio = int(dt_gui / dt) +k = 0 +x = x0 +active_ws_idx = 0 +converged = False +collision = False +timing = {'workspace': [], 'target': [], 'mpc': []} +if 'workspace_detailed' in controller.timing: + workspace_timing_detailed = {'cluster': [], 'admker': [], 'hull': [], 'convex': []} + +# Init video writing +if make_video: + frame_cntr = 0 + video_name = input("Video file name: ") + video_writer = VideoWriter(video_name, 1/dt_gui) + gui.paused = False +else: + # Init gui plot + gui.update(x, k*dt) + plt.pause(0.005) + + +ls = [] + +# for i, attr in enumerate(scene.ws_attractors[:-1]): +# gui.ax.plot(*attr, 'y*', markersize=10) +# gui.ax.plot(*scene.ws_attractors[-1], 'g*', markersize=14, zorder=-1) + +# xlim_closeup, ylim_closeup = [-4.2, -0.8], [-5.2, -2.4] +# gui.ax.plot([xlim_closeup[0], xlim_closeup[1], xlim_closeup[1], xlim_closeup[0], xlim_closeup[0]], +# [ylim_closeup[0], ylim_closeup[0], ylim_closeup[1], ylim_closeup[1], ylim_closeup[0]], 'k-') + +while gui.fig_open and k*dt <= T_max and controller.theta <= theta_max and not converged and not collision: + + if gui.paused and not gui.step_once: + gui.fig.waitforbuttonpress() + + p = robot.h(x) + if active_ws_idx + 1 < len(workspaces): + ws_smaller = workspaces[active_ws_idx + 1].dilated_obstacle(-0.1, id='temp') + if controller.rhrp_path is not None and all([ws_smaller.interior_point(r) for r in controller.rhrp_path]): + active_ws_idx += 1 + if len(scene.reference_path) == 1: + controller.set_reference_path([scene.ws_attractors[active_ws_idx]]) + scene.reference_path = [scene.ws_attractors[active_ws_idx]] + + # Move obstacles + scene.step(dt, p) + + # Control update + if k % ctrl_sim_dt_ratio == 0: + gui.step_once = False + + # # For Fig 9.a + # if k == 1120: + # nominal_rhrp, _ = controller.nominal_rhrp(controller.r_plus, scene.obstacles[0].polygon()) + + # Compute mpc + controller.update_policy(x, scene.obstacles, workspace=workspaces[active_ws_idx]) + u = controller.compute_u(x) + + # Add input noise + u_noise = [.1 * np.random.randn(), .1 * np.random.randn()] + print(u_noise[0]/1, u_noise[1]/1.5) + u[0] = u[0] + u_noise[0] + u[1] = u[1] + u_noise[1] + + gui.update(x, k*dt, controller, u) + # Update timing + if k > 0: + for key in timing.keys(): + if key == 'tot': + continue + timing[key] += [controller.timing[key]] + if 'workspace_detailed' in controller.timing and controller.timing['workspace_detailed'] is not None: + for i, key in enumerate(workspace_timing_detailed.keys()): + workspace_timing_detailed[key] += [controller.timing['workspace_detailed'][i]] + + # if controller.mode == ControlMode.SBC: + # print("SBC at time {:.2f} (k={:.0f})".format(k*dt, k)) + + # For Fig 8.b + # if k == 1250: + # xmin, ymin, xmax, ymax = controller.workspace_rho.polygon().bounds + # draw_vector_field(controller.pg, controller.obstacles_star, xlim=[xmin, xmax], ylim=[ymin, ymax], + # ax=gui.ax, n=40, density=0.5, linewidth=1, color='tab:olive', zorder=-1) + # plt.pause(0.5) + # while not gui.fig.waitforbuttonpress(): pass + # + # For Fig 8.c + # if k == 1360: + # print("SBC at time {:.2f} (k={:.0f})".format(k*dt, k)) + # xlim, ylim = gui.ax.get_xlim(), gui.ax.get_ylim() + # gui.ax.set_xlim([-5.5, -2.6]) + # gui.ax.set_ylim([-4, 2]) + # draw_vector_field(controller.pg, controller.obstacles_star, workspace=controller.workspace_rho, + # ax=gui.ax, n=80, density=1.4, linewidth=1, color='tab:olive', zorder=-1) + # robot.draw(x, ax=gui.ax, color='k', markersize=26, alpha=1) + # plt.pause(0.5) + # while not gui.fig.waitforbuttonpress(): pass + # gui.ax.set_xlim(xlim) + # gui.ax.set_ylim(ylim) + + # # For Fig 8.d + # if k == 1520: + # xlim, ylim = gui.ax.get_xlim(), gui.ax.get_ylim() + # gui.ax.set_xlim([-5.5, -2.6]) + # gui.ax.set_ylim([-4, 2]) + # gui.mpc_path_handle.set_color('k') + # gui.mpc_path_handle.set_linestyle('-') + # robot.draw(x, ax=gui.ax, color='k', markersize=26, alpha=1) + # plt.pause(0.5) + # while not gui.fig.waitforbuttonpress(): pass + # gui.ax.set_xlim(xlim) + # gui.ax.set_ylim(ylim) + # gui.mpc_path_handle.set_color('None') + + # # For Fig 9.a + # if k == 1120: + # gui.ax.plot(*nominal_rhrp.xy, 'm-', linewidth=3, zorder=1) + # for o in controller.obstacles_rho: + # o.draw(ax=gui.ax, fc='y', zorder=0, show_reference=0) + # while not gui.fig.waitforbuttonpress(): pass + + # GUI update + if k % gui_sim_dt_ratio == 0: + gui.update(x, k*dt) + + if make_video: + + # if scene_id == 19: + # wait_frames = {1/dt: 2, 5/dt: 2, 6.6/dt: 2} + # for j in range(2/gui_sim_dt_ratio): + # video_writer.add_frame(gui.fig) + + video_writer.add_frame(gui.fig, frame_cntr) + frame_cntr += 1 + print("[VideoWriter] wrote frame at time {:.2f}/{:.2f}".format(k * dt, T_max)) + else: + plt.pause(0.005) + + # + # r = controller.rhrp_path[-1] + # mu = controller.obstacles_star[0].reference_direction(r) + # n = controller.obstacles_star[0].normal(r) + # b = controller.obstacles_star[0].boundary_mapping(r) + # gui.ax.quiver(*b, *mu) + # gui.ax.quiver(*b, *n, color='r') + # + # from motion_control.soads import compute_weights + # import shapely + # gammas = [obs.distance_function(p) for obs in controller.obstacles_star] + # xrs = [obs.xr() for obs in controller.obstacles_star] + # ds = [obs.polygon().distance(shapely.geometry.Point(p)) for obs in controller.obstacles_star] + # if controller.workspace_rho is not None: + # xrs += [controller.workspace_rho.xr()] + # gammas += [1 / controller.workspace_rho.distance_function(p+0.01)] + # ds += [controller.workspace_rho.polygon().exterior.distance(shapely.geometry.Point(p))] + # ws = compute_weights(gammas) + # ws = compute_weights(ds, gamma_lowerlimit=0) + # if ls: + # [l.remove() for l in ls if l is not None] + # ls = [] + # for xr, w, gamma, d in zip(xrs, ws, gammas, ds): + # ls += [gui.ax.text(*xr, "{:.2f}".format(w))] + + # while not gui.fig.waitforbuttonpress(): pass + # gui.ax.set_xlim(np.array(xlim_closeup) + np.array([-0.01, 0.01])) + # gui.ax.set_ylim(np.array(ylim_closeup) + np.array([-0.01, 0.01])) + + + # Robot integration + u = controller.compute_u(x) + x, _ = robot.move(x, u, dt) + k += 1 + + # Convergence and collision check + converged = controller.theta >= controller.theta_g and np.linalg.norm(robot.h(x)-scene.reference_path[-1]) < controller.params['convergence_tolerance'] + # print(np.linalg.norm(robot.h(x)-scene.reference_path[-1]), controller.params['convergence_tolerance'], converged) + collision = any([o.interior_point(robot.h(x)) for o in scene.obstacles]) + if collision: + print("Collision") + +if make_video: + # close video writer + video_writer.release() + print("Finished") + fig_open = False +else: + gui.update(x, k*dt, controller, u) + gui.ax.set_title("Time: {:.1f} s. Finished".format(k*dt)) + + if show_timing: + t = [np.array(timing['workspace']), np.array(timing['target']), np.array(timing['mpc'])] + fig, ax = plt.subplots() + ax.fill_between(range(len(timing['workspace'])), 0*t[0], t[0]) + ax.fill_between(range(len(timing['workspace'])), t[0], t[0]+t[1]) + ax.fill_between(range(len(timing['workspace'])), t[0]+t[1], t[0]+t[1]+t[2]) + # ax.plot(np.array(timing['workspace'])+np.array(timing['target'])) + # ax.plot(np.array(timing['workspace'])+np.array(timing['target'])+np.array(timing['mpc']),'--') + # ax.plot(timing['tot'], '--') + ax.legend(['Environment modification', 'RHRP', 'MPC']) + + columns = ['min', 'max', 'mean'] + rows = [] + data = [] + for k, v in timing.items(): + first_zero = np.where(np.array(v) == 0)[0][0] + rows += [k] + data += [[np.min(v[:first_zero]), np.max(v), np.mean(v)]] + # if 'workspace_detailed' in controller.timing: + # rows += [""] + # data += [[None, None, None]] + # for k, v in workspace_timing_detailed.items(): + # rows += [k] + # data += [[np.min(v), np.max(v), np.mean(v)]] + + # Plot bars and create text labels for the table + cell_text = [] + for row in range(len(rows)): + if data[row][0] is None: + cell_text.append(["", "", ""]) + else: + cell_text.append(["{:.2f}".format(t) for t in data[row]]) + # Add a table at the bottom of the axes + the_table = plt.table(cellText=cell_text, + rowLabels=rows, + colLabels=columns, + loc='bottom') + + # Adjust layout to make room for the table: + # plt.subplots_adjust(left=0.2, bottom=0.4) + fig.tight_layout() + plt.subplots_adjust(hspace=0.2) + + # plt.ylabel(f"Loss in ${value_increment}'s") + # plt.yticks(values * value_increment, ['%d' % val for val in values]) + plt.xticks([]) + plt.title('Computation time') + + # Wait until figure closed when converged + plt.show() diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py new file mode 100644 index 0000000..f07c299 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py @@ -0,0 +1,158 @@ +import numpy as np +import matplotlib.pyplot as plt +from config.load_config import load_config +from visualization import PFMPCGUI, VideoWriter +from motion_control.pfmpc_ds import pol2pos +import shapely + +# ------------------------------- # + +make_video = 1 + +# Scenario settings +ctrl_param_files = ['pfmpc_ds_params.yaml','pfmpc_obstacle_constraints_params.yaml', 'pfmpc_artificial_reference_params.yaml'] +ctrl_param_files = ['pfmpc_ds_params.yaml'] +robot_type_id = 3 +scene_id = 15 +verbosity = 0 + +# Simulation settings +T_max = 200 +dt = 0.01 +dt_gui = 0.1 + +# GUI settings +ctrl_colors = ['k', 'r', 'y'] +gui_robot_idx = 0 # GUI environment is shown for robot corresponding to this x0 idx + +# ------------------------------- # + + +N = len(ctrl_param_files) +travelled_path_handles = [None] * N +robot_handles = [[]] * N +controllers = [[]] * N +scenes = [[]] * N + +assert gui_robot_idx < N +scenes[gui_robot_idx], robot, controllers[gui_robot_idx], x0 = load_config(ctrl_param_file=ctrl_param_files[gui_robot_idx], robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity) +gui = PFMPCGUI(robot, scenes[gui_robot_idx], x0, scenes[gui_robot_idx].xlim, scenes[gui_robot_idx].ylim, + controller=controllers[gui_robot_idx], robot_color=ctrl_colors[gui_robot_idx], + reference_color='y', + theta_pos_color='None', + # obstacles_star_color='None', + # pg_color='None', + workspace_rho_color='tab:blue', + # indicate_sbc=False, + mpc_artificial_path_color='None', + s1_pos_color='None', + # mpc_path_color='None', + # mpc_tunnel_color='None', + # receding_path_color='None', + travelled_path_color=ctrl_colors[gui_robot_idx] + ) + +for i in range(N): + if i == gui_robot_idx: + continue + scenes[i], _, controllers[i], _ = load_config(ctrl_param_file=ctrl_param_files[i], robot_type_id=3, scene_id=scene_id, verbosity=verbosity) + robot_handles[i], _ = robot.draw(x0, ax=gui.ax, markersize=10, color=ctrl_colors[i], alpha=1) + travelled_path_handles[i] = gui.ax.plot([], [], c=ctrl_colors[i], ls=travelled_path_linestyle, lw=travelled_path_linewidth, zorder=0)[0] + +# Initialize simulation +ctrl_sim_dt_ratio = int(controllers[0].params['dt'] / dt) +gui_sim_dt_ratio = int(dt_gui / dt) +k = 0 +tmp_hs = [] +xs = np.tile(x0, (N, 1)) +travelled_path = xs[:, :2] +converged = [False] * N +collision = False +workspaces = scenes[0].workspace if isinstance(scenes[0].workspace, list) else [scenes[0].workspace] +active_ws_idcs = [0] * N + +# Init video writing +if make_video: + video_name = input("Video file name: ") + video_writer = VideoWriter(video_name, 1/dt_gui) + gui.paused = False +else: + # Init gui plot + gui.update(xs[gui_robot_idx, :], k*dt) + plt.pause(0.005) + + +while gui.fig_open and k*dt <= T_max and not all(converged) and not collision: + if gui.paused and not gui.step_once: + gui.fig.waitforbuttonpress() + + # Move obstacles + for i in range(N): + scenes[i].step(dt, robot.h(xs[i, :])) + + # Control update + if k % ctrl_sim_dt_ratio == 0: + gui.step_once = False + + for i in range(N): + # Check current workspace + if active_ws_idcs[i] + 1 < len(workspaces): + ws_smaller = workspaces[active_ws_idcs[i] + 1].dilated_obstacle(-0.1, id='temp') + if controllers[i].rhrp_path is not None and all( + [ws_smaller.interior_point(r) for r in controllers[i].rhrp_path]): + active_ws_idcs[i] += 1 + if len(scenes[i].reference_path) == 1: + controllers[i].set_reference_path([scenes[i].ws_attractors[active_ws_idcs[i]]]) + # Update control policy + controllers[i].update_policy(xs[i, :], scenes[i].obstacles, workspace=workspaces[active_ws_idcs[i]]) + + # GUI update + if k % gui_sim_dt_ratio == 0: + [h.remove() for h in tmp_hs if not None] + tmp_hs = [] + if k % ctrl_sim_dt_ratio == 0: + gui.update(xs[gui_robot_idx, :], k*dt, controllers[gui_robot_idx], controllers[gui_robot_idx].compute_u(xs[gui_robot_idx, :])) + else: + gui.update(xs[gui_robot_idx, :], k*dt) + for i in range(N): + if i == gui_robot_idx: + continue + robot.update_plot(xs[i, :], robot_handles[i]) + travelled_path_handles[i].set_data(travelled_path[i, ::2], travelled_path[i, 1::2]) + + if make_video: + video_writer.add_frame(gui.fig) + print("[VideoWriter] wrote frame at time {:.2f}/{:.2f}".format(k * dt, T_max)) + else: + plt.pause(0.005) + + # Robot integration + for i in range(N): + if converged[i]: + u = np.zeros(robot.nu) + else: + u = controllers[i].compute_u(xs[i, :]) + xs[i, :], _ = robot.move(xs[i, :], u, dt) + travelled_path = np.hstack((travelled_path, xs[:, :2])) + + # Convergence and collision check + for i in range(N): + converged[i] = controllers[i].theta >= controllers[i].theta_g and np.linalg.norm(robot.h(xs[i, :])-scenes[i].reference_path[-1]) < controllers[i].params['convergence_tolerance'] + if any([o.interior_point(robot.h(xs[i, :])) for o in scenes[i].obstacles]): + collision = True + print("Collision") + + k += 1 + + +if make_video: + # close video writer + video_writer.release() + print("Finished") + fig_open = False +else: + gui.update(xs[gui_robot_idx, :], k*dt) + gui.ax.set_title("Time: {:.1f} s. Finished".format(k*dt)) + + # Wait until figure closed when converged + plt.show() diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py new file mode 100644 index 0000000..18a98a5 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py @@ -0,0 +1,190 @@ +import numpy as np +import matplotlib.pyplot as plt +from config.load_config import load_config +from visualization import PFMPCGUI, VideoWriter +from motion_control.pfmpc_ds import pol2pos +import shapely + +# ------------------------------- # + +make_video = 0 + +# Scenario settings +ctrl_param_file = 'pfmpc_ds_params.yaml' +robot_type_id = 1 +scene_id = 22 +verbosity = 0 + +# Simulation settings +T_max = 200 +dt = 0.01 +dt_gui = 0.1 + +# GUI settings +gui_robot_idx = 3 # GUI environment is shown for robot corresponding to this x0 idx +show_mpc_tunnel = 0 # 0: Don't show. 1: Show for gui robot. 2: Show for all robots. +show_mpc_sol = 0 # 0: Don't show. 1: Show for gui robot. 2: Show for all robots. +show_travelled_path = 2 # 0: Don't show. 1: Show for gui robot. 2: Show for all robots. + +gui_robot_color, robot_color = 'tab:orange', 'k' +gui_robot_color, robot_color = 'k', 'k' +travelled_path_color, travelled_path_linestyle, travelled_path_linewidth = 'k', '--', 2 +mpc_sol_color, mpc_sol_linestyle, mpc_sol_linewidth = 'tab:orange', '-', 1 +mpc_tunnel_color, mpc_tunnel_linestyle, mpc_tunnel_linewidth = 'r', '--', 1 +rhrp_color, rhrp_linestyle, rhrp_linewidth = 'g', '-', 2 + +# ------------------------------- # + +# dict {scene_id: x0s} +scene_x0_mapping = {8: [[6, 0, np.pi/2], [3, 0, np.pi/2], [8, 0, np.pi/2], [4, -2, np.pi/2], [8, 2, np.pi/2]], + 10: [[-2.5, 1.5, 0.], [2.4, -5.6, 0.], [3, .5, 0.], [2, 2.5, 0.], [-5, 1.5, 0.], [-2.8, -3.8, 0.]], + 12: [[-1, 5, 0], [-4, -5.7, 0], [7, 0.25, 0], [1.2, 1.2, 0], [-3, 3, 0], [3.5, 3.5, 0]], + 13: [[0, -5, np.pi/2], [-2, -5, np.pi/2], [2, -5, np.pi/2], [-4, -5, np.pi/2], [4, -5, np.pi/2]], + 22: [[0, -5, np.pi/2], [-2, -5, np.pi/2], [2, -5, np.pi/2], [-4, -5, np.pi/2], [4, -5, np.pi/2]]} +x0s = np.array(scene_x0_mapping[scene_id]) + +if robot_type_id == 0: + x0s = x0s[:, :2] + +N = x0s.shape[0] + +travelled_path_handles = [[]] * N +robot_handles = [[]] * N +controllers = [[]] * N +scenes = [[]] * N + +assert gui_robot_idx < N +scenes[gui_robot_idx], robot, controllers[gui_robot_idx], _ = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity) +if scene_id == 8: # Special fix to show all initial positions in this scenario + scenes[gui_robot_idx].xlim[1] += 2 +if show_mpc_tunnel == 0: + mpc_tunnel_color, rhrp_color = 'None', 'None' +if show_mpc_sol == 0: + mpc_sol_color = 'None' +if show_travelled_path == 0: + travelled_path_color = 'None' +gui = PFMPCGUI(robot, scenes[gui_robot_idx], x0s[gui_robot_idx], scenes[gui_robot_idx].xlim, scenes[gui_robot_idx].ylim, + controller=controllers[gui_robot_idx], robot_alpha=1., robot_color=gui_robot_color, robot_markersize=10, + reference_color='y', reference_marker='*', reference_markersize=10, + pg_color='y', pg_markersize=10, pg_marker='*', + theta_pos_color='c', theta_pos_marker='o', theta_pos_markersize=2, + obstacles_star_alpha=0.2, obstacles_star_show_reference=0, obstacles_star_color='b', + workspace_rho_color='b', workspace_rho_alpha=0.2, indicate_sbc=0, + s1_pos_color='None', + mpc_path_color=mpc_sol_color, mpc_path_linestyle=mpc_sol_linestyle, mpc_path_linewidth=mpc_sol_linewidth, + mpc_tunnel_color=mpc_tunnel_color, mpc_tunnel_linestyle=mpc_tunnel_linestyle, mpc_tunnel_linewidth=mpc_tunnel_linewidth, + travelled_path_color=travelled_path_color, travelled_path_linestyle=travelled_path_linestyle, travelled_path_linewidth=travelled_path_linewidth, + receding_path_color=rhrp_color, receding_path_linestyle=rhrp_linestyle, receding_path_linewidth=rhrp_linewidth, + show_time=1, show_timing=0, show_axis=0 + ) + +for i in range(N): + if i == gui_robot_idx: + continue + scenes[i], _, controllers[i], _ = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity) + robot_handles[i], _ = robot.draw(x0s[i, :], ax=gui.ax, markersize=10, color=robot_color, alpha=1) + travelled_path_handles[i] = gui.ax.plot([], [], c=travelled_path_color, ls=travelled_path_linestyle, lw=travelled_path_linewidth, zorder=0)[0] + +# Initialize simulation +ctrl_sim_dt_ratio = int(controllers[0].params['dt'] / dt) +gui_sim_dt_ratio = int(dt_gui / dt) +k = 0 +tmp_hs = [] +travelled_path = x0s[:, :2] +xs = x0s.copy() +converged = [False] * N +collision = False + +# Init video writing +if make_video: + video_name = input("Video file name: ") + video_writer = VideoWriter(video_name, 1/dt_gui) + gui.paused = False + frame_cntr = 0 +else: + # Init gui plot + gui.update(xs[gui_robot_idx, :], k*dt) + plt.pause(0.005) + + +while gui.fig_open and k*dt <= T_max and not all(converged) and not collision: + if gui.paused and not gui.step_once: + gui.fig.waitforbuttonpress() + + # Move obstacles + for i in range(N): + scenes[i].step(dt, robot.h(xs[i, :])) + + # Control update + if k % ctrl_sim_dt_ratio == 0: + gui.step_once = False + for i in range(N): + controllers[i].update_policy(xs[i, :], scenes[i].obstacles, workspace=scenes[i].workspace) + + + # GUI update + if k % gui_sim_dt_ratio == 0: + [h.remove() for h in tmp_hs if not None] + tmp_hs = [] + if k % ctrl_sim_dt_ratio == 0: + gui.update(xs[gui_robot_idx, :], k*dt, controllers[gui_robot_idx], controllers[gui_robot_idx].compute_u(xs[gui_robot_idx, :])) + else: + gui.update(xs[gui_robot_idx, :], k*dt) + for i in range(N): + if i == gui_robot_idx: + continue + robot.update_plot(xs[i, :], robot_handles[i]) + travelled_path_handles[i].set_data(travelled_path[i, ::2], travelled_path[i, 1::2]) + if show_mpc_tunnel == 2 or show_mpc_sol == 2: + for i in range(N): + if i == gui_robot_idx: + continue + c = controllers[i] + x_sol, s_sol, e_sol = c.mpc.sol2state(c.solution, xs[i, :], c.path_pol) + if show_mpc_tunnel == 2: + e_max = c.rho - c.epsilon + rhrp = np.array([pol2pos(c.path_pol, s, c.mpc.build_params['n_pol']) for s in s_sol]) + tunnel_polygon = shapely.geometry.LineString(list(zip(rhrp[:, 0], rhrp[:, 1]))).buffer(e_max) + tmp_hs += gui.ax.plot(rhrp[:, 0], rhrp[:, 1], c=rhrp_color, ls=rhrp_linestyle, lw=rhrp_linewidth) + tmp_hs += gui.ax.plot(*tunnel_polygon.exterior.xy, c=mpc_tunnel_color, ls=mpc_tunnel_linestyle, lw=mpc_tunnel_linewidth) + if show_mpc_sol == 2: + mpc_path = np.array([robot.h(x_sol[k * robot.nx:(k + 1) * robot.nx]) for k in range(c.params['N'] + 1)]) + tmp_hs += gui.ax.plot(*mpc_path.T, c=mpc_sol_color, ls=mpc_sol_linestyle, lw=mpc_sol_linewidth) + + if make_video: + video_writer.add_frame(gui.fig, frame_cntr) + frame_cntr += 1 + print("[VideoWriter] wrote frame at time {:.2f}/{:.2f}".format(k * dt, T_max)) + else: + plt.pause(0.005) + + # Robot integration + for i in range(N): + if converged[i]: + u = np.zeros(robot.nu) + else: + u = controllers[i].compute_u(xs[i, :]) + xs[i, :], _ = robot.move(xs[i, :], u, dt) + travelled_path = np.hstack((travelled_path, xs[:, :2])) + + # Convergence and collision check + for i in range(N): + converged[i] = controllers[i].theta >= controllers[i].theta_g and np.linalg.norm(robot.h(xs[i, :])-scenes[i].reference_path[-1]) < controllers[i].params['convergence_tolerance'] + if any([o.interior_point(robot.h(xs[i, :])) for o in scenes[i].obstacles]): + collision = True + print("Collision") + + k += 1 + + +if make_video: + # close video writer + video_writer.release() + print("Finished") + fig_open = False +else: + gui.update(xs[gui_robot_idx, :], k*dt) + gui.ax.set_title("Time: {:.1f} s. Finished".format(k*dt)) + + # Wait until figure closed when converged + plt.show() diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py b/python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py new file mode 100755 index 0000000..34cc437 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py @@ -0,0 +1,161 @@ +import numpy as np +import matplotlib.pyplot as plt +from config.load_config import load_config +from motion_control.soads import draw_vector_field, compute_weights + +ctrl_param_file = 'soads_ctrl_params.yaml' +scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=0, verbosity=3) + +# Initialize +T_max = 30 +dt = controller.params['dt'] +K = int(T_max / dt) +u = np.zeros((robot.nu, K)) +x = np.zeros((x0.size, K+1)) +x[:, 0] = x0 +pg = scene.reference_path[-1] +timing_history = {'obstacle': [], 'control': []} +previous_path = [] +convergence_threshold = 0.01 +converged = False +paused = True +step_once = False +i = 0 + +# Init plotting +fig_scene, ax_scene = plt.subplots() +ax_scene.set_xlabel('x1 [m]', fontsize=15) +ax_scene.set_ylabel('x2 [m]', fontsize=15) +travelled_path_handle = ax_scene.plot([], [], 'k-', linewidth=2)[0] +goal_handle = ax_scene.plot(*pg, 'g*', markersize=16)[0] +obstacle_handles, _ = scene.init_plot(ax=ax_scene, draw_p0=0, draw_ref=0, show_obs_name=1) +scene.update_plot(obstacle_handles) +robot.width = 0 +robot_handles, _ = robot.init_plot(ax=ax_scene, color='y', alpha=1, markersize=16) +robot.update_plot(x0, robot_handles) +obstacle_star_handles = [] +color_list = plt.cm.gist_ncar(np.linspace(0, 1, len(scene.obstacles))) + +streamplot_handle = None + +fig_open = True + +def on_close(event): + global fig_open + fig_open = False + +def on_press(event): + global paused, step_once, streamplot_handle + if event.key == ' ': + paused = not paused + elif event.key == 'right': + step_once = True + paused = True + elif event.key == 't': + fig_timing, ax = plt.subplots() + ax.plot(timing_history['obstacle'], '-o', label='obstacle') + ax.plot(timing_history['control'], '-o', label='control') + fig_timing.canvas.draw() + elif event.key == 'w': + ax_scene.axis('off') + ax_scene.title.set_visible(False) + file_name = input("-------------\nFile name: ") + fig_scene.savefig("utils/" + file_name, transparent=True) + ax_scene.axis('on') + ax_scene.title.set_visible(True) + elif event.key == 'a': + n = int(input("-------------\nStreamplot resolution: ")) + streamplot_handle = draw_vector_field(pg, controller.obstacles_star, ax_scene, workspace=scene.workspace, n=n, color='orange') + else: + print(event.key) + +def remove_local_plots(): + global streamplot_handle + # Remove streamplot + if streamplot_handle is not None: + from matplotlib.patches import FancyArrowPatch + streamplot_handle.lines.remove() + for art in ax_scene.get_children(): + if not isinstance(art, FancyArrowPatch): + continue + art.remove() # Method 1 + streamplot_handle = None + +fig_scene.canvas.mpl_connect('close_event', on_close) +fig_scene.canvas.mpl_connect('key_press_event', on_press) + +ls = [] + +while fig_open and not converged: + + if i < K and (not paused or step_once): + p = robot.h(x[:, i]) + step_once = False + # Move obstacles + scene.step(dt, p) + # Compute mpc + u[:, i] = controller.compute_u(x[:, i], pg, scene.obstacles, workspace=scene.workspace) + # Integrate robot state with new control signal + x[:, i+1], _ = robot.move(x[:, i], u[:, i], dt) + # Add timing to history + for k in timing_history.keys(): + timing_history[k] += [controller.timing[k]] + + # Update plots + robot.update_plot(x[:, i], robot_handles) + scene.update_plot(obstacle_handles) + [h.remove() for h in obstacle_star_handles if h is not None] + obstacle_star_handles = [] + for j, cl in enumerate(controller.obstacle_clusters): + for o in cl.obstacles: + lh, _ = o.draw(ax=ax_scene, fc='lightgrey', show_reference=False) + obstacle_star_handles += lh + for o in controller.obstacles_star: + lh, _ = o.draw(ax=ax_scene, fc='red', show_reference=False, alpha=0.9, zorder=0) + obstacle_star_handles += lh + lh = ax_scene.plot(*o.xr(), 'gd', markersize=8) + obstacle_star_handles += lh + + # obstacle_star_handles += [ax_scene.quiver(*scene.boundary.boundary_mapping(x[:, i]), *scene.boundary.normal(x[:, i]))] + + gamma = [obs.distance_function(p) for obs in controller.obstacles_star] + w = compute_weights(gamma) + + if ls: + [l.remove() for l in ls] + ls = [] + for j, o in enumerate(controller.obstacles_star): + ls += [ax_scene.text(*o.xr(), "{:.2f}".format(w[j]))] + if o.reference_direction(p).dot(pg-p) > 0 and o.normal(p).dot(pg-p) > 0: + ls += ax_scene.plot(*o.xr(), 'r+', ms=12, zorder=10) + else: + ls += ax_scene.plot(*o.xr(), 'k+', ms=12, zorder=10) + # b = o.boundary_mapping(x[:, i]) + # dx_o = f(x[:, i], pg, [o]) + # ls += ax_scene.plot(*b, 'yx') + # ls += [ax_scene.quiver(*b, *dx_o, zorder=3, color='y')] + + travelled_path_handle.set_data(x[0, :i+1], x[1, :i+1]) + + ax_scene.set_title("Time: {:.1f} s".format(i*dt)) + + i += 1 + + fig_scene.canvas.draw() + + converged = np.linalg.norm(robot.h(x[:, i])-pg) < convergence_threshold + + if i == K or converged: + ax_scene.set_title("Time: {:.1f} s. Finished".format(i * dt)) + fig_scene.canvas.draw() + + plt.pause(0.005) + + +ot = timing_history['obstacle'] +print("Timing\n-----\nMean: {:.2f}\nMax: {:.2f}\nStdDev: {:.2f}".format(np.mean(ot), np.max(ot), np.std(ot))) +plt.figure() +plt.plot(ot) + +# Wait until figure closed when converged +plt.show() diff --git a/python/ur_simple_control/path_generation/star_navigation/setup.py b/python/ur_simple_control/path_generation/star_navigation/setup.py new file mode 100644 index 0000000..38cc882 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/setup.py @@ -0,0 +1,19 @@ +from setuptools import setup, find_packages + +setup(name='star_navigation', + version='1.0', + packages=find_packages(), + # install_requires=[ + # 'pyyaml', + # 'numpy', + # 'scipy', + # 'matplotlib', + # 'shapely', + # # 'casadi', + # 'opengen', + # #'opencv-python', + # #'cargo', + # #'rust', + # # 'Tkinter' + # ] +) diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO new file mode 100644 index 0000000..f45c709 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO @@ -0,0 +1,3 @@ +Metadata-Version: 2.1 +Name: star_navigation +Version: 1.0 diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt new file mode 100644 index 0000000..a108d48 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt @@ -0,0 +1,51 @@ +.gitignore +.gitmodules +README.md +setup.py +config/__init__.py +config/load_config.py +config/scene.py +config/params/dmp_ctrl_params.yaml +config/params/pfmpc_artificial_reference_params.yaml +config/params/pfmpc_ds_params.yaml +config/params/pfmpc_obstacle_constraints_params.yaml +config/params/robot_params.yaml +config/params/soads_ctrl_params.yaml +motion_control/__init__.py +motion_control/dmp/__init__.py +motion_control/dmp/dmp.py +motion_control/pfmpc_artificial_reference/__init__.py +motion_control/pfmpc_artificial_reference/motion_controller.py +motion_control/pfmpc_artificial_reference/mpc.py +motion_control/pfmpc_artificial_reference/mpc_build/.gitignore +motion_control/pfmpc_ds/__init__.py +motion_control/pfmpc_ds/motion_controller.py +motion_control/pfmpc_ds/mpc.py +motion_control/pfmpc_ds/path_generator.py +motion_control/pfmpc_ds/workspace_modification.py +motion_control/pfmpc_ds/mpc_build/.gitignore +motion_control/pfmpc_obstacle_constraints/__init__.py +motion_control/pfmpc_obstacle_constraints/motion_controller.py +motion_control/pfmpc_obstacle_constraints/mpc.py +motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore +motion_control/soads/__init__.py +motion_control/soads/soads.py +robot/__init__.py +robot/bicycle.py +robot/mobile_robot.py +robot/omnidirectional.py +robot/unicycle.py +scripts/compute_time.py +scripts/run_simulation.py +scripts/simulate_several_controllers.py +scripts/simulate_several_initpos.py +scripts/test_soads.py +star_navigation.egg-info/PKG-INFO +star_navigation.egg-info/SOURCES.txt +star_navigation.egg-info/dependency_links.txt +star_navigation.egg-info/top_level.txt +visualization/__init__.py +visualization/scene_gui.py +visualization/video_writer.py +visualization/figures/.gitignore +visualization/videos/.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt new file mode 100644 index 0000000..3242cb5 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt @@ -0,0 +1,4 @@ +config +motion_control +robot +visualization diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py b/python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py new file mode 100644 index 0000000..9d7c854 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py @@ -0,0 +1,2 @@ +from .scene_gui import SceneGUI, StarobsGUI, PFMPCGUI +from .video_writer import VideoWriter \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore b/python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py b/python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py new file mode 100644 index 0000000..dad824a --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py @@ -0,0 +1,383 @@ +import matplotlib.pyplot as plt +import shapely +import numpy as np +import pathlib +from motion_control.pfmpc_ds import MotionController as PFMPC_ds +from motion_control.pfmpc_ds import ControlMode +from motion_control.pfmpc_obstacle_constraints import MotionController as PFMPC_obstacle_constraints +from motion_control.pfmpc_artificial_reference import MotionController as PFMPC_artificial_reference +from motion_control.pfmpc_artificial_reference import pol2pos +from starworlds.utils.misc import draw_shapely_polygon +from motion_control.soads import draw_vector_field + + +class SceneGUI: + + def __init__(self, robot, scene, xlim, ylim, show_axis=False, + robot_color='k', robot_markersize=14, robot_alpha=0.7, + reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14, + obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False, + travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2): + self.scene = scene + self.robot = robot + self.fig, self.ax = plt.subplots() + if not show_axis: + self.ax.set_axis_off() + self.scene_handles, _ = self.scene.init_plot(self.ax, obstacle_color=obstacle_color, obstacle_edge_color=obstacle_edge_color, show_obs_name=show_obs_name, draw_p0=0, draw_ref=1, reference_color=reference_color, reference_alpha=reference_alpha, reference_marker=reference_marker, reference_markersize=reference_markersize) + self.robot_handles, _ = robot.init_plot(ax=self.ax, color=robot_color, markersize=robot_markersize, alpha=robot_alpha) + self.travelled_path_handle = self.ax.plot([], [], color=travelled_path_color, linestyle=travelled_path_linestyle, linewidth=travelled_path_linewidth, zorder=0)[0] + self.ax.set_xlim(xlim), self.ax.set_ylim(ylim) + # Simulation ctrl + self.fig_open = True + self.paused = True + self.step_once = False + self.draw_vector_field = False + self.fig.canvas.mpl_connect('close_event', self.on_close) + self.fig.canvas.mpl_connect('key_press_event', self.on_press) + # Memory + self.travelled_path = [] + + def update(self, robot_state=None, time=None): + if time: + self.ax.set_title("Time: {:.1f} s".format(time)) + + self.scene.update_plot(self.scene_handles) + # Obstacles + # [oh[0].update_plot(oh[1]) for oh in zip(self.obstacles, self.obstacle_handles)] + + # Robot and goal position + if robot_state is not None: + self.travelled_path += list(self.robot.h(robot_state)) + self.robot.update_plot(robot_state, self.robot_handles) + self.travelled_path_handle.set_data(self.travelled_path[::2], self.travelled_path[1::2]) + + def on_close(self, event): + self.fig_open = False + + def on_press(self, event): + if event.key == ' ': + self.paused = not self.paused + elif event.key == 'right': + self.step_once = True + self.paused = True + # elif event.key == 'f': + # if len(self.scene.reference_path) > 1: + # print("Vector field is only shown for setpoint setup!") + # return + # self.paused = True + # self.step_once = True + # self.draw_vector_field = True + # draw_vector_field(self.scene.reference_path, controller.obstacles_star, gui.ax, workspace=controller.workspace_rho, n=200, + # color='orange', zorder=-3) + elif event.key == 'w': + fig_name = input("Figure file name: ") + self.save_fig(fig_name) + else: + print(event.key) + + def save_fig(self, name): + figures_path = pathlib.PurePath(__file__).parents[0].joinpath("figures") + title = self.ax.get_title() + self.ax.set_title("") + self.fig.savefig(str(figures_path.joinpath(name + ".png")), bbox_inches='tight') + self.ax.set_title(title) + + +class StarobsGUI(SceneGUI): + def __init__(self, robot, scene, init_robot_state, xlim, ylim, show_axis=False, + robot_color='k', robot_markersize=14, robot_alpha=0.7, + obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False, + reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14, + travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2, + obstacles_star_color='r', obstacles_star_show_reference=True, obstacles_star_alpha=0.1, + show_time = True, show_timing = False): + self.show_timing = show_timing + self.show_time = show_time + self.obstacle_star_handles = [] + self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference, + 'alpha': obstacles_star_alpha, 'zorder': 0} + # super().__init__(robot, scene, xlim, ylim, show_axis, robot_color, robot_markersize, + # robot_alpha, obstacle_color, obstacle_edge_color, show_obs_name, goal_color, goal_marker, + # goal_markersize, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth) + super().__init__(robot, scene, xlim, ylim, show_axis, robot_color, robot_markersize, + robot_alpha, reference_color, reference_alpha, reference_marker, reference_markersize, + obstacle_color, obstacle_edge_color, show_obs_name, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth) + + super().update(init_robot_state, 0) + + def update(self, robot_state=None, time=None, controller=None, u=None): + # SceneFig update + super().update(robot_state) + + title = "Time: {:.1f} s".format(time) if self.show_time else "" + # if self.show_timing: + # if controller is None: + # prev_title = self.ax.get_title() + # time_timing = prev_title.split('\n') + # title += "\n" + time_timing[-1] + # else: + # title += "\nCompute Time ({:.1f} / {:.1f} / {:.1f})".format(controller.timing['workspace'], + # controller.timing['target'], + # controller.timing['mpc']) + self.ax.set_title(title) + + if controller is None: + return + + # Star obstacles + [h.remove() for h in self.obstacle_star_handles if h is not None] + self.obstacle_star_handles = [] + if controller.obstacles_star is not None: + for o in controller.obstacles_star: + lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options) + self.obstacle_star_handles += lh + +class PFMPCGUI(SceneGUI): + + def __init__(self, robot, scene, init_robot_state, xlim, ylim, show_axis=False, + robot_color='c', robot_markersize=10, robot_alpha=1, + obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False, + reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=10, + travelled_path_color='k', travelled_path_linestyle='--', travelled_path_linewidth=2, + theta_pos_color='y', theta_pos_marker='*', theta_pos_markersize=10, + s1_pos_color='None', s1_pos_marker='+', s1_pos_markersize=10, + pg_color='b', pg_marker='*', pg_markersize=10, + receding_path_color='g', receding_path_linestyle='-', receding_path_linewidth=1, receding_path_marker=None, + mpc_path_color='k', mpc_path_linestyle='-', mpc_path_linewidth=1, + base_sol_color='None', base_sol_linestyle=':', base_sol_linewidth=1, + mpc_artificial_path_color='y', mpc_artificial_path_linestyle='-.', mpc_artificial_path_linewidth=4, + mpc_tunnel_color='r', mpc_tunnel_linestyle='--', mpc_tunnel_linewidth=1, mpc_tunnel_alpha=1, + obstacles_star_color='b', obstacles_star_show_reference=False, obstacles_star_alpha=0.2, + workspace_rho_color='None', workspace_rho_show_reference=False, workspace_rho_alpha=0.2, + workspace_horizon_color='tab:blue', workspace_horizon_linestyle='--', workspace_horizon_linewidth=1, + show_u_history=False, u_history_color='k', u_history_marker='o', + show_mpc_solution=False, show_mpc_cost=False, controller=None, + indicate_sbc=True, sbc_color='r', + show_time=True, show_timing=False): + self.show_timing = show_timing + self.show_time = show_time + self.indicate_sbc = indicate_sbc + self.robot_color, self.sbc_color = robot_color, sbc_color + self.obstacle_star_handles = [] + self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference, + 'alpha': obstacles_star_alpha, 'zorder': 0} + self.workspace_rho_draw_options = {'ec': workspace_rho_color, 'fc': 'None', 'ls': '--', + 'show_reference': workspace_rho_show_reference, + 'alpha': workspace_rho_alpha, 'zorder': 0} + super().__init__(robot, scene, xlim, ylim, show_axis, robot_color, robot_markersize, + robot_alpha, reference_color, reference_alpha, reference_marker, reference_markersize, obstacle_color, obstacle_edge_color, show_obs_name, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth) + self.theta_pos_handle = self.ax.plot([], [], color=theta_pos_color, marker=theta_pos_marker, markersize=theta_pos_markersize, zorder=0)[0] + self.pg_handle = self.ax.plot([], [], color=pg_color, marker=pg_marker, markersize=pg_markersize)[0] + self.receding_path_handle = self.ax.plot([], [], color=receding_path_color, linestyle=receding_path_linestyle, linewidth=receding_path_linewidth, marker=receding_path_marker, zorder=0)[0] + self.mpc_path_handle = self.ax.plot([], [], color=mpc_path_color, linestyle=mpc_path_linestyle, linewidth=mpc_path_linewidth, zorder=0)[0] + self.s1_pos_handle = self.ax.plot([], [], color=s1_pos_color, marker=s1_pos_marker, markersize=s1_pos_markersize, zorder=0)[0] + # Tunnel and base_solution for PFMPC_DS + self.mpc_tunnel_handle = self.ax.plot([], [], color=mpc_tunnel_color, linestyle=mpc_tunnel_linestyle, linewidth=mpc_tunnel_linewidth, alpha=mpc_tunnel_alpha, zorder=0)[0] + self.base_sol_path_handle = self.ax.plot([], [], color=base_sol_color, linestyle=base_sol_linestyle, linewidth=base_sol_linewidth, zorder=0, dashes=(0.8, 0.8))[0] + + self.workspace_horizon_handle = self.ax.plot([], [], color=workspace_horizon_color, linestyle=workspace_horizon_linestyle, linewidth=workspace_horizon_linewidth)[0] + + # Artifical path for PFMPC_artificial + self.mpc_artificial_path_handle = self.ax.plot([], [], color=mpc_artificial_path_color, linestyle=mpc_artificial_path_linestyle, linewidth=mpc_artificial_path_linewidth, zorder=4)[0] + + self.show_mpc_solution = show_mpc_solution + self.show_mpc_cost = show_mpc_cost + self.show_u_history = show_u_history + if show_u_history: + self.fig_u_history, self.ax_u_history = plt.subplots(self.robot.nu, 1) + self.u_history = [] + self.u_history_handles = [] + self.u_infeasible = [] + self.u_infeasible_handles = [] + for i in range(self.robot.nu): + self.u_history_handles += self.ax_u_history[i].plot([], [], color=u_history_color, marker=u_history_marker) + self.u_infeasible_handles += self.ax_u_history[i].plot([], [], color='r', marker=u_history_marker, linestyle="None") + u_span = self.robot.u_max[i] - self.robot.u_min[i] + self.ax_u_history[i].set_ylim([self.robot.u_min[i]-0.1*u_span, self.robot.u_max[i]+0.1*u_span]) + self.ax_u_history[i].plot([0, 1e10], [robot.u_min[i], robot.u_min[i]], 'r--') + self.ax_u_history[i].plot([0, 1e10], [robot.u_max[i], robot.u_max[i]], 'r--') + + if show_mpc_solution: + N = controller.mpc.build_params['N'] + self.fig_mpc_solution, self.ax_mpc_solution = plt.subplots(2, 3) + self.s_handle = self.ax_mpc_solution[0, 0].plot(np.linspace(1, N, N), [None] * N, '-o')[0] + self.lam_rho_handle = self.ax_mpc_solution[0, 0].plot([0, N], [None, None], 'r--')[0] + self.ax_mpc_solution[0, 0].plot([0, N], [N, N], 'r--') + self.ax_mpc_solution[0, 0].set_ylim(0, 1.1*N) + self.ax_mpc_solution[0, 0].set_title('Path coordinate, s') + self.rho_handle = self.ax_mpc_solution[1, 0].plot([0, N], [None, None], 'k--')[0] + self.emax_handle = self.ax_mpc_solution[1, 0].plot([0, N], [None, None], 'r--')[0] + self.e_handle = self.ax_mpc_solution[1, 0].plot(np.linspace(0, N, N+1, '-o'), [None] * (N+1), '-o')[0] + self.mpc_artificial_error_handle = self.ax_mpc_solution[1, 0].plot(np.linspace(0, N, N+1, '-o'), [None] * (N+1), 'g-o')[0] + + self.ax_mpc_solution[1, 0].set_xlim(0, N) + if hasattr(controller, 'rho0'): + self.ax_mpc_solution[1, 0].set_ylim(0, 1.5*controller.params['rho0']) + self.ax_mpc_solution[1, 0].set_title('Tracking error') + # Assumes 2 control signals + self.u1_handle = self.ax_mpc_solution[0, 1].plot(np.linspace(-1, N-1, N+1), [None] * (N + 1), '-o')[0] + self.ax_mpc_solution[0, 1].plot([0, N], [robot.u_min[0], robot.u_min[0]], 'r--') + self.ax_mpc_solution[0, 1].plot([0, N], [robot.u_max[0], robot.u_max[0]], 'r--') + self.ax_mpc_solution[0, 1].set_title('u1') + self.u2_handle = self.ax_mpc_solution[1, 1].plot(np.linspace(-1, N-1, N+1), [None] * (N + 1), '-o')[0] + self.ax_mpc_solution[1, 1].plot([0, N], [robot.u_min[1], robot.u_min[1]], 'r--') + self.ax_mpc_solution[1, 1].plot([0, N], [robot.u_max[1], robot.u_max[1]], 'r--') + self.ax_mpc_solution[1, 1].set_title('u2') + if show_mpc_cost: + # draw the initial pie chart + self.cost_ax_handle = self.ax_mpc_solution[0, 2] + self.cost_u_ax_handle = self.ax_mpc_solution[1, 2] + + plt.figure(self.fig) + + super().update(init_robot_state, 0) + # self.update(init_robot_state, None, 0) + + def update(self, robot_state, time, controller=None, u=None): + # SceneFig update + super().update(robot_state) + + title = "Time: {:.1f} s".format(time) if self.show_time else "" + if self.show_timing: + if controller is None: + prev_title = self.ax.get_title() + time_timing = prev_title.split('\n') + title += "\n" + time_timing[-1] + else: + title += "\nCompute Time ({:.1f} / {:.1f} / {:.1f})".format(controller.timing['workspace'], + controller.timing['target'], + controller.timing['mpc']) + self.ax.set_title(title) + + if controller is None: + return + + if isinstance(controller, PFMPC_ds): + if controller.pg is None: + self.pg_handle.set_data([],[]) + else: + self.pg_handle.set_data(*controller.pg) + # Star environment + [h.remove() for h in self.obstacle_star_handles if h is not None] + self.obstacle_star_handles = [] + for o in controller.obstacles_star: + lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options) + self.obstacle_star_handles += lh + if self.scene.workspace is not None: + # lh, _ = workspace.draw(ax=self.ax, **self.workspace_rho_draw_options) + # self.obstacle_star_handles += lh + # lh, _ = controller.workspace_rho.draw(ax=self.ax, fc='w', zorder=-8, show_reference=0) + lh, _ = controller.workspace_rho.draw(ax=self.ax, **self.workspace_rho_draw_options) + self.obstacle_star_handles += lh + if controller.params['workspace_horizon'] > 0: + self.workspace_horizon_handle.set_data(*shapely.geometry.Point(self.robot.h(robot_state)).buffer(controller.params['workspace_horizon']).exterior.xy) + + if self.indicate_sbc and controller.mode == ControlMode.SBC: + self.robot_handles[0].set_color(self.sbc_color) + else: + self.robot_handles[0].set_color(self.robot_color) + + # SBC base solution + if controller.sol_feasible: + self.base_sol_path_handle.set_data([], []) + else: + x_sol_base, _, _ = controller.mpc.sol2state(controller.mpc.base_solution(robot_state, controller.path_pol, 0), robot_state, controller.path_pol) + base_sol_path = np.array( + [self.robot.h(x_sol_base[k * self.robot.nx:(k + 1) * self.robot.nx]) for k in range(controller.params['N'] + 1)]) + self.base_sol_path_handle.set_data(base_sol_path[:, 0], base_sol_path[:, 1]) + + # RHRP + self.receding_path_handle.set_data(controller.rhrp_path[:, 0], controller.rhrp_path[:, 1]) + if controller.reference_path.geom_type == 'LineString': + self.theta_pos_handle.set_data(*controller.reference_path.interpolate(controller.theta).coords[0]) + + if controller.sol_feasible: + if isinstance(controller, PFMPC_ds): + u_sol, ds_sol = controller.mpc.sol2uds(controller.solution) + x_sol, s_sol, e_sol = controller.mpc.sol2state(controller.solution, robot_state, controller.path_pol) + # Draw tunnel + e_max = controller.rho - controller.epsilon + tunnel_polygon = shapely.geometry.LineString( + list(zip(controller.rhrp_path[:, 0], controller.rhrp_path[:, 1]))).buffer( + e_max) + if tunnel_polygon.geom_type == 'Polygon': + self.mpc_tunnel_handle.set_data(*tunnel_polygon.exterior.xy) + else: + print("[SceneFig]: Tunnel polygon not polygon.") + + if isinstance(controller, PFMPC_obstacle_constraints): + u_sol, ds_sol = controller.mpc.sol2uds(controller.solution) + x_sol, s_sol, e_sol = controller.mpc.sol2state(controller.solution, robot_state, controller.path_pol) + + if isinstance(controller, PFMPC_artificial_reference): + xa0, u_sol, ua_sol, ds_sol = controller.mpc.sol2xa0uuaw(controller.solution) + x_sol, s_sol, xa_sol = controller.mpc.sol2state(controller.solution, robot_state, controller.path_pol) + ref_sol = np.array([pol2pos(controller.path_pol, s, controller.mpc.build_params['n_pol']) for s in s_sol]) + mpc_path = np.array([self.robot.h(x_sol[k * self.robot.nx:(k + 1) * self.robot.nx]) for k in + range(controller.params['N'] + 1)]) + mpc_artificial_path = np.array([self.robot.h(xa_sol[k * self.robot.nx:(k + 1) * self.robot.nx]) + for k in range(controller.params['N'] + 1)]) + e_sol = np.linalg.norm(ref_sol - mpc_path, axis=1) + self.mpc_artificial_path_handle.set_data(mpc_artificial_path[:, 0], mpc_artificial_path[:, 1]) + self.receding_path_handle.set_data(ref_sol[:, 0], ref_sol[:, 1]) + + # print(x_sol) + mpc_path = np.array([self.robot.h(x_sol[k * self.robot.nx:(k + 1) * self.robot.nx]) for k in range(controller.params['N'] + 1)]) + self.mpc_path_handle.set_data(mpc_path[:, 0], mpc_path[:, 1]) + self.s1_pos_handle.set_data(*pol2pos(controller.path_pol, s_sol[1], controller.mpc.build_params['n_pol'])) + + + if self.show_u_history: + self.u_infeasible += u if not controller.sol_feasible else [None] * self.robot.nu + self.u_history += u + for i in range(self.robot.nu): + if len(self.u_history) > 2: + N = len(self.u_history) // self.robot.nu + self.u_history_handles[i].set_data(np.arange(0, N), self.u_history[i::self.robot.nu]) + self.u_infeasible_handles[i].set_data(np.arange(0, N), self.u_infeasible[i::self.robot.nu]) + self.ax_u_history[i].set_xlim([0, N]) + + # MPC solution plot + if self.show_mpc_solution: + sol_color = 'tab:blue' if controller.sol_feasible else 'tab:brown' + self.s_handle.set_ydata(np.array(s_sol[1:]) / (controller.params['dt'] * controller.mpc.build_params['w_max'])) + self.s_handle.set_color(sol_color) + self.e_handle.set_ydata(e_sol) + self.e_handle.set_color(sol_color) + if isinstance(controller, PFMPC_ds): + self.lam_rho_handle.set_ydata(controller.lam_rho * np.ones(2)) + self.emax_handle.set_ydata([e_max, e_max]) + self.rho_handle.set_ydata([controller.rho, controller.rho]) + if isinstance(controller, PFMPC_artificial_reference): + ea_sol = np.linalg.norm(mpc_path - mpc_artificial_path, axis=1) + self.mpc_artificial_error_handle.set_ydata(ea_sol) + self.u1_handle.set_ydata([controller.u_prev[0]] + u_sol[::2]) + self.u2_handle.set_ydata([controller.u_prev[1]] + u_sol[1::2]) + self.u1_handle.set_color(sol_color) + self.u2_handle.set_color(sol_color) + if self.show_mpc_cost: + if isinstance(controller, PFMPC_ds): + cost = controller.mpc.sol2cost(controller.solution, robot_state, controller.path_pol, controller.params, controller.u_prev) + tot_cost = cost['s'] + cost['e'] + cost['u'] + cost['ud'] + print("cost: {:.3f}. s: {:2.0f}%, e: {:2.0f}%, u: {:2.0f}%, ud: {:2.0f}%".format(tot_cost, 100*cost['s']/tot_cost, 100*cost['e']/tot_cost, 100*cost['u']/tot_cost, 100*cost['ud']/tot_cost)) + + if isinstance(controller, PFMPC_artificial_reference): + + o_par = controller.extract_obs_par(self.obstacles).copy() + # Add safety margin in constraints + for i in range(controller.mpc.build_params['max_No_ell']): + o_par[6 * i + 4] += 0.2 + o_par[6 * i + 5] += 0.2 + print(controller.mpc.sol2cost(controller.solution, robot_state, controller.path_pol, o_par, controller.params)) + # print(x_sol) + # print(xa_sol) + # print(u_sol) + # print(ua_sol) + # print(ds_sol) + # if self.show_mpc_cost and cost is not None: + # cost_dict = dict(list(cost.items())[:-4]) + # u_cost_dict = dict(list(cost.items())[-4:]) + # self.cost_ax_handle.clear() + # self.cost_ax_handle.pie(cost_dict.values(), labels=cost_dict.keys(), wedgeprops=dict(width=0.5)) + # self.cost_u_ax_handle.clear() + # self.cost_u_ax_handle.pie(u_cost_dict.values(), labels=u_cost_dict.keys(), wedgeprops=dict(width=0.5)) diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py b/python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py new file mode 100644 index 0000000..1124cb1 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py @@ -0,0 +1,45 @@ +import matplotlib.pyplot as plt +from matplotlib.backends.backend_agg import FigureCanvasAgg as FigureCanvas +import cv2 +import numpy as np +import pathlib +import os +import subprocess +import glob + +class VideoWriter: + + def __init__(self, file_name, framerate): + self.file_name = file_name + self.video = None + self.framerate = framerate + self.videos_path = str(pathlib.PurePath(pathlib.Path(__file__).parents[0].joinpath("videos"))) + + def add_frame(self, fig, k): + fig.savefig(str(pathlib.PurePath(self.videos_path, self.file_name + '%02d.png' % k))) + return None + + fig.set_size_inches(16, 9) + fig.tight_layout() + canvas = FigureCanvas(fig) + canvas.draw() + mat = np.array(canvas.renderer._renderer) + mat = cv2.cvtColor(mat, cv2.COLOR_RGB2BGR) + if self.video is None: + file_path = str(pathlib.PurePath(pathlib.Path(__file__).parents[0].joinpath("videos"), self.file_name + '.mp4')) + self.video = cv2.VideoWriter(file_path, cv2.VideoWriter_fourcc(*'mp4v'), self.framerate, (mat.shape[1], mat.shape[0])) + + # write frame to video + self.video.write(mat) + + def release(self): + os.chdir(self.videos_path) + subprocess.call([ + 'ffmpeg', '-i', self.file_name + '%02d.png', '-framerate', str(self.framerate), '-pix_fmt', 'yuv420p', + self.file_name+'.mp4' + ]) + for file_name in glob.glob("*.png"): + os.remove(file_name) + return None + cv2.destroyAllWindows() + self.video.release() diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore b/python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc b/python/ur_simple_control/path_generation/starworld_tunnel_mpc deleted file mode 160000 index 94c3d55..0000000 --- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 94c3d55671c9c57bcfc2509d6f8a64f26217861e diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md new file mode 100644 index 0000000..5c5ae97 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md @@ -0,0 +1,22 @@ +# Starworld tunnel-MPC + + +## Installation +Clone repo and initialize submodule +``` +git clone https://github.com/albindgit/starworld_tunnel_mpc.git +cd starworld_tunnel_mpc/starworlds +git submodule init +git submodule update +cd .. +``` +Create and activate virtual environment +``` +python -m venv venv +. venv/bin/activate +``` +Install package +``` +pip install -e . +pip install -e starworlds +``` diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py new file mode 100644 index 0000000..b38df1d --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py @@ -0,0 +1,79 @@ +import yaml +import pathlib +from config.scene import Scene, scene_description +from motion_control.soads import SoadsController +from motion_control.tunnel_mpc import TunnelMpcController +from motion_control.tunnel_mpc_convergence import TunnelMpcController as TunnelMpcControllerConvergence +from robot import Unicycle, Omnidirectional, Bicycle +import numpy as np + + +def load_config(scene_id=None, robot_type_id=None, ctrl_param_file=None, all_convex_obstacles=False, verbosity=0): + # Scene init + if scene_id is None: + print("Select scene ID\n -------------") + for i, description in scene_description.items(): + print(str(i) + ": " + description) + scene_id = int(input("-------------\nScene ID: ")) + scene = Scene(scene_id, all_convex=all_convex_obstacles) + + param_path = pathlib.PurePath(__file__).parents[0].joinpath('params') + + # Load robot + robot_param_path = str(param_path.joinpath('robot_params.yaml')) + with open(r'' + robot_param_path) as stream: + params = yaml.safe_load(stream) + if robot_type_id is None: + print("Select robot ID\n -------------") + for i, k in enumerate(params.keys()): + print(str(i) + ": " + k) + robot_type_id = int(input("-------------\nRobot type: ")) + robot_type = list(params.keys())[robot_type_id] + robot_params = params[robot_type] + if robot_params['model'] == 'Omnidirectional': + robot = Omnidirectional(width=robot_params['width'], + vel_max=robot_params['vel_max'], + name=robot_type) + x0 = scene.p0 + elif robot_params['model'] == 'Unicycle': + robot = Unicycle(width=robot_params['width'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']], + vel_max=[robot_params['lin_vel_max'], robot_params['ang_vel_max']], + name=robot_type) + try: + x0 = np.append(scene.p0, [scene.theta0]) + except AttributeError: + x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])]) + elif robot_params['model'] == 'Bicycle': + robot = Bicycle(width=robot_params['width'], + vel_min=[robot_params['lin_vel_min'], -robot_params['steer_vel_max']], + vel_max=[robot_params['lin_vel_max'], robot_params['steer_vel_max']], + steer_max=robot_params['steer_max'], + name=robot_type) + try: + x0 = np.append(scene.p0, [scene.theta0, 0]) + # x0 = np.append(scene.p0, [scene.theta0]) + except AttributeError: + x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0]), 0]) + # x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])]) + else: + raise Exception("[Load Config]: Invalid robot model.\n" + "\t\t\tSelection: {}\n" + "\t\t\tValid selections: [Omnidirectional, Unicycle, Bicycle]".format(robot_params['model'])) + + # Load control parameters + ctrl_param_path = str(param_path.joinpath(ctrl_param_file)) + # param_file = str(pathlib.PurePath(pathlib.Path(__file__).parent, ctrl_param_file)) + with open(r'' + ctrl_param_path) as stream: + params = yaml.safe_load(stream) + if 'soads' in params: + controller = SoadsController(params['soads'], robot, verbosity) + elif 'tunnel_mpc' in params: + controller = TunnelMpcController(params['tunnel_mpc'], robot, verbosity) + elif 'tunnel_mpc_convergence' in params: + controller = TunnelMpcControllerConvergence(params['tunnel_mpc_convergence'], robot, verbosity) + else: + raise Exception("[Load Config]: No valid controller selection in param file.\n" + "\t\t\tSelection: {}\n" + "\t\t\tValid selections: [soads, tunnel_mpc, tunnel_mpc_convergence]".format(str(list(params.keys())[0]))) + + return scene, robot, controller, x0 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml new file mode 100644 index 0000000..fe1dcbf --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml @@ -0,0 +1,27 @@ + +Omnidirectional: + model: 'Omnidirectional' + width: 0.5 + vel_max: 1.5 + +Unicycle_forward: + model: 'Unicycle' + width: 0.5 + lin_vel_min: 0. + lin_vel_max: 1.5 + ang_vel_max: 1.5 + +Unicycle: + model: 'Unicycle' + width: 0.5 + lin_vel_min: -0.5 + lin_vel_max: 1.5 + ang_vel_max: 1.5 + +Bicycle: + model: 'Bicycle' + width: 0.5 + steer_max: 1.0 + lin_vel_min: -0.5 + lin_vel_max: 1.5 + steer_vel_max: 0.5 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml new file mode 100644 index 0000000..8a902d0 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml @@ -0,0 +1,14 @@ +soads: + dt: 0.1 + starify: 1 + hull_epsilon: 0.5 + max_compute_time: 1000 + make_convex: 0 + adapt_obstacle_velocity: 1 + unit_magnitude: 0 + crep: 1.5 + reactivity: 2 + tail_effect: 0 + convergence_tolerance: 0.01 + lin_vel: 1. + dp_decay_rate: 0.5 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml new file mode 100644 index 0000000..6181186 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml @@ -0,0 +1,39 @@ + +tunnel_mpc_convergence: + # Workspace modification + rho0: 0.3 + gamma: 0.5 + make_convex: 1 + max_obs_compute_time: 40 + hull_epsilon: 0.3 + use_prev_workspace: 1 + # Target generation + convergence_tolerance: 1.e-3 + max_compute_time: 10 + crep: 1.2 + reactivity: 1. + buffer: 1 + # MPC + ce: 100. + cs: 500. + e_penalty: 0 + dg: 0.2 + cg: 10000 + R: [250, 2.5] + convergence_margin: 0.02 + lambda: 0.2 + + #-------- MPC Build params --------- # + build_mode: 'release' + integration_method: 'euler' + n_pol: 10 + N: 5 + dt: 0.2 + solver_tol: 1.e-5 + solver_max_time: 500 # Maximum duration for mpc solver in milliseconds + solver_max_inner_iterations: 1000 + solver_max_outer_iterations: 10 + solver_initial_tol: 1.e-4 + solver_delta_tol: 1.e-4 + solver_weight_update_factor: 10.0 + solver_initial_penalty: 1000.0 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml new file mode 100644 index 0000000..ebc6ba8 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml @@ -0,0 +1,38 @@ + +tunnel_mpc: + # Workspace modification + rho0: 0.3 + gamma: 0.5 + make_convex: 1 + max_obs_compute_time: 40 + hull_epsilon: 0.3 + use_prev_workspace: 1 + # Target generation + convergence_tolerance: 1.e-3 + max_compute_time: 10 + crep: 1. + reactivity: 1. + buffer: 1 + # MPC + ce: 100. + cs: 500. + e_penalty: 0 + dg: 0.2 + cg: 10000 + R: [250, 2.5] + convergence_margin: 0.02 + + #-------- MPC Build params --------- # + build_mode: 'release' + integration_method: 'euler' + n_pol: 10 + N: 5 + dt: 0.2 + solver_tol: 1.e-5 + solver_max_time: 500 # Maximum duration for mpc solver in milliseconds + solver_max_inner_iterations: 1000 + solver_max_outer_iterations: 10 + solver_initial_tol: 1.e-4 + solver_delta_tol: 1.e-4 + solver_weight_update_factor: 10.0 + solver_initial_penalty: 1000.0 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py new file mode 100644 index 0000000..691e919 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py @@ -0,0 +1,555 @@ +import numpy as np +from starworlds.obstacles import Ellipse, StarshapedPolygon, motion_model, Polygon +import matplotlib.pyplot as plt + +scene_description = { + 1: "No obstacles.", + 2: "1 static ellipse.", + 3: "2 polygons. One static, one moving slowly towards, and into, the other.", + 4: "Intersecting static ellipse and polygon. Ellipse moving towards polygon.", + 5: "6 moving ellipses.", + 6: "3 static polygons and 3 moving ellipses.", + 7: "1 static polygon and 2 static ellipses.", + 8: "3 static ellipses.", + 9: "Corridor with moving ellipse into corridor.", + 10: "Crowd.", + 11: "Boundary.", + 12: "Start stop in corridor. Ellipse obstacle." +} + + +class Scene: + + def __init__(self, id=None, all_convex=False, scene_setup=None): + self.id = id + self.obstacles = None + self.p0 = None + self.pg = None + self.xlim = None + self.ylim = None + self._obstacles_to_plot = None + + if id is None: + self.obstacles = scene_setup['obstacles'] + self.p0 = scene_setup['p0'] + self.pg = scene_setup['pg'] + self.xlim = scene_setup['xlim'] + self.ylim = scene_setup['ylim'] + self._obstacles_to_plot = self.obstacles + elif id is not None: + self.init_scene_id(id, all_convex) + else: + print("Bad scene initialization") + + def init_scene_id(self, id, all_convex): + scene_id = 0 + # Scene 1 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + ] + self.p0 = np.array([-5., 0.]) + self.pg = np.array([5., 0.]) + self.xlim = [-10, 10] + self.ylim = [-10, 10] + + # Scene 2 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + Ellipse([1., 1.], motion_model=motion_model.Static(pos=[2., 0.])), + ] + self.p0 = np.array([0., -2.]) + self.pg = np.array([2., 2.]) + self.xlim = [-1, 5] + self.ylim = [-3, 3] + + # Scene 3 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + # Polygon([[3, 0], [3, -4], [5, -4], [5, -2], [8, -2], [8, -4], [10, -4], [10, 0]]), + StarshapedPolygon([[3, 0], [3, -4], [5, -4], [5, -2], [8, -2], [8, 0]]), + StarshapedPolygon([[8, 0], [8, -4], [10, -4], [10, 0]]), + StarshapedPolygon([[6, -3], [6, -7], [7, -7], [7, -3]], motion_model=motion_model.SinusVelocity(pos=[0, -1], x2_mag=0.2)) + ] + self.p0 = np.array([0, -4]) + self.pg = np.array([11, -2]) + self.xlim = [-1, 13] + self.ylim = [-8, 2] + + # Scene 4 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + StarshapedPolygon([[4, 0], [8, 0], [8, 2], [6, 2], [6, 6], [4, 6]]), + Ellipse([1, 1], motion_model=motion_model.Static(pos=[4, -1])), + Ellipse([1, 1], motion_model=motion_model.Interval([8, -3], [(5, [8, -1])])), + # StarshapedPolygon([[1, -1], [1.5, -1], [1.5, 16], [1, 16]]), + # StarshapedPolygon([[2, -1], [3, -1], [3, 16], [2, 16]]), + ] + self.p0 = np.array([3., -4.]) + # self.p0 = np.array([1.75, 0.5]) + # self.theta0 = np.pi/2 + self.pg = np.array([8., 6.]) + self.xlim = [0, 10] + self.ylim = [-5, 8] + + # Scene 5 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, -5], x1_mag=-0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[4, -3], x1_mag=-0.5)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[7, -1], x1_mag=-0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[5, -2], x1_mag=-0.25, x2_mag=-0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[1, -5], x1_mag=0.3, x2_mag=0.5)), + Ellipse([0.4, 1], motion_model=motion_model.SinusVelocity(pos=[8, -3], x2_mag=2, x2_period=5)), + # Ellipse([2, 1], motion_model=motion_model.SinusVelocity(pos=[7, -2], x1_mag=2, x1_period=3)), + Ellipse([0.4,0.4], motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)), + ] + self.p0 = np.array([0., -3.]) + self.pg = np.array([11., -3.]) + self.xlim = [-1, 12] + self.ylim = [-8, 4] + + # n_pol = 10 + # t = np.linspace(0, 2 * np.pi, n_pol, endpoint=False) + # ell_polygon = np.vstack((1 * np.cos(t), 1 * np.sin(t))).T + # ell_polygon2 = np.vstack((0.4 * np.cos(t), 1 * np.sin(t))).T + # ell_polygon3 = np.vstack((0.4 * np.cos(t), 0.4 * np.sin(t))).T + # self.obstacles += [ + # StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[6, -5], x1_mag=-0.2)), + # StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[4, -3], x1_mag=-0.5)), + # StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[7, -1], x1_mag=-0.2)), + # StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[5, -2], x1_mag=-0.25, x2_mag=-0.2)), + # StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[1, -5], x1_mag=0.3, x2_mag=0.5)), + # StarshapedPolygon(ell_polygon2, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[8, -3], x2_mag=2, x2_period=5)), + # StarshapedPolygon(ell_polygon3, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)), + # ] + + # Scene 6 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + # Polygon([(8, 0), (9, 0), (9, 5), (7, 5), (7, 7), (6, 7), (6, 3), (8, 3)], is_convex=False), + StarshapedPolygon([(7, 3), (7, 7), (6, 7), (6, 3)], is_convex=True), + StarshapedPolygon([(11, 4), (7, 4), (7, 3), (11, 3)], is_convex=True), + StarshapedPolygon([(11, 1), (12, 1), (12, 5), (11, 5)], is_convex=True), + # StarshapedPolygon([(8, 5), (7, 5), (7, 3), (8, 3)], is_convex=True), + # StarshapedPolygon([(8, 0), (9, 0), (9, 5), (8, 5)], is_convex=True), + StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], is_convex=True), + # StarshapedPolygon([(6, 11), (2, 11), (2, 10), (6, 10)], is_convex=True), + # Ellipse([1, 1], motion_model=motion_model.Waypoints([12, 3], [(9.8, 5.5), (9.8, 9.5), (7, 9.5), (3, 4)], 1.)), + Ellipse([1, 1], motion_model=motion_model.Waypoints([3, 5.5], [(4, 5.5), (7.3, 9.5), (10, 9.5), (10.3, 4.5)], 1.)), + Ellipse([1, 1], motion_model=motion_model.Interval([5, 8], [(3, (7.6, 7.2))])), + # Ellipse([1, 1], motion_model=motion_model.Interval([9, 10], [(5, (4.5, 10)), (5, (4.5, 4))])) + # Ellipse([1, 1], motion_model=motion_model.Interval([3, 4], [(5, (4., 5)), (5, (4., 9)), (4, (9, 9))])) + Ellipse([1, 1], motion_model=motion_model.Interval([3.3, 10], [(6, (3.3, 6)), (6, (5.5, 6))])) + ] + self.p0 = np.array([13, 6.]) + self.pg = np.array([1., 5.]) + # self.pg = np.array([1., 5.]) + self.xlim = [0, 13] + self.ylim = [-1, 11] + + # Scene 7 + scene_id += 1 + if id == scene_id: + ell_ax = [0.8, 0.8] + self.obstacles = [ + StarshapedPolygon([(8, 5), (7, 5), (7, 3), (8, 3)], is_convex=True), + StarshapedPolygon([(7, 3), (7, 7), (6, 7), (6, 3)], is_convex=True), + StarshapedPolygon([(8, 0), (9, 0), (9, 5), (8, 5)], is_convex=True), + StarshapedPolygon([(3, 6), (4, 6), (4, 10), (3, 10)], is_convex=True), + StarshapedPolygon([(9.5, 6), (10.5, 6), (10.5, 10), (9.5, 10)], is_convex=True), + # Ellipse(ell_ax, motion_model=motion_model.Waypoints([12, 3], [(9.8, 5.5), (9.8, 9.5), (7, 9.5)], 0.8)), + Ellipse(ell_ax, motion_model=motion_model.Static([9, 9])), + Ellipse(ell_ax, motion_model=motion_model.Interval([12, 5], [(2, (11, 6))])), + Ellipse(ell_ax, motion_model=motion_model.Interval([5, 8], [(3, (7.8, 7))])), + # Ellipse(ell_ax, motion_model=motion_model.Interval([3, 4], [(5, (5, 4)), (5, (4.5, 9))])), + Ellipse(ell_ax, motion_model=motion_model.Interval([2, 3], [(3, (4., 6))])), + Ellipse(ell_ax, motion_model=motion_model.SinusVelocity([4.7, 12], x2_mag=-0.2)), + ] + + # n_pol = 10 + # t = np.linspace(0, 2 * np.pi, n_pol, endpoint=False) + # ell_polygon = np.vstack((1 * np.cos(t), 1 * np.sin(t))).T + # self.obstacles += [StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.Waypoints([12, 3], [(9.8, 5.5), (9.8, 9.5), (7, 9.5), (3, 4)], 1.))] + # self.obstacles += [StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.Interval([5, 8], [(3, (7.8, 7))]))] + # self.obstacles += [StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.Interval([3, 4], [(5, (5, 4)), (5, (4.5, 9)), (4, (9, 9))]))] + + self.p0 = np.array([12., 7.]) + self.pg = np.array([1., 4.]) + self.xlim = [0, 13] + self.ylim = [-1, 11] + # self.obstacles = [ + # StarshapedPolygon([(3, 7), (6, 7), (6, 5), (7, 5), (7, 8), (3, 8)]), + # Ellipse([2, 0.5], motion_model=motion_model.Static([5, 4], -np.pi/3)), + # Ellipse([2, 1], motion_model=motion_model.Static([9, 4], np.pi/2)) + # ] + # self.p0 = np.array([1, 6]) + # self.pg = np.array([11, 6]) + # self.xlim = [0, 12] + # self.ylim = [0, 10] + + # Scene 8 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + Ellipse([2, 1], motion_model=motion_model.Static([5, 6], -np.pi/3)), + Ellipse([1.2, 1.2], motion_model=motion_model.Static([4, 4], 0)), + Ellipse([1, 1], motion_model=motion_model.Static([4, 8], 0)), + Ellipse([1, 1], motion_model=motion_model.Static([9, 4], 0)) + ] + self.p0 = np.array([1, 6]) + self.pg = np.array([11, 6]) + self.xlim = [0, 12] + self.ylim = [0, 10] + + # Scene 9 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]), + StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]), + StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]), + Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])), + # StarshapedPolygon(Ellipse([1, 1]).polygon(), motion_model=motion_model.Interval([-1, 4], [(9, (10, 4))])), + # Ellipse([1, 1], motion_model=motion_model.Interval([-2, 4], [(9, (11, 4))])), + ] + # self.p0 = np.array([9, 5]) + self.p0 = np.array([9, 4]) + self.pg = np.array([0.5, 5.5]) + self.xlim = [0, 10] + self.ylim = [2, 9] + + # Scene 10 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + # StarshapedPolygon([[7, 13], [10, 10], [11, 11], [8, 14]]), + # StarshapedPolygon([[10, 11], [10, 7], [11, 7], [11, 11]]), + # StarshapedPolygon([[0, 0], [0, 1], [1, 1], [1, 0]], motion_model=motion_model.Interval([5, 11], [(5, [5, 6]), (2, [8, 6])])), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[12, 9], x1_mag=-0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[15, 8], x1_mag=-0.8)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[10, 11], x1_mag=0.)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[10, 7], x1_mag=0.8)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[12, 11.5], x1_mag=0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[15, 13.5], x2_mag=-0.1)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, 7], x1_mag=0.3)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[17, 5], x2_mag=0.1)), + Ellipse([1, 1], motion_model=motion_model.Interval([16, 4], [(11, [16, 7]), (5., [16, 10])])), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[3, 10], x1_mag=0.3)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[4, 8.5], x1_mag=0.1)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[5, 11.5], x1_mag=-0.1)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[8, 6], x1_mag=-0.1)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[10, 4], x2_mag=0.1)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[11, 3], x2_mag=0.2)), + Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[13, 13.5], x2_mag=-0.2)), + # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, 7], x1_mag=0.3)), + # StarshapedPolygon([[0, 0], [0, 1], [1, 1], [1, 0]], motion_model=motion_model.Interval([14, 3], [(5, [9.5, 6.5])])), + # Ellipse([1, 1], motion_model=motion_model.Interval([6, -5], [])), + # Ellipse([1, 1], motion_model=motion_model.Interval([4, -3], [])), + # Ellipse([1, 1], motion_model=motion_model.Interval([7, -1], [])), + # Ellipse([1, 1], motion_model=motion_model.Interval([5, -2], x1_mag=-0.25, x2_mag=-0.2)), + # Ellipse([1, 1], motion_model=motion_model.Interval([1, -5], x1_mag=0.3, x2_mag=0.5)), + # Ellipse([0.4, 1], motion_model=motion_model.SinusVelocity(pos=[8, -3], x1_mag=3, x1_period=4, x2_mag=2, + # x2_period=2)), + # Ellipse([2, 1], motion_model=motion_model.SinusVelocity(pos=[7, -2], x1_mag=2, x1_period=3)), + # Ellipse([0.4, 0.4], motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)), + ] + self.p0 = np.array([1., 9.]) + self.pg = np.array([19., 9.]) + self.xlim = [0, 20] + self.ylim = [0, 18] + + # Scene 11 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + # PolynomialObstacle(xys=np.array([[0, 0], [10, 0], [10, 3], [4, 5], [10, 7], [10, 10], [0, 10], [0, 0]], dtype=np.float64) + np.array([-2, -5]), pol_deg=60, is_boundary=True), + # Ellipse([5, 5], is_boundary=True), + Ellipse([1, 1], motion_model=motion_model.Interval([0., 0.5], [(0.5, [1.2, 0.5])])), + Ellipse([1, 1], motion_model=motion_model.Static([0, -0.5])), + Ellipse([1, 1], motion_model=motion_model.Static([0, 1.5])) + ] + self.p0 = np.array([-1.5, 1.5]) + # self.p0 = np.array([5.6, -1.55]) + self.pg = np.array([1.5, -1.5]) + self.xlim = [-3, 3] + self.ylim = [-3, 3] + + # Scene 12 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + StarshapedPolygon([[0, 0], [2, 0], [2, 6], [0, 6]]), + StarshapedPolygon([[2.5, 0], [6, 0], [6, 6], [2.5, 6]]), + StarshapedPolygon([[7, 0], [10, 0], [10, 6], [7, 6]]), + Ellipse([0.5, 0.5], motion_model=motion_model.Interval([4.5, -0.3], [(3, (4.5, -0.3)), (1, (4.5, -2))])) + # StarshapedPolygon([[6.5, 0], [10, 0], [10, 6], [6.5, 6]]), + ] + self.p0 = np.array([2.25, 0.5]) + self.pg = np.array([6.5, 1]) + # self.pg = np.array([6.25, 1]) + self.theta0 = np.pi/2 + self.xlim = [0, 10] + self.ylim = [-4, 2] + + # Scene 13 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 0.7])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3.5, -0.1])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, -0.9])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([7, 0.6])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([8, -0.2])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([4.5, 1.8])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 0.4])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5., -1.4])) + ] + [o.polygon() for o in self.obstacles] + + + self.p0 = np.array([1, 0]) + self.pg = np.array([9, 0]) + # self.pg = np.array([6.25, 1]) + # self.theta0 = np.pi / 2 + self.xlim = [0, 10] + self.ylim = [-5, 5] + + # Scene 14 + scene_id += 1 + if id == scene_id: + n_pol = 20 + self.obstacles = [ + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([3, 0.7], x1_mag=-0.13)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([4, -0.1], x1_mag=-0.5)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([3, -0.9], x1_mag=-0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([7, 0.6], x1_mag=-0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([8, -0.2], x1_mag=-0.3, x2_mag=-0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([6, -0.5], x1_mag=-0.1, x2_mag=0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([5.3, 0.4], x1_mag=-0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([5., -1.4], x2_mag=0.0)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([6.3, 1.2], x1_mag=0.)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([4.2, 1.4], x1_mag=-0.3, x2_mag=-0.2)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([8., 1.2], x1_mag=-0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([8.5, -0.75], x1_mag=-0.1)), + Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([7, -1.25], x1_mag=-0.05)) + ] + # self.obstacles = [StarshapedPolygon(o.polygon(output_frame=Frame.OBSTACLE), motion_model=o._motion_model) for o in self.obstacles] + + self.p0 = np.array([0.9, 0]) + self.p0 = np.array([1.5, 0]) + self.pg = np.array([9., 0]) + # self.pg = np.array([6.25, 1]) + # self.theta0 = np.pi / 2 + self.xlim = [0, 10] + self.ylim = [-5, 5] + + # Scene 15 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + Polygon([[3, 1], [4, 1], [4, 4], [2.5, 4], [2.5, 6], [1.5, 6], [1.5, 4], [0, 4], [0, 3], [3, 3]]), + Polygon([[4, 6], [5, 6], [5, 9], [2, 9], [2, 8], [2, 7.2], [3, 7.2], [3, 8], [4, 8]]), + StarshapedPolygon([[6, 6], [8, 6], [8, 7], [7, 7], [7, 8], [6, 8]]), + Polygon([[10, 8], [10, 2], [6, 2], [6, 4], [7, 4], [7, 3], [9, 3], [9, 8]]), + Ellipse([.4, .4], motion_model=motion_model.Static([5.5, 6.5])), + Ellipse([.4, .4], motion_model=motion_model.Waypoints([8.5, 6.5], [[8.5, 4.5], [4.5, 4.5]], 0.2)), + Ellipse([.4, .4], motion_model=motion_model.Waypoints([5, 4.5], [[5, 1]], 0.2)), + # Ellipse([.6, .6], motion_model=motion_model.Static([2, 6.5])) + ] + self.p0 = np.array([1, 1]) + self.pg = np.array([5.5, 8]) + self.xlim = [0, 10] + self.ylim = [0, 10] + + # Scene 16 + scene_id += 1 + if id == scene_id: + # np.random.seed(2) + No = 30 + ell_radius_mean = 0.5 + ell_radius_std = 0.2 + self.xlim = [0, 10] + self.ylim = [0, 10] + + def random_scene_point(): + return np.array([np.random.rand() * (self.xlim[1]-self.xlim[0]) + self.xlim[0], + np.random.rand() * (self.ylim[1]-self.ylim[0]) + self.ylim[0]]) + + self.obstacles = [ + Ellipse(a=np.random.normal(ell_radius_mean, ell_radius_std, 2)) for _ in range(No) + ] + [o.set_motion_model(motion_model.Static(random_scene_point())) for o in self.obstacles] + self.p0 = random_scene_point() + while any([o.interior_point(self.p0) for o in self.obstacles]): + self.p0 = random_scene_point() + self.pg = random_scene_point() + while any([o.interior_point(self.pg) for o in self.obstacles]): + self.pg = random_scene_point() + + # Scene 17 + scene_id += 1 + if id == scene_id: + self.obstacles = [ + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 5.7])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3.5, 4.9])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 4.1])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([7, 5.6])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([8, 4.8])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([4.5, 6.8])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 5.4])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5., 3.6])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 2.8])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([7., 6.2])), + Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 1.8])) + ] + self.p0 = np.array([1, 5]) + self.pg = np.array([9, 5]) + self.xlim = [0, 10] + self.ylim = [0, 10] + + # Scene 18 + scene_id += 1 + if id == scene_id: + ell_ax = [0.8, 0.8] + self.obstacles = [ + StarshapedPolygon([(7, 5), (6, 5), (6, 3), (7, 3)], is_convex=True), + StarshapedPolygon([(6, 3), (6, 7), (5, 7), (5, 3)], is_convex=True), + StarshapedPolygon([(7, 1), (8, 1), (8, 5), (7, 5)], is_convex=True), + StarshapedPolygon([(6, 7), (6, 8), (4, 8), (4, 7)], is_convex=True), + StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], is_convex=True), + StarshapedPolygon([(3, 9), (3, 10), (6, 10), (6, 9)], is_convex=True), + StarshapedPolygon([(8, 6), (9, 6), (9, 10), (8, 10)], is_convex=True), + Ellipse(ell_ax, motion_model=motion_model.Static([7.5, 8])), + Ellipse(ell_ax, motion_model=motion_model.Static([4.8, 5])), + # Ellipse(ell_ax, motion_model=motion_model.Static([4, 8])), + # Ellipse(ell_ax, motion_model=motion_model.Static([2.5, 5.5])), + Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])), + # Ellipse(ell_ax, motion_model=motion_model.Interval([2, 3], [(3, (4., 6))])), + # Ellipse(ell_ax, motion_model=motion_model.SinusVelocity([4.7, 12], x2_mag=-0.2)), + ] + self.p0 = np.array([1., 1.]) + # self.p0 = np.array([1., 3.]) + # self.p0 = np.array([1., 5.]) + self.pg = np.array([9., 5.]) + self.xlim = [-1, 11] + self.ylim = [-1, 11] + self.theta0 = 0 + + # Scene 19 + scene_id += 1 + if id == scene_id: + ell_ax = [0.8, 0.8] + self.obstacles = [ + StarshapedPolygon([(4, 1), (6, 1), (6, 9), (4, 9)], is_convex=True), + StarshapedPolygon([(1, 4), (9, 4), (9, 6), (1, 6)], is_convex=True), + Ellipse(ell_ax, motion_model=motion_model.Static([6.3, 0.5])), + Ellipse(ell_ax, motion_model=motion_model.Static([1.2, 6.7])), + # Ellipse(ell_ax, motion_model=motion_model.Static([1.7, 8])), + Ellipse(ell_ax, motion_model=motion_model.Static([6, 0])), + Ellipse(ell_ax, motion_model=motion_model.Static([8.6, 6.7])), + # Ellipse(ell_ax, motion_model=motion_model.Static([2.5, 5.5])), + # Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])), + ] + self.p0 = np.array([3, 7.]) + self.p0 = np.array([7, 6.5]) + # self.p0 = np.array([7., 3.]) + self.pg = np.array([3., 3.]) + self.xlim = [-2, 10] + self.ylim = [-2, 10] + self.theta0 = 0 + + # Scene 20 + scene_id += 1 + if id == scene_id: + ell_ax = [0.8, 0.8] + self.obstacles = [ + # StarshapedPolygon([[0, 0], [4, 0], [4, 1], [1, 1], [1, 3], [0, 3]]), + StarshapedPolygon([[0, 0], [1, 0], [1, 3], [0, 3]]), + StarshapedPolygon([[1, 0], [3, 0], [3, 1], [1, 1]]), + StarshapedPolygon([[3, 0], [4, 0], [4, 3], [3, 3]]), + # Polygon([[0, 0], [4, 0], [4, 3], [3, 3], [3, 1], [1, 1], [1, 3], [0, 3]]), + Ellipse([1, 1], motion_model=motion_model.Static([-0.5, 3.7])), + Ellipse([0.5, 1.5], motion_model=motion_model.Static([4, -1.])) + ] + self.p0 = np.array([0, -2.]) + self.pg = np.array([2, 2]) + # self.pg = np.array([4, 4]) + self.xlim = [-3, 6] + self.ylim = [-3, 6] + + if not (0 < id < scene_id+1): + text = 'Invalid scene id: ' + str(id) + '\n\nScene scenarios\n---------\n' + for i, description in scene_description.items(): + text += str(i) + ": " + description + "\n" + raise Exception(text) + + # Make all obstacles convex + self._obstacles_to_plot = self.obstacles + if all_convex: + convex_obstacles = [] + for o in self.obstacles: + if o.is_convex(): + convex_obstacles += [o] + else: + pol = py2dPol.from_tuples(np.asarray(o.polygon().exterior.coords)[:-1, :]) + conv_pols = py2dPol.convex_decompose(pol) + for cp in conv_pols: + convex_obstacles += [StarshapedPolygon([(v.x, v.y) for v in cp.points], is_convex=True)] + self.obstacles = convex_obstacles + # self._obstacles_to_plot = self.obstacles + + # Compute all polygon + [o.polygon() for o in self.obstacles] + [o.is_convex() for o in self.obstacles] + + def step(self, dt, X_exclude=None, dist=0): + [o.move(dt, X_exclude, dist) for o in self.obstacles] + [o.move(dt, X_exclude, dist) for o in self._obstacles_to_plot] + + def init_plot(self, ax=None, obstacle_color='lightgrey', draw_p0=1, draw_pg=1, show_obs_name=False, draw_streamlines=0): + if ax is None: + _, ax = plt.subplots(subplot_kw={'aspect': 'equal'}) + line_handles = [] + if draw_p0: + ax.plot(*self.p0, 'ks', markersize=10) + if draw_pg: + ax.plot(*self.pg, 'k*', markersize=10) + + # lh = self.obstacles[0].init_plot(ax=ax, fc='None', ec='k', show_reference=False) + # line_handles.append(lh) + for o in self._obstacles_to_plot: + # lh = o.init_plot(ax=ax, fc=obstacle_color, ec='k', show_reference=True) + lh, _ = o.init_plot(ax=ax, fc=obstacle_color, ec='k', show_name=show_obs_name, show_reference=False) + line_handles.append(lh) + ax.set_xlim(self.xlim) + ax.set_ylim(self.ylim) + + if draw_streamlines: + from motion_control.path_generator import plot_vector_field + plot_vector_field(self.pg, self.obstacles, ax, xlim=self.xlim, ylim=self.ylim, n=50) + # line_handles = scene.init_plot() + # scene.update_plot(line_handles) + # ax = plt.gca() + # plot_vector_field(scene.xg, scene.obstacles, ax, xlim=None, ylim=None, n=150) + # x = np.array([6.5, -2]) + # ax.plot(*scene.obstacles[0].boundary_mapping(x), 'o') + # ax.quiver(*x, *f(x, scene.xg, scene.obstacles, unit_magnitude=1)) + # plt.show() + + return line_handles, ax + + def update_plot(self, line_handles): + for i, o in enumerate(self._obstacles_to_plot): + o.update_plot(line_handles[i]) + + def draw(self, ax=None, obstacle_color='lightgrey', draw_p0=1, draw_pg=1, show_obs_name=False, draw_streamlines=0): + lh, ax = self.init_plot(ax, obstacle_color, draw_p0, draw_pg, show_obs_name, draw_streamlines) + self.update_plot(lh) + return lh, ax diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py new file mode 100644 index 0000000..861796e --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py @@ -0,0 +1 @@ +from .soads import f, SoadsController, draw_vector_field \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py new file mode 100644 index 0000000..8ea5bbe --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py @@ -0,0 +1,260 @@ +import numpy as np +from starworlds.starshaped_hull import cluster_and_starify +from starworlds.utils.misc import tic, toc + +class SoadsController: + + def __init__(self, params, robot, verbose=False): + if not robot.__class__.__name__ == 'Omnidirectional': + raise NotImplementedError("SoadsController only implemented for Omnidirectional robots.") + self.params = params + self.robot = robot + self.obstacle_clusters = None + self.obstacles_star = [] + self.dp_prev = None + self.verbose = verbose + self.timing = {'obstacle': 0, 'control': 0} + + def compute_u(self, x, pg, obstacles): + p = self.robot.h(x) + + if self.params['starify']: + self.obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles, p, pg, + self.params['hull_epsilon'], + self.params['max_compute_time'], + previous_clusters=self.obstacle_clusters, + # dx_prev=self.dp_prev, + make_convex=self.params['make_convex'], + verbose=self.verbose) + self.timing['obstacle'] = sum(obstacle_timing) + self.obstacles_star = [cl.cluster_obstacle for cl in self.obstacle_clusters] + + # print(obstacle_timing) + + # self.obstacles_star = [] + # for cl in self.obstacle_clusters: + # cl_obstacles = cl.obstacles + # for i in range(len(cl_obstacles)): + # cl_obstacles[i].set_xr(cl.cluster_obstacle.xr()) + # self.obstacles_star += cl_obstacles + # print(obstacle_timing, n_iter) + # print("(Cl/Adm/Hull) calculation ({:.2f}/{:.2f}/{:.2f}) [{:.0f}]".format(*obstacle_timing[:-1], n_iter)) + + else: + self.timing['obstacle'] += 0 + self.obstacles_star = obstacles + + # if any([o.interior_point(p) for o in self.obstacles_star]): + # import matplotlib.pyplot as plt + # from obstacles.tools import draw_shapely_polygon, is_cw, is_ccw, is_collinear, RayCone + # from starshaped_hull import admissible_kernel + # print("p in obstacles") + # for cl in self.obstacle_clusters: + # _, ax = plt.subplots() + # # cl.cluster_obstacle.draw(ax=ax, fc='r', alpha=0.5) + # for o in cl.obstacles: + # o.draw(ax=ax, show_name=1) + # # if o.id() == 1: + # # ad_ker = admissible_kernel(o, p) + # # # tp1, tp2 = o.tangent_points(p) + # # # if is_ccw(p, tp1, tp2): + # # # print('ccw') + # # # elif is_cw(p, tp1, tp2): + # # # print('cw') + # # # elif is_collinear(p, tp1, tp2): + # # # print('collinear') + # # # ax.plot(*tp1, 'rx') + # # # ax.plot(*tp2, 'rs') + # # draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.3) + # + # ax.plot(*p, 'o') + # ax.set_xlim([0, 13]) + # ax.set_ylim([-1, 11]) + # adm_ker_robot = RayCone.intersection_same_apex([admissible_kernel(o, p) for o in cl.obstacles], debug_info={'o': obstacles, 'x': p, 'xlim': [0, 13], 'ylim': [-1, 11]}) + # if cl.admissible_kernel is not None: + # # draw_shapely_polygon(cl.admissible_kernel, ax=ax, fc='y', alpha=0.3) + # draw_shapely_polygon(adm_ker_robot.polygon(), ax=ax, fc='y', alpha=0.3) + # plt.show() + + t0 = tic() + dist_to_goal = np.linalg.norm(p - pg) + if self.params['lin_vel'] > 0: + lin_vel = min(self.params['lin_vel'], dist_to_goal / self.params['dt']) + unit_mag = True + else: + lin_vel = 1 + unit_mag = False + dp = lin_vel * f(p, pg, self.obstacles_star, unit_magnitude=unit_mag, crep=self.params['crep'], + reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'], + adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'], + convergence_tolerance=self.params['convergence_tolerance']) + + p_next = p + dp * self.params['dt'] + + p_next_in_obstacle = False + scale = 1. + while any([o.interior_point(p_next) for o in self.obstacles_star]): + dp *= self.params['dp_decay_rate'] + p_next = p + dp * self.params['dt'] + # Additional compute time check + if toc(t0) > self.params['max_compute_time']: + if self.verbose: + print("[Max compute time in soads when adjusting for collision. ") + dp *= 0 + break + self.timing['control'] = toc(t0) + + self.dp_prev = dp + + return dp + + +# TODO: Check if can make more computationally efficient +def f(r, rg, obstacles, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False, + convergence_tolerance=1e-4, d=False): + goal_vector = rg - r + goal_dist = np.linalg.norm(goal_vector) + if goal_dist < convergence_tolerance: + return 0 * r + + No = len(obstacles) + fa = goal_vector / goal_dist # Attractor dynamics + if No == 0: + return fa + + ef = [-fa[1], fa[0]] + Rf = np.vstack((fa, ef)).T + + mu = [obs.reference_direction(r) for obs in obstacles] + normal = [obs.normal(r) for obs in obstacles] + gamma = [obs.distance_function(r) for obs in obstacles] + # Compute weights + w = compute_weights(gamma, weightPow=1) + + # Compute obstacle velocities + xd_o = np.zeros((2, No)) + if adapt_obstacle_velocity: + for i, obs in enumerate(obstacles): + xd_o[:, i] = obs.vel_intertial_frame(r) + + kappa = 0. + f_mag = 0. + for i in range(No): + # Compute basis matrix + E = np.zeros((2, 2)) + E[:, 0] = mu[i] + E[:, 1] = [-normal[i][1], normal[i][0]] + # Compute eigenvalues + D = np.zeros((2, 2)) + D[0, 0] = 1 - crep / (gamma[i] ** (1 / reactivity)) if tail_effect or normal[i].dot(fa) < 0. else 1 + D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity) + # Compute modulation + M = E.dot(D).dot(np.linalg.inv(E)) + # f_i = M.dot(fa) + f_i = M.dot(fa - xd_o[:, i]) + xd_o[:, i] + # Compute contribution to velocity magnitude + f_i_abs = np.linalg.norm(f_i) + f_mag += w[i] * f_i_abs + # Compute contribution to velocity direction + nu_i = f_i / f_i_abs + nu_i_hat = Rf.T.dot(nu_i) + kappa_i = np.arccos(np.clip(nu_i_hat[0], -1, 1)) * np.sign(nu_i_hat[1]) + kappa += w[i] * kappa_i + kappa_norm = abs(kappa) + f_o = Rf.dot([np.cos(kappa_norm), np.sin(kappa_norm) / kappa_norm * kappa]) if kappa_norm > 0. else fa + + if unit_magnitude: + f_mag = 1. + return f_mag * f_o + + +def compute_weights( + distMeas, + N=0, + distMeas_lowerLimit=1, + weightType="inverseGamma", + weightPow=2, +): + """Compute weights based on a distance measure (with no upper limit)""" + distMeas = np.array(distMeas) + n_points = distMeas.shape[0] + + critical_points = distMeas <= distMeas_lowerLimit + + if np.sum(critical_points): # at least one + if np.sum(critical_points) == 1: + w = critical_points * 1.0 + return w + else: + # TODO: continuous weighting function + # warnings.warn("Implement continuity of weighting function.") + w = critical_points * 1.0 / np.sum(critical_points) + return w + + distMeas = distMeas - distMeas_lowerLimit + w = (1 / distMeas) ** weightPow + if np.sum(w) == 0: + return w + w = w / np.sum(w) # Normalization + return w + + +def rollout(x, xg, obstacles, step_size=0.01, max_nr_steps=10000, max_distance=np.inf, convergence_threshold=0.05, crep=1., reactivity=1.): + xs = np.zeros((2, max_nr_steps)) + xs[:, 0] = x + dist_travelled = 0 + for k in range(1, max_nr_steps): + dist_to_goal = np.linalg.norm(xs[:, k-1]-xg) + if dist_to_goal < convergence_threshold or dist_travelled > max_distance: + return xs[:, :k] + else: + step_mag = min(step_size, dist_to_goal) + step_dir = f(xs[:, k-1], xg, obstacles, unit_magnitude=1, crep=crep, reactivity=reactivity) + xs[:, k] = xs[:, k-1] + step_mag * step_dir + while any([o.interior_point(xs[:, k]) for o in obstacles]): + step_mag /= 2. + xs[:, k] = xs[:, k-1] + step_mag * step_dir + dist_travelled += step_mag + return xs + + +def draw_streamlines(xg, obstacles, ax, init_pos_list, step_size=0.01, max_nr_steps=10000, max_distance=np.inf, + convergence_threshold=0.05, crep=1., reactivity=1., + arrow_step=0.5, color='k', linewidth=1, **kwargs): + for x0 in init_pos_list: + xs = rollout(x0, xg, obstacles, step_size, max_nr_steps, max_distance, convergence_threshold, crep, reactivity) + travelled_dist = np.cumsum(np.linalg.norm(np.diff(xs, axis=1), axis=0)) + travelled_dist = np.insert(travelled_dist, 0, 0) + arrow_dist = np.arange(0, travelled_dist[-1], arrow_step) + arrows_X = np.interp(arrow_dist, travelled_dist, xs[0, :]) + arrows_Y = np.interp(arrow_dist, travelled_dist, xs[1, :]) + arrows_U = np.zeros_like(arrows_X) + arrows_V = np.zeros_like(arrows_X) + # ax.plot(arrows_X, arrows_Y, 'k*') + for j in range(len(arrow_dist)): + arrows_U[j], arrows_V[j] = f(np.array([arrows_X[j], arrows_Y[j]]), xg, obstacles, unit_magnitude=1, + crep=crep, reactivity=reactivity) + ax.quiver(arrows_X, arrows_Y, arrows_U, arrows_V, color=color, scale=50, width=0.005, scale_units='width', headlength=2, headaxislength=2) + # ax.plot(xs[0, :], xs[1, :], color=color, linewidth=linewidth, **kwargs) + +def draw_vector_field(xg, obstacles, ax, xlim=None, ylim=None, n=50, **kwargs): + if xlim is None: + xlim = ax.get_xlim() + if ylim is None: + ylim = ax.get_ylim() + + Y, X = np.mgrid[ylim[0]:ylim[1]:n*1j, xlim[0]:xlim[1]:n*1j] + U = np.zeros(X.shape) + V = np.zeros(X.shape) + mask = np.zeros(U.shape, dtype=bool) + for i in range(X.shape[1]): + for j in range(X.shape[0]): + x = np.array([X[i, j], Y[i, j]]) + if np.any([o.interior_point(x) for o in obstacles]): + mask[i, j] = True + U[i, j] = np.nan + else: + U[i, j], V[i, j] = f(x, xg, obstacles, unit_magnitude=1) + U = np.ma.array(U, mask=mask) + # return ax.quiver(X, Y, U, V) + return ax.streamplot(X, Y, U, V, **kwargs) \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py new file mode 100644 index 0000000..e6a308c --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py @@ -0,0 +1,4 @@ +from .workspace_modification import workspace_modification +from .path_generator import path_generator, pol2pos +from .tunnel_mpc import TunnelMpc +from .tunnel_mpc_controller import TunnelMpcController \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py new file mode 100644 index 0000000..fae769c --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py @@ -0,0 +1,108 @@ +import numpy as np +import time +import shapely +from starworlds.utils.misc import tic, toc +from motion_control.soads.soads import f as soads_f + + +def pol2pos(path_pol, s): + n_pol = len(path_pol) // 2 - 1 + return [sum([path_pol[j * (n_pol + 1) + i] * s ** (n_pol - i) for i in range(n_pol + 1)]) for j in range(2)] + + +def path_generator(r0, rg, obstacles, dp_max, N, dt, max_compute_time, n_pol, ds_decay_rate=0.5, + ds_increase_rate=2., max_nr_steps=1000, convergence_tolerance=1e-5, P_prev=None, s_prev=None, + reactivity=1., crep=1., tail_effect=False, reference_step_size=0.5, verbosity=0): + t0 = tic() + + # Initialize + ds = 1 + s = np.zeros(max_nr_steps) + r = np.zeros((max_nr_steps, r0.size)) + if P_prev is not None: + i = P_prev.shape[0] + r[:i, :] = P_prev + s[:i] = s_prev + else: + i = 1 + r[0, :] = r0 + + while True: + dist_to_goal = np.linalg.norm(r[i - 1, :] - rg) + # Check exit conditions + if dist_to_goal < convergence_tolerance: + if verbosity > 1: + print("[Path Generator]: Path converged. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + if s[i - 1] >= N: + if verbosity > 1: + print("[Path Generator]: Completed path length. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + if toc(t0) > max_compute_time: + if verbosity > 1: + print("[Path Generator]: Max compute time in path integrator. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + if i >= max_nr_steps: + if verbosity > 1: + print("[Path Generator]: Max steps taken in path integrator. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + + # Movement using SOADS dynamics + dr = min(dp_max, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, adapt_obstacle_velocity=False, + unit_magnitude=True, crep=crep, + reactivity=reactivity, tail_effect=tail_effect, + convergence_tolerance=convergence_tolerance) + + r[i, :] = r[i - 1, :] + dr * ds + + ri_in_obstacle = False + while any([o.interior_point(r[i, :]) for o in obstacles]): + if verbosity > 1: + print("[Path Generator]: Path inside obstacle. Reducing integration step from {:5f} to {:5f}.".format(ds, ds*ds_decay_rate)) + ds *= ds_decay_rate + r[i, :] = r[i - 1, :] + dr * ds + # Additional compute time check + if toc(t0) > max_compute_time: + ri_in_obstacle = True + break + if ri_in_obstacle: + continue + + # Update travelled distance + s[i] = s[i - 1] + ds + # Try to increase step rate again + ds = min(ds_increase_rate * ds, 1) + # Increase iteration counter + i += 1 + + r = r[:i, :] + s = s[:i] + + # Evenly spaced path + s_vec = np.arange(0, s[-1] + reference_step_size, reference_step_size) + xs, ys = np.interp(s_vec, s, r[:, 0]), np.interp(s_vec, s, r[:, 1]) + # Append not finished path with fixed final position + s_vec = np.append(s_vec, np.arange(s[-1] + reference_step_size, N + reference_step_size, reference_step_size)) + xs = np.append(xs, xs[-1] * np.ones(len(s_vec)-len(xs))) + ys = np.append(ys, ys[-1] * np.ones(len(s_vec)-len(ys))) + + reference_path = [el for p in zip(xs, ys) for el in p] + + # TODO: Fix when close to goal + # TODO: Adjust for short arc length, skip higher order terms.. + path_pol = np.polyfit(s_vec, reference_path[::2], n_pol).tolist() + \ + np.polyfit(s_vec, reference_path[1::2], n_pol).tolist() + # Force init position to be correct + path_pol[n_pol] = reference_path[0] + path_pol[-1] = reference_path[1] + + # Compute polyfit approximation error + epsilon = [np.linalg.norm(np.array(reference_path[2 * i:2 * (i + 1)]) - np.array(pol2pos(path_pol, s_vec[i]))) for i in + range(N + 1)] + + compute_time = toc(t0) + return path_pol, epsilon, reference_path, compute_time diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py new file mode 100644 index 0000000..f72b24f --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py @@ -0,0 +1,377 @@ +import casadi.casadi as casadi +import opengen as og +import numpy as np +import os, sys +import yaml + + +class NoSolutionError(Exception): + '''raise this when there's no solution to the mpc problem''' + pass + + +class TunnelMpc: + + def __init__(self, params, robot): + self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build') + # Load parameters + self.build_params = None + self.robot = robot + self.set_build_params(params) + rebuild, self.build_name = self.get_build_version() + # Build if different from latest build + if rebuild: + self.build() + else: + print("Found MPC build: {}".format(self.build_name)) + + sys.path.insert(1, os.path.join(self.build_dir, self.build_name)) + optimizer = __import__(self.build_name) + self.solver = optimizer.solver() + self.reset() + + def reset(self): + self.sol_prev = None + + def set_build_params(self, params): + self.build_params = { + 'mpc_ver': 5, + 'mode': params['build_mode'], + 'N': params['N'], + 'dt': params['dt'], + 'solver_tol': params['solver_tol'], + 'solver_max_time': params['solver_max_time'], + 'solver_max_inner_iterations': params['solver_max_inner_iterations'], + 'solver_max_outer_iterations': params['solver_max_outer_iterations'], + 'x_min': self.robot.x_min, + 'x_max': self.robot.x_max, + 'u_min': self.robot.u_min, + 'u_max': self.robot.u_max, + 'nx': self.robot.nx, + 'nu': self.robot.nu, + 'np': 2, + 'robot_model': self.robot.__class__.__name__, + 'n_pol': params['n_pol'], + 'integration_method': params['integration_method'] + } + + def get_build_version(self): + builds = [name for name in os.listdir(self.build_dir) + if os.path.isdir(os.path.join(self.build_dir, name))] + for build_ver in builds: + if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')): + continue + with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file: + build_ver_params = yaml.load(file, Loader=yaml.FullLoader) + if self.build_params == build_ver_params: + return False, build_ver + ver = 0 + while 'ver' + str(ver) in os.listdir(self.build_dir): + ver += 1 + return True, 'ver' + str(ver) + + + def e_cost(self, e, ce): + return ce * e + + def ud_cost(self, u, u_prev, R): + ud = [(u[i] - u_prev[i]) for i in range(self.robot.nu)] + return R[0] * ud[0] ** 2 + R[1] * ud[1] ** 2 + + def ref(self, path_pol, s): + return [sum([path_pol[j*(self.build_params['n_pol']+1) + i] * s**(self.build_params['n_pol']-i) for i in range(self.build_params['n_pol']+1)]) + for j in range(self.build_params['np'])] + + def fz(self, z, mu): + return self.robot.f(z[:-1], mu[:-1]) + [mu[-1] / self.build_params['dt']] + + def fz_d(self, z, mu): + nz = self.build_params['nx'] + 1 + if self.build_params['integration_method'] == 'euler': + zd = self.fz(z, mu) + return [z[i] + zd[i] * self.build_params['dt'] for i in range(nz)] + # if self.build_params['integration_method'] == 'RK4': + # k1 = self.fz(z, u, v, path_pol) + # k2 = self.fz([z[i] + self.build_params['dt'] / 2 * k1[i] for i in range(nz)], u, v, path_pol) + # k3 = self.fz([z[i] + self.build_params['dt'] / 2 * k2[i] for i in range(nz)], u, v, path_pol) + # k4 = self.fz([z[i] + self.build_params['dt'] * k3[i] for i in range(nz)], u, v, path_pol) + # return [z[i] + (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) * self.build_params['dt'] / 6 for i in range(nz)] + raise NotImplemented + + def one_step_feasible(self, sol, x0, path_pol, e_max, d=False): + u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol) + sol_ok = True + eps = 1e-6 + # x constraints + if not all([self.build_params['x_min'][i] - eps <= x[0][i] <= self.build_params['x_max'][i] + eps for i in + range(self.build_params['nx'])]): + sol_ok = False + if d: + print("Bad x") + print(x[:self.build_params['nx']]) + # s constraints + if not (0 - eps <= s[1] <= self.build_params['N'] + eps): + sol_ok = False + if d: + print("Bad s") + print(s[1]) + # u constraints + if not all([self.build_params['u_min'][i] - eps <= u[i] <= self.build_params['u_max'][i] + eps for i in + range(self.build_params['nu'])]): + sol_ok = False + if d: + print("Bad u") + print(u[:self.build_params['nu']]) + # v constraints + if not (0 - eps <= ds[0] <= np.inf + eps): + sol_ok = False + if d: + print("Bad ds") + print(ds[0]) + # e constraints + if not (e[1] <= e_max + eps): + sol_ok = False + if d: + print("Bad e ({:.4f} > {:.4f})".format(e[1], e_max)) + + # All constraints ok + return sol_ok + + def is_feasible(self, sol, x0, path_pol, e_max, d=False): + u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol) + sol_ok = True + + eps = 1e-2 + # x constraints + if not all([self.build_params['x_min'][i]-eps <= xk[i] <= self.build_params['x_max'][i]+eps for i in range(self.build_params['nx']) for xk in x]): + sol_ok = False + if d: + print("Bad x") + print(x) + # s constraints + if not all([0-eps <= sk <= self.build_params['N'] + eps for sk in s]): + sol_ok = False + if d: + print("Bad s") + print(s) + # u constraints + if not all([self.build_params['u_min'][i]-eps <= uk <= self.build_params['u_max'][i]+eps for i in range(self.build_params['nu']) for uk in u[i::self.build_params['nu']]]): + sol_ok = False + if d: + print("Bad u") + print(u) + # v constraints + if not all([0-eps <= dsk <= np.inf * self.build_params['dt'] + eps for dsk in ds]): + sol_ok = False + if d: + print("Bad ds") + print(ds) + # e constraints + if not all([ek <= e_max + eps for ek in e]): + sol_ok = False + if d: + print("Bad e") + print(e) + + # All constraints ok + return sol_ok + + def e_sqr(self, z, path_pol): + ref = self.ref(path_pol, z[-1]) + p = self.robot.h(z[:-1]) + e_sqr = sum([(ref[i] - p[i])**2 for i in range(self.build_params['np'])]) + return e_sqr + + def unpack_solution(self, sol, x0, path_pol): + u = sol[:self.build_params['nu'] * self.build_params['N']] + ds = sol[self.build_params['nu'] * self.build_params['N']:] + zk = x0 + [0] + x = [zk[:-1]] + s = [zk[-1]] + e = [self.e_sqr(zk, path_pol)] + for k in range(self.build_params['N']): # LOOP OVER TIME STEPS + # Dynamics + muk = u[self.build_params['nu'] * k:self.build_params['nu'] * (k + 1)] + [ds[k]] + zk = self.fz_d(zk, muk) + x += [zk[:-1]] + s += [zk[-1]] + e += [np.sqrt(self.e_sqr(zk, path_pol))] + return u, ds, x, s, e + + def sol2cost(self, sol, x0, path_pol, u_prev, params): + u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol) + u = u_prev + u + v_cost, ud_cost, omega_cost = [0.] * 3 + for i in range(len(ds)): + ui_prev = u[self.build_params['nu'] * i:self.build_params['nu'] * (i + 1)] + ui = u[self.build_params['nu'] * (i+1):self.build_params['nu'] * (i + 2)] + ud_cost += self.ud_cost(ui, ui_prev, params['R']) + v_cost += self.v_cost(ds[i], params['cv'], params['lambda'], i) + e_end_cost = self.e_cost(e[-1]**2, params['ce_end']) + return v_cost, ud_cost, e_end_cost, omega_cost + + def build(self): + # Build parametric optimizer + # ------------------------------------ + params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'], 'path_pol': self.build_params['np'] * (self.build_params['n_pol']+1), + 'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'e_max': 1, 'rg': self.build_params['np'], 'e_penalty': 1, 'cg': 1} + par_dim = sum(list(params.values())) + + + # Exchange parameter dimension with value + par = casadi.SX.sym("par", par_dim) # Parameters + p_idx = 0 + for key, dim in params.items(): + params[key] = par[p_idx:p_idx + dim] + p_idx += dim + + # Init value + uk_prev = params['u_prev'] + zk = casadi.vertcat(params['x0'], 0) + cost = 0 + # Decision variable + mu = casadi.SX.sym('uds', (self.build_params['nu'] + 1) * self.build_params['N']) + # Constraint variables + f1, f1_min, f1_max = [], [], [] + if True: + for k in range(self.build_params['N']): # LOOP OVER TIME STEPS + # Decision variables + uk = mu[k * self.build_params['nu']:(k + 1) * self.build_params['nu']] + dsk = mu[self.build_params['nu'] * self.build_params['N'] + k] + muk = casadi.vertcat(uk, dsk) + + # Step forward + zk1 = casadi.vertcat(*self.fz_d(zk, muk)) + ek1_sqr = self.e_sqr(zk1, params['path_pol']) + + # Add stage cost + cost += params['e_penalty'] * ek1_sqr + cost += self.ud_cost(uk, uk_prev, params['R']) + + # Add state and error constraint + f1 = casadi.vertcat(f1, zk1, ek1_sqr - params['e_max']**2) + f1_min += self.build_params['x_min'] + [0] + [-1e10] + f1_max += self.build_params['x_max'] + [self.build_params['N']] + [0] + + uk_prev = uk + zk = zk1 + + + ## Terminal cost + # Cost on last tracking error + cost += self.e_cost(ek1_sqr, params['ce']) + cost += - params['cs'] * zk1[-1] + # Convergence cost + p_end = self.robot.h(zk[:-1]) + xg_err = sum([(p_end[i] - params['rg'][i])**2 for i in range(self.build_params['np'])]) + cost += params['cg'] * xg_err + # Final orientation cost + # cost += params['cg_orientation'] * zk[-1] / (self.build_params['N'] * self.build_params['dt']) * (zk[2]-params['rg'][2])**2 + + # Constraint set + set_c = og.constraints.Rectangle(f1_min, f1_max) + # Decision variable constraints + mu_bounds = og.constraints.Rectangle(self.build_params['u_min'] * self.build_params['N'] + + [0.] * self.build_params['N'], + self.build_params['u_max'] * self.build_params['N'] + + [1] * self.build_params['N']) + + # Setup builder + problem = og.builder.Problem(mu, par, cost) \ + .with_constraints(mu_bounds) \ + .with_aug_lagrangian_constraints(f1, set_c) + build_config = og.config.BuildConfiguration() \ + .with_build_directory(self.build_dir) \ + .with_build_mode(self.build_params['mode']) \ + .with_build_python_bindings() + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(self.build_name) + solver_config = og.config.SolverConfiguration() \ + .with_tolerance(self.build_params['solver_tol']) \ + .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \ + .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \ + .with_max_outer_iterations(self.build_params['solver_max_outer_iterations']) + builder = og.builder.OpEnOptimizerBuilder(problem, + meta, + build_config, + solver_config) \ + .with_verbosity_level(1) + builder.build() + print('') + + # Save build params + with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file: + yaml.dump(self.build_params, file) + + def trivial_solution(self, x, path_pol, e_max): + s_kappa = 0.9 * e_max / self.robot.vmax + p_ref = self.ref(path_pol, s_kappa) + + u = [0] * (self.build_params['N'] * self.build_params['nu']) + ds = [0] * (self.build_params['N']) + xk = x + sk = 0 + zk = xk + [sk] + for k in range(self.build_params['N']): # LOOP OVER TIME STEPS + # Control law + if self.build_params['robot_model'] == 'Unicycle': + ref_dist = np.linalg.norm(np.array(xk[:2])-np.array(p_ref)) + if ref_dist > 1e-5: + theta_ref = np.arctan2(p_ref[1]-xk[1], p_ref[0]-xk[0]) + theta_diff = theta_ref - xk[-1] + if theta_diff > np.pi: + theta_diff -= 2 * np.pi + if theta_diff < -np.pi: + theta_diff += 2 * np.pi + if abs(theta_diff) < 1e-2: # Less than 0.57 degree error + # Linear velocity + u[k*self.build_params['nu']] = min(self.build_params['u_max'][0], ref_dist / self.build_params['dt']) + else: + # Angular velocity + if theta_diff > 0: + u[k * self.build_params['nu'] + 1] = min(self.build_params['u_max'][1], theta_diff / self.build_params['dt']) + else: + u[k * self.build_params['nu'] + 1] = max(self.build_params['u_min'][1], theta_diff / self.build_params['dt']) + + # Dynamics + muk = u[k * self.build_params['nu']:k * self.build_params['nu'] + 2] + [0] + zk = self.fz_d(zk, muk) + xk = zk[:-1] + + return u + ds + + def run(self, x, u_prev, path_pol, params, e_max, rg, verbosity=0): + robot_within_dg = np.linalg.norm(np.array(self.robot.h(rg))-np.array(self.robot.h(x))) < params['dg'] + cg = params['cg'] if robot_within_dg else 0 + + p = x + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + \ + [0.95*e_max] + rg + [params['e_penalty'], cg] + + if self.sol_prev is None or not self.is_feasible(self.sol_prev, x, path_pol, 2*e_max): + # Use trivial solution as initial guess + self.sol_prev = self.trivial_solution(x, path_pol, e_max) + # Run solver + solution_data = self.solver.run(p=p, initial_guess=self.sol_prev) + + + # TODO: When not one step feasible, check for actual collision with mpc before using trivial solution. + if solution_data is None or not self.one_step_feasible(solution_data.solution, x, path_pol, e_max, d=verbosity>0): + if verbosity > 0: + print("[MPC]: Not feasible solution. Using trivial solution.") + sol = self.trivial_solution(x, path_pol, e_max) + self.sol_prev = None + exit_status = "Trivial solution" + cost = -1 + else: + sol = solution_data.solution + exit_status = solution_data.exit_status + cost = solution_data.cost + + self.sol_prev = sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \ + sol[self.build_params['nu'] * (self.build_params['N']-1):self.build_params['nu'] * self.build_params['N']] \ + + sol[self.build_params['nu'] * self.build_params['N']:] + + # Unpack solution + u, ds, x_pred, s_pred, e_pred = self.unpack_solution(sol, x, path_pol) + + return {'u': u, 'x': x_pred, 'e': e_pred, 's': s_pred, 'cost': cost, 'exit_status': exit_status} \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py new file mode 100644 index 0000000..fa846a8 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py @@ -0,0 +1,101 @@ +from motion_control.tunnel_mpc import workspace_modification, path_generator, pol2pos, TunnelMpc +from starworlds.utils.misc import tic, toc +import numpy as np + + +class TunnelMpcController: + def __init__(self, params, robot, verbosity=0): + self.params = params + self.params['dp_max'] = robot.vmax * self.params['dt'] + self.robot = robot + self.mpc = TunnelMpc(params, robot) + self.verbosity = verbosity + self.reset() + + def reset(self): + self.mpc.reset() + self.obstacle_clusters = None + self.obstacles_star = [] + self.free_star = None + self.r0 = None + self.rg = None + self.target_path = [] + self.path_pol = None + self.rho = None + self.epsilon = None + self.solution = None + self.u_prev = [0] * self.robot.nu + self.timing = {'workspace': 0, 'target': 0, 'mpc': 0} + + def compute_u(self, x, pg, obstacles): + p = self.robot.h(x) + + # Update obstacles + if not self.params['use_prev_workspace']: + self.free_star = None + self.obstacle_clusters, self.r0, self.rg, self.rho, self.free_star, self.timing['workspace'], exit_flag = \ + workspace_modification(obstacles, p, pg, self.params['rho0'], self.params['max_obs_compute_time'], + self.params['hull_epsilon'], self.params['gamma'], + make_convex=self.params['make_convex'], previous_obstacle_clusters=self.obstacle_clusters, + free_star_prev=self.free_star, verbosity=self.verbosity) + self.obstacles_star = [o.cluster_obstacle for o in self.obstacle_clusters] + + # Buffer previous target path + P_prev, s_prev = None, None + if self.params['buffer'] and self.target_path: + P_prev = np.array([self.target_path[::2], self.target_path[1::2]]).T + # Shift path to start at point closest to robot position + P_prev = P_prev[np.argmin(np.linalg.norm(p - P_prev, axis=1)):, :] + # P_prev[0, :] = self.r0 + if np.linalg.norm(p - P_prev[0, :]) > self.rho: + if self.verbosity > 0: + print("[Path Generator]: No reuse of previous path. Path not within distance rho from robot.") + P_prev = None + else: + for r in P_prev: + if any([o.interior_point(r) for o in self.obstacles_star]): + if self.verbosity > 0: + print("[Path Generator]: No reuse of previous path. Path not collision-free.") + P_prev = None + + # P_prev = None + if P_prev is not None: + # Cut off stand still padding in previous path + P_prev_stepsize = np.linalg.norm(np.diff(P_prev, axis=0), axis=1) + s_prev = np.hstack((0, np.cumsum(P_prev_stepsize) / self.params['dp_max'])) + P_prev_mask = [True] + (P_prev_stepsize > 1e-8).tolist() + P_prev = P_prev[P_prev_mask, :] + s_prev = s_prev[P_prev_mask] + + # Make sure all polygon representations are computed + [o._compute_polygon_representation() for o in self.obstacles_star] + + # Check for convergence + if np.linalg.norm(np.array(p) - np.array(pg)) < self.params['convergence_margin']: + self.timing['target'] = 0 + self.timing['mpc'] = 0 + return np.zeros(self.robot.nu) + + # Update target path + self.path_pol, self.epsilon, self.target_path, self.timing['target'] = \ + path_generator(self.r0, self.rg, self.obstacles_star, self.params['dp_max'], self.params['N'], + self.params['dt'], self.params['max_compute_time'], self.params['n_pol'], + ds_decay_rate=0.5, ds_increase_rate=2., max_nr_steps=1000, P_prev=P_prev, s_prev=s_prev, + reactivity=self.params['reactivity'], crep=self.params['crep'], + convergence_tolerance=self.params['convergence_tolerance'], verbosity=self.verbosity) + + # Compute MPC solution + t0 = tic() + # Set parameter for tracking error constraint + if self.rho > 0: + e_max = self.rho - max(self.epsilon) + else: + e_max = 1.e6 + + self.solution = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, e_max, self.rg.tolist(), verbosity=self.verbosity) + self.u_prev = self.solution['u'][:self.robot.nu] + self.timing['mpc'] = toc(t0) + + # Extract first control signal + return np.array(self.solution['u'][:self.mpc.build_params['nu']]) + diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py new file mode 100644 index 0000000..86d0f3d --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py @@ -0,0 +1,66 @@ +import numpy as np +from starworlds.starshaped_hull import cluster_and_starify, ObstacleCluster +from starworlds.utils.misc import tic, toc +import shapely + +# TODO: Check why computational time varies so much for same static scene +def workspace_modification(obstacles, p, pg, rho0, max_compute_time, hull_epsilon, gamma=0.5, make_convex=0, + previous_obstacle_clusters=None, free_star_prev=None, verbosity=0): + + # Clearance variable initialization + rho = rho0 / gamma # First rho should be rho0 + t_init = tic() + + while True: + if toc(t_init) > max_compute_time: + if verbosity > 0: + print("[Workspace modification]: Max compute time in rho iteration.") + break + + # Reduce rho + rho *= gamma + + # Pad obstacles with rho + obstacles_rho = [o.dilated_obstacle(padding=rho, id="duplicate") for o in obstacles] + + # TODO: Fix boundaries + free_rho = shapely.geometry.box(-20, -20, 20, 20) + for o in obstacles_rho: + free_rho = free_rho.difference(o.polygon()) + + # TODO: Check buffering fix + # Find P0 + Bp = shapely.geometry.Point(p).buffer(0.95 * rho) + initial_reference_set = Bp.intersection(free_rho.buffer(-0.1 * rho)) + + if not initial_reference_set.is_empty: + break + + # Initial and goal reference position selection + r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(p)) + r0 = np.array(r0_sh.coords[0]) + rg_sh, _ = shapely.ops.nearest_points(free_rho, shapely.geometry.Point(pg)) + rg = np.array(rg_sh.coords[0]) + + + # TODO: Check more thoroughly + if free_star_prev is not None: + free_star_prev = free_star_prev.buffer(-1e-4) + if free_star_prev is not None and free_star_prev.contains(r0_sh) and free_star_prev.contains(rg_sh) and free_rho.contains(free_star_prev):# not any([free_star_prev.covers(o.polygon()) for o in obstacles_rho]): + if verbosity > 1: + print("[Workspace modification]: Reuse workspace from previous time step.") + obstacle_clusters = previous_obstacle_clusters + exit_flag = 10 + else: + # Apply cluster and starify + obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles_rho, r0, rg, + hull_epsilon, max_compute_time-toc(t_init), + previous_clusters=previous_obstacle_clusters, + make_convex=make_convex, verbose=verbosity) + + free_star = shapely.geometry.box(-20, -20, 20, 20) + for o in obstacle_clusters: + free_star = free_star.difference(o.polygon()) + + compute_time = toc(t_init) + return obstacle_clusters, r0, rg, rho, free_star, compute_time, exit_flag \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py new file mode 100644 index 0000000..e6a308c --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py @@ -0,0 +1,4 @@ +from .workspace_modification import workspace_modification +from .path_generator import path_generator, pol2pos +from .tunnel_mpc import TunnelMpc +from .tunnel_mpc_controller import TunnelMpcController \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore new file mode 100644 index 0000000..86d0cb2 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py new file mode 100644 index 0000000..fae769c --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py @@ -0,0 +1,108 @@ +import numpy as np +import time +import shapely +from starworlds.utils.misc import tic, toc +from motion_control.soads.soads import f as soads_f + + +def pol2pos(path_pol, s): + n_pol = len(path_pol) // 2 - 1 + return [sum([path_pol[j * (n_pol + 1) + i] * s ** (n_pol - i) for i in range(n_pol + 1)]) for j in range(2)] + + +def path_generator(r0, rg, obstacles, dp_max, N, dt, max_compute_time, n_pol, ds_decay_rate=0.5, + ds_increase_rate=2., max_nr_steps=1000, convergence_tolerance=1e-5, P_prev=None, s_prev=None, + reactivity=1., crep=1., tail_effect=False, reference_step_size=0.5, verbosity=0): + t0 = tic() + + # Initialize + ds = 1 + s = np.zeros(max_nr_steps) + r = np.zeros((max_nr_steps, r0.size)) + if P_prev is not None: + i = P_prev.shape[0] + r[:i, :] = P_prev + s[:i] = s_prev + else: + i = 1 + r[0, :] = r0 + + while True: + dist_to_goal = np.linalg.norm(r[i - 1, :] - rg) + # Check exit conditions + if dist_to_goal < convergence_tolerance: + if verbosity > 1: + print("[Path Generator]: Path converged. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + if s[i - 1] >= N: + if verbosity > 1: + print("[Path Generator]: Completed path length. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + if toc(t0) > max_compute_time: + if verbosity > 1: + print("[Path Generator]: Max compute time in path integrator. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + if i >= max_nr_steps: + if verbosity > 1: + print("[Path Generator]: Max steps taken in path integrator. " + str( + int(100 * (s[i - 1] / N))) + "% of path completed.") + break + + # Movement using SOADS dynamics + dr = min(dp_max, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, adapt_obstacle_velocity=False, + unit_magnitude=True, crep=crep, + reactivity=reactivity, tail_effect=tail_effect, + convergence_tolerance=convergence_tolerance) + + r[i, :] = r[i - 1, :] + dr * ds + + ri_in_obstacle = False + while any([o.interior_point(r[i, :]) for o in obstacles]): + if verbosity > 1: + print("[Path Generator]: Path inside obstacle. Reducing integration step from {:5f} to {:5f}.".format(ds, ds*ds_decay_rate)) + ds *= ds_decay_rate + r[i, :] = r[i - 1, :] + dr * ds + # Additional compute time check + if toc(t0) > max_compute_time: + ri_in_obstacle = True + break + if ri_in_obstacle: + continue + + # Update travelled distance + s[i] = s[i - 1] + ds + # Try to increase step rate again + ds = min(ds_increase_rate * ds, 1) + # Increase iteration counter + i += 1 + + r = r[:i, :] + s = s[:i] + + # Evenly spaced path + s_vec = np.arange(0, s[-1] + reference_step_size, reference_step_size) + xs, ys = np.interp(s_vec, s, r[:, 0]), np.interp(s_vec, s, r[:, 1]) + # Append not finished path with fixed final position + s_vec = np.append(s_vec, np.arange(s[-1] + reference_step_size, N + reference_step_size, reference_step_size)) + xs = np.append(xs, xs[-1] * np.ones(len(s_vec)-len(xs))) + ys = np.append(ys, ys[-1] * np.ones(len(s_vec)-len(ys))) + + reference_path = [el for p in zip(xs, ys) for el in p] + + # TODO: Fix when close to goal + # TODO: Adjust for short arc length, skip higher order terms.. + path_pol = np.polyfit(s_vec, reference_path[::2], n_pol).tolist() + \ + np.polyfit(s_vec, reference_path[1::2], n_pol).tolist() + # Force init position to be correct + path_pol[n_pol] = reference_path[0] + path_pol[-1] = reference_path[1] + + # Compute polyfit approximation error + epsilon = [np.linalg.norm(np.array(reference_path[2 * i:2 * (i + 1)]) - np.array(pol2pos(path_pol, s_vec[i]))) for i in + range(N + 1)] + + compute_time = toc(t0) + return path_pol, epsilon, reference_path, compute_time diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py new file mode 100644 index 0000000..81c55d9 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py @@ -0,0 +1,386 @@ +import casadi.casadi as casadi +import opengen as og +import numpy as np +import os, sys +import yaml + + +class NoSolutionError(Exception): + '''raise this when there's no solution to the mpc problem''' + pass + + +class TunnelMpc: + + def __init__(self, params, robot): + self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build') + # Load parameters + self.build_params = None + self.robot = robot + self.set_build_params(params) + rebuild, self.build_name = self.get_build_version() + # Build if different from latest build + if rebuild: + self.build() + else: + print("Found MPC build: {}".format(self.build_name)) + + sys.path.insert(1, os.path.join(self.build_dir, self.build_name)) + optimizer = __import__(self.build_name) + self.solver = optimizer.solver() + self.reset() + + def reset(self): + self.sol_prev = None + + def set_build_params(self, params): + self.build_params = { + 'mpc_ver': 5, + 'mode': params['build_mode'], + 'N': params['N'], + 'dt': params['dt'], + 'solver_tol': params['solver_tol'], + 'solver_max_time': params['solver_max_time'], + 'solver_max_inner_iterations': params['solver_max_inner_iterations'], + 'solver_max_outer_iterations': params['solver_max_outer_iterations'], + 'x_min': self.robot.x_min, + 'x_max': self.robot.x_max, + 'u_min': self.robot.u_min, + 'u_max': self.robot.u_max, + 'nx': self.robot.nx, + 'nu': self.robot.nu, + 'np': 2, + 'robot_model': self.robot.__class__.__name__, + 'n_pol': params['n_pol'], + 'integration_method': params['integration_method'] + } + + def get_build_version(self): + builds = [name for name in os.listdir(self.build_dir) + if os.path.isdir(os.path.join(self.build_dir, name))] + for build_ver in builds: + if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')): + continue + with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file: + build_ver_params = yaml.load(file, Loader=yaml.FullLoader) + if self.build_params == build_ver_params: + return False, build_ver + ver = 0 + while 'ver' + str(ver) in os.listdir(self.build_dir): + ver += 1 + return True, 'ver' + str(ver) + + + def e_cost(self, e, ce): + return ce * e + + def ud_cost(self, u, u_prev, R): + ud = [(u[i] - u_prev[i]) for i in range(self.robot.nu)] + return R[0] * ud[0] ** 2 + R[1] * ud[1] ** 2 + + def ref(self, path_pol, s): + return [sum([path_pol[j*(self.build_params['n_pol']+1) + i] * s**(self.build_params['n_pol']-i) for i in range(self.build_params['n_pol']+1)]) + for j in range(self.build_params['np'])] + + def fz(self, z, mu): + return self.robot.f(z[:-1], mu[:-1]) + [mu[-1] / self.build_params['dt']] + + def fz_d(self, z, mu): + nz = self.build_params['nx'] + 1 + if self.build_params['integration_method'] == 'euler': + zd = self.fz(z, mu) + return [z[i] + zd[i] * self.build_params['dt'] for i in range(nz)] + # if self.build_params['integration_method'] == 'RK4': + # k1 = self.fz(z, u, v, path_pol) + # k2 = self.fz([z[i] + self.build_params['dt'] / 2 * k1[i] for i in range(nz)], u, v, path_pol) + # k3 = self.fz([z[i] + self.build_params['dt'] / 2 * k2[i] for i in range(nz)], u, v, path_pol) + # k4 = self.fz([z[i] + self.build_params['dt'] * k3[i] for i in range(nz)], u, v, path_pol) + # return [z[i] + (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) * self.build_params['dt'] / 6 for i in range(nz)] + raise NotImplemented + + def one_step_feasible(self, sol, x0, path_pol, e_max, s_kappa, obstacles, d=False): + u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol) + sol_ok = True + eps = 1e-3 + # x constraints + if not all([self.build_params['x_min'][i] - eps <= x[0][i] <= self.build_params['x_max'][i] + eps for i in + range(self.build_params['nx'])]): + sol_ok = False + if d: + print("[MPC]: Bad x") + print(x[0]) + # s constraints + if not (0 - eps <= s[1] <= self.build_params['N'] + eps): + sol_ok = False + if d: + print("[MPC]: Bad s") + print(s[1]) + # u constraints + if not all([self.build_params['u_min'][i] - eps <= u[i] <= self.build_params['u_max'][i] + eps for i in + range(self.build_params['nu'])]): + sol_ok = False + if d: + print("[MPC]: Bad u") + print(u[0]) + # e constraints + if not (e[1] <= e_max + eps): + sol_ok = False + if d: + print("[MPC]: Bad e ({:.4f} > {:.4f})".format(e[1], e_max)) + p = x[0][:self.build_params['np']] + if not any([o.interior_point(p) for o in obstacles]): + sol_ok = True + if d: + print("[MPC]: Position collision free. OK solution.") + # ds constraints + if not (s_kappa - eps <= ds[0] <= 1 + eps): + sol_ok = False + if d: + print("[MPC]: Bad ds ({:.4f} < {:.4f})".format(ds[0], s_kappa)) + + return sol_ok + + def is_feasible(self, sol, x0, path_pol, e_max, s_kappa, d=False): + u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol) + sol_ok = True + + eps = 1e-2 + # x constraints + if not all([self.build_params['x_min'][i]-eps <= xk[i] <= self.build_params['x_max'][i]+eps for i in range(self.build_params['nx']) for xk in x]): + sol_ok = False + if d: + print("[MPC]: Bad x") + print(x) + # s constraints + if not all([0-eps <= sk <= self.build_params['N'] + eps for sk in s]): + sol_ok = False + if d: + print("[MPC]: Bad s") + print(s) + # u constraints + if not all([self.build_params['u_min'][i]-eps <= uk <= self.build_params['u_max'][i]+eps for i in range(self.build_params['nu']) for uk in u[i::self.build_params['nu']]]): + sol_ok = False + if d: + print("[MPC]: Bad u") + print(u) + # v constraints + if not all([0-eps <= dsk <= np.inf * self.build_params['dt'] + eps for dsk in ds] or ds[0]) < s_kappa: + sol_ok = False + if d: + print("[MPC]: Bad ds") + print(ds) + print(s_kappa) + # e constraints + if not all([ek <= e_max + eps for ek in e]): + sol_ok = False + if d: + print("[MPC]: Bad e") + print(e) + + # All constraints ok + return sol_ok + + def e_sqr(self, z, path_pol): + ref = self.ref(path_pol, z[-1]) + p = self.robot.h(z[:-1]) + e_sqr = sum([(ref[i] - p[i])**2 for i in range(self.build_params['np'])]) + return e_sqr + + def unpack_solution(self, sol, x0, path_pol): + u = sol[:self.build_params['nu'] * self.build_params['N']] + ds = sol[self.build_params['nu'] * self.build_params['N']:] + zk = x0 + [0] + x = [zk[:-1]] + s = [zk[-1]] + e = [self.e_sqr(zk, path_pol)] + for k in range(self.build_params['N']): # LOOP OVER TIME STEPS + # Dynamics + muk = u[self.build_params['nu'] * k:self.build_params['nu'] * (k + 1)] + [ds[k]] + zk = self.fz_d(zk, muk) + x += [zk[:-1]] + s += [zk[-1]] + e += [np.sqrt(self.e_sqr(zk, path_pol))] + return u, ds, x, s, e + + def sol2cost(self, sol, x0, path_pol, u_prev, params): + u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol) + u = u_prev + u + v_cost, ud_cost, omega_cost = [0.] * 3 + for i in range(len(ds)): + ui_prev = u[self.build_params['nu'] * i:self.build_params['nu'] * (i + 1)] + ui = u[self.build_params['nu'] * (i+1):self.build_params['nu'] * (i + 2)] + ud_cost += self.ud_cost(ui, ui_prev, params['R']) + v_cost += self.v_cost(ds[i], params['cv'], params['lambda'], i) + e_end_cost = self.e_cost(e[-1]**2, params['ce_end']) + return v_cost, ud_cost, e_end_cost, omega_cost + + def build(self): + # Build parametric optimizer + # ------------------------------------ + params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'], 'path_pol': self.build_params['np'] * (self.build_params['n_pol']+1), + 'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'e_max': 1, 'rg': self.build_params['np'], 'e_penalty': 1, 'cg': 1, 's_kappa': 1} + par_dim = sum(list(params.values())) + + + # Exchange parameter dimension with value + par = casadi.SX.sym("par", par_dim) # Parameters + p_idx = 0 + for key, dim in params.items(): + params[key] = par[p_idx:p_idx + dim] + p_idx += dim + + # Init value + uk_prev = params['u_prev'] + zk = casadi.vertcat(params['x0'], 0) + cost = 0 + # Decision variable + mu = casadi.SX.sym('uds', (self.build_params['nu'] + 1) * self.build_params['N']) + # Constraint variables + f1, f1_min, f1_max = [], [], [] + + # Loop over time steps + for k in range(self.build_params['N']): + # Decision variables + uk = mu[k * self.build_params['nu']:(k + 1) * self.build_params['nu']] + dsk = mu[self.build_params['nu'] * self.build_params['N'] + k] + muk = casadi.vertcat(uk, dsk) + + # Step forward + zk1 = casadi.vertcat(*self.fz_d(zk, muk)) + ek1_sqr = self.e_sqr(zk1, params['path_pol']) + + # Add stage cost + cost += params['e_penalty'] * ek1_sqr + cost += self.ud_cost(uk, uk_prev, params['R']) + + # Add state and error constraint + f1 = casadi.vertcat(f1, zk1, ek1_sqr - params['e_max']**2) + f1_min += self.build_params['x_min'] + [0] + [-1e10] + f1_max += self.build_params['x_max'] + [self.build_params['N']] + [0] + + # Initial reference increment constraint + if k == 0: + f1 = casadi.vertcat(f1, dsk - params['s_kappa']) + f1_min += [0] + f1_max += [1e10] + + uk_prev = uk + zk = zk1 + + + ## Terminal cost + # Cost on last tracking error + cost += self.e_cost(ek1_sqr, params['ce']) + cost += - params['cs'] * zk1[-1] + # Convergence cost + p_end = self.robot.h(zk[:-1]) + xg_err = sum([(p_end[i] - params['rg'][i])**2 for i in range(self.build_params['np'])]) + cost += params['cg'] * xg_err + # Final orientation cost + # cost += params['cg_orientation'] * (1 / (xg_err + 1e-3) ) * (zk[2]-params['rg'][2])**2 + + # Constraint set + set_c = og.constraints.Rectangle(f1_min, f1_max) + # Decision variable constraints + mu_bounds = og.constraints.Rectangle(self.build_params['u_min'] * self.build_params['N'] + + [0.] * self.build_params['N'], + self.build_params['u_max'] * self.build_params['N'] + + [1] * self.build_params['N']) + + # Setup builder + problem = og.builder.Problem(mu, par, cost) \ + .with_constraints(mu_bounds) \ + .with_aug_lagrangian_constraints(f1, set_c) + build_config = og.config.BuildConfiguration() \ + .with_build_directory(self.build_dir) \ + .with_build_mode(self.build_params['mode']) \ + .with_build_python_bindings() + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(self.build_name) + solver_config = og.config.SolverConfiguration() \ + .with_tolerance(self.build_params['solver_tol']) \ + .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \ + .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \ + .with_max_outer_iterations(self.build_params['solver_max_outer_iterations']) + builder = og.builder.OpEnOptimizerBuilder(problem, + meta, + build_config, + solver_config) \ + .with_verbosity_level(1) + builder.build() + print('') + + # Save build params + with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file: + yaml.dump(self.build_params, file) + + def trivial_solution(self, x, path_pol, e_max, s_kappa): + p_ref = self.ref(path_pol, s_kappa) + + u = [0] * (self.build_params['N'] * self.build_params['nu']) + ds = [0] * (self.build_params['N']) + xk = x + sk = 0 + zk = xk + [sk] + for k in range(self.build_params['N']): # LOOP OVER TIME STEPS + # Control law + if self.build_params['robot_model'] == 'Unicycle': + ref_dist = np.linalg.norm(np.array(xk[:2])-np.array(p_ref)) + if ref_dist > 1e-5: + theta_ref = np.arctan2(p_ref[1]-xk[1], p_ref[0]-xk[0]) + theta_diff = theta_ref - xk[-1] + if theta_diff > np.pi: + theta_diff -= 2 * np.pi + if theta_diff < -np.pi: + theta_diff += 2 * np.pi + if abs(theta_diff) < 1e-2: # Less than 0.57 degree error + # Linear velocity + u[k*self.build_params['nu']] = min(self.build_params['u_max'][0], ref_dist / self.build_params['dt']) + else: + # Angular velocity + if theta_diff > 0: + u[k * self.build_params['nu'] + 1] = min(self.build_params['u_max'][1], theta_diff / self.build_params['dt']) + else: + u[k * self.build_params['nu'] + 1] = max(self.build_params['u_min'][1], theta_diff / self.build_params['dt']) + + # Dynamics + muk = u[k * self.build_params['nu']:k * self.build_params['nu'] + 2] + [0] + zk = self.fz_d(zk, muk) + xk = zk[:-1] + + return u + ds + + def run(self, x, u_prev, path_pol, params, e_max, rg, s_kappa, obstacles, verbosity=0): + robot_within_dg = np.linalg.norm(np.array(self.robot.h(rg))-np.array(self.robot.h(x))) < params['dg'] + cg = params['cg'] if robot_within_dg else 0 + + p = x + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + \ + [0.95*e_max] + rg + [params['e_penalty'], cg, s_kappa] + + if self.sol_prev is None or not self.is_feasible(self.sol_prev, x, path_pol, 2*e_max, s_kappa): + # Use trivial solution as initial guess + self.sol_prev = self.trivial_solution(x, path_pol, e_max, s_kappa) + # Run solver + solution_data = self.solver.run(p=p, initial_guess=self.sol_prev) + + # TODO: When not one step feasible, check for actual collision with mpc before using trivial solution. + if solution_data is None or not self.one_step_feasible(solution_data.solution, x, path_pol, e_max, s_kappa, obstacles, d=verbosity>0): + if verbosity > 0: + print("[MPC]: Not feasible solution. Using trivial solution.") + sol = self.trivial_solution(x, path_pol, e_max, s_kappa) + self.sol_prev = None + exit_status = "Trivial solution" + cost = -1 + else: + sol = solution_data.solution + exit_status = solution_data.exit_status + cost = solution_data.cost + + self.sol_prev = sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \ + sol[self.build_params['nu'] * (self.build_params['N']-1):self.build_params['nu'] * self.build_params['N']] \ + + sol[self.build_params['nu'] * self.build_params['N']:] + + # Unpack solution + u, ds, x_pred, s_pred, e_pred = self.unpack_solution(sol, x, path_pol) + + return {'u': u, 'x': x_pred, 'e': e_pred, 's': s_pred, 'cost': cost, 'exit_status': exit_status} \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py new file mode 100644 index 0000000..8ca8b2f --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py @@ -0,0 +1,107 @@ +from motion_control.tunnel_mpc_convergence import workspace_modification, path_generator, pol2pos, TunnelMpc +from starworlds.utils.misc import tic, toc +import numpy as np + + +class TunnelMpcController: + def __init__(self, params, robot, verbosity=0): + self.params = params + self.params['dp_max'] = robot.vmax * self.params['dt'] + self.robot = robot + self.mpc = TunnelMpc(params, robot) + self.verbosity = verbosity + self.reset() + + def reset(self): + self.mpc.reset() + self.obstacle_clusters = None + self.obstacles_star = [] + self.free_star = None + self.ri = None + self.rg = None + self.rs = None + self.target_path = [] + self.path_pol = None + self.rho = None + self.epsilon = None + self.solution = None + self.u_prev = [0] * self.robot.nu + self.timing = {'workspace': 0, 'target': 0, 'mpc': 0} + + def compute_u(self, x, pg, obstacles): + p = self.robot.h(x) + + # Initialize rs to robot position + if self.rs is None: + self.rs = p + # Update obstacles + if not self.params['use_prev_workspace']: + self.free_star = None + self.obstacle_clusters, self.ri, self.rg, self.rho, self.free_star, self.timing['workspace'], exit_flag = \ + workspace_modification(obstacles, p, pg, self.rs, self.params['rho0'], self.params['max_obs_compute_time'], + self.params['hull_epsilon'], self.params['gamma'], + make_convex=self.params['make_convex'], previous_obstacle_clusters=self.obstacle_clusters, + free_star_prev=self.free_star, verbosity=self.verbosity) + self.obstacles_star = [o.cluster_obstacle for o in self.obstacle_clusters] + + # Buffer previous target path + P_prev, s_prev = None, None + if self.params['buffer'] and self.target_path: + P_prev = np.array([self.target_path[::2], self.target_path[1::2]]).T + # Shift path to start closest to current rs + P_prev = P_prev[np.argmin(np.linalg.norm(self.rs - P_prev, axis=1)):, :] + P_prev[0, :] = self.rs + + # ||self.rs - p|| < rho from construction + for r in P_prev: + if any([o.interior_point(r) for o in self.obstacles_star]): + if self.verbosity > 0: + print("[Path Generator]: No reuse of previous path. Path not collision-free.") + P_prev = None + break + + # P_prev = None + if P_prev is not None: + # Cut off stand still padding in previous path + P_prev_stepsize = np.linalg.norm(np.diff(P_prev, axis=0), axis=1) + s_prev = np.hstack((0, np.cumsum(P_prev_stepsize) / self.params['dp_max'])) + P_prev_mask = [True] + (P_prev_stepsize > 1e-8).tolist() + P_prev = P_prev[P_prev_mask, :] + s_prev = s_prev[P_prev_mask] + + # Make sure all polygon representations are computed + [o._compute_polygon_representation() for o in self.obstacles_star] + + # Check for convergence + if np.linalg.norm(np.array(p) - np.array(pg)) < self.params['convergence_margin']: + self.timing['target'] = 0 + self.timing['mpc'] = 0 + return np.zeros(self.robot.nu) + + # Update target path + self.path_pol, self.epsilon, self.target_path, self.timing['target'] = \ + path_generator(self.ri, self.rg, self.obstacles_star, self.params['dp_max'], self.params['N'], + self.params['dt'], self.params['max_compute_time'], self.params['n_pol'], + ds_decay_rate=0.5, ds_increase_rate=2., max_nr_steps=1000, P_prev=P_prev, s_prev=s_prev, + reactivity=self.params['reactivity'], crep=self.params['crep'], + convergence_tolerance=self.params['convergence_tolerance'], verbosity=self.verbosity) + + # Compute MPC solution + t0 = tic() + # Set parameter for tracking error constraint + if self.rho > 0: + e_max = self.rho - max(self.epsilon) + else: + e_max = 1.e6 + + self.s_kappa = self.params['lambda'] * self.rho / self.params['dp_max'] + self.solution = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, e_max, self.rg.tolist(), self.s_kappa, obstacles, verbosity=self.verbosity) + self.u_prev = self.solution['u'][:self.robot.nu] + self.timing['mpc'] = toc(t0) + + # Update rs + self.rs = self.mpc.ref(self.path_pol, self.solution['s'][1]) + + # Extract first control signal + return np.array(self.solution['u'][:self.mpc.build_params['nu']]) + diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py new file mode 100644 index 0000000..920277d --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py @@ -0,0 +1,66 @@ +import numpy as np +from starworlds.starshaped_hull import cluster_and_starify, ObstacleCluster +from starworlds.utils.misc import tic, toc +import shapely + +# TODO: Check why computational time varies so much for same static scene +def workspace_modification(obstacles, p, pg, rs, rho0, max_compute_time, hull_epsilon, gamma=0.5, make_convex=0, + previous_obstacle_clusters=None, free_star_prev=None, verbosity=0): + + # Clearance variable initialization + rho = rho0 / gamma # First rho should be rho0 + t_init = tic() + + while True: + if toc(t_init) > max_compute_time: + if verbosity > 0: + print("[Workspace modification]: Max compute time in rho iteration.") + break + + # Reduce rho + rho *= gamma + + # Pad obstacles with rho + obstacles_rho = [o.dilated_obstacle(padding=rho, id="duplicate") for o in obstacles] + + # TODO: Fix boundaries + free_rho = shapely.geometry.box(-20, -20, 20, 20) + for o in obstacles_rho: + free_rho = free_rho.difference(o.polygon()) + + # TODO: Check buffering fix + # Find P0 + Bp = shapely.geometry.Point(p).buffer(0.95 * rho) + initial_reference_set = Bp.intersection(free_rho.buffer(-0.1 * rho)) + + if not initial_reference_set.is_empty: + break + + # Initial and goal reference position selection + r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(rs)) + r0 = np.array(r0_sh.coords[0]) + rg_sh, _ = shapely.ops.nearest_points(free_rho, shapely.geometry.Point(pg)) + rg = np.array(rg_sh.coords[0]) + + + # TODO: Check more thoroughly + if free_star_prev is not None: + free_star_prev = free_star_prev.buffer(-1e-4) + if free_star_prev is not None and free_star_prev.contains(r0_sh) and free_star_prev.contains(rg_sh) and free_rho.contains(free_star_prev):# not any([free_star_prev.covers(o.polygon()) for o in obstacles_rho]): + if verbosity > 1: + print("[Workspace modification]: Reuse workspace from previous time step.") + obstacle_clusters = previous_obstacle_clusters + exit_flag = 10 + else: + # Apply cluster and starify + obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles_rho, r0, rg, + hull_epsilon, max_compute_time-toc(t_init), + previous_clusters=previous_obstacle_clusters, + make_convex=make_convex, verbose=verbosity) + + free_star = shapely.geometry.box(-20, -20, 20, 20) + for o in obstacle_clusters: + free_star = free_star.difference(o.polygon()) + + compute_time = toc(t_init) + return obstacle_clusters, r0, rg, rho, free_star, compute_time, exit_flag \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py new file mode 100644 index 0000000..a0f0b8a --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py @@ -0,0 +1,7 @@ +""" +The :mod:`robot` module implements mobile robots for obstacle avoidance. +""" +from .mobile_robot import MobileRobot +from .unicycle import Unicycle +from .bicycle import Bicycle +from .omnidirectional import Omnidirectional \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py new file mode 100644 index 0000000..2aa541a --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py @@ -0,0 +1,55 @@ +import numpy as np +from robot import MobileRobot + + +class Bicycle(MobileRobot): + + def __init__(self, width, vel_min=None, vel_max=None, steer_max=None, name='robot'): + self.vmax = vel_max[0] + x_min = [-np.inf] * 3 + [-steer_max] + x_max = [np.inf] * 3 + [steer_max] + super().__init__(nu=2, nx=4, width=width, name=name, u_min=vel_min, u_max=vel_max, x_min=x_min, x_max=x_max) + + # [x,y of backwheel, orientation, steering angle] + # def f(self, x, u): + # return [u[0] * np.cos(x[2]), + # u[0] * np.sin(x[2]), + # u[0] * np.tan(u[1]) / self.width] + def f(self, x, u): + return [u[0] * np.cos(x[2]), + u[0] * np.sin(x[2]), + u[0] * np.tan(u[1]) / self.width, + u[1]] + # def f(self, x, u): + # return [u[0] * np.cos(x[2]) * np.cos(x[3]), + # u[0] * np.sin(x[2]) * np.cos(x[3]), + # u[0] * np.sin(x[3]) / self.width, + # u[1]] + + def h(self, x): + return [x[0] + self.width/2 * np.cos(x[2]), + x[1] + self.width/2 * np.sin(x[2])] + + def vel_min(self): + return self.u_min + + def vel_max(self): + return self.u_max + + def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10): + h = super(Bicycle, self).init_plot(ax=ax, color=color, alpha=alpha) + h += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color) + h += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w') + h += ax.plot(0, 0, color=color, alpha=alpha) + return h + + def update_plot(self, x, handles): + super(Bicycle, self).update_plot(x, handles) + handles[1].set_data(self.h(x)) + handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2))) + handles[1].set_markersize(handles[1].get_markersize()) + handles[2].set_data(x[0], x[1]) + handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2))) + handles[2].set_markersize(handles[2].get_markersize()) + handles[3].set_data([x[0], x[0] + self.width * np.cos(x[2])], + [x[1], x[1] + self.width * np.sin(x[2])]) \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py new file mode 100644 index 0000000..759a37e --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py @@ -0,0 +1,62 @@ +import matplotlib.pyplot as plt +import matplotlib.patches as patches +from abc import ABC, abstractmethod +import numpy as np + + +class MobileRobot(ABC): + + def __init__(self, nu, nx, width, name, u_min=None, u_max=None, x_min=None, x_max=None): + self.nx = nx + self.nu = nu + self.width = width + self.name = name + def valid_u_bound(bound): return bound is not None and len(bound) == self.nu + def valid_q_bound(bound): return bound is not None and len(bound) == self.nx + self.u_min = u_min if valid_u_bound(u_min) else [-np.inf] * self.nu + self.u_max = u_max if valid_u_bound(u_max) else [np.inf] * self.nu + self.x_min = x_min if valid_q_bound(x_min) else [-np.inf] * self.nx + self.x_max = x_max if valid_q_bound(x_max) else [np.inf] * self.nx + + @abstractmethod + def f(self, x, u): + pass + + @abstractmethod + def h(self, q): + pass + + @abstractmethod + def vel_min(self): + pass + + @abstractmethod + def vel_max(self): + pass + + def move(self, x, u, dt): + u_sat = np.clip(u, self.u_min, self.u_max) + x_next = x + np.array(self.f(x, u_sat)) * dt + x_next = np.clip(x_next, self.x_min, self.x_max) + return x_next, u_sat + + def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10): + if ax is None: + _, ax = plt.subplots(subplot_kw={'aspect': 'equal'}) + if self.width == 0: + handles = plt.plot(0, 0, '+', color=color, alpha=alpha, markersize=markersize, label=self.name) + else: + handles = [patches.Circle((0, 0), self.width / 2, color=color, alpha=alpha, label=self.name)] + ax.add_patch(handles[0]) + return handles, ax + + def update_plot(self, x, handles): + if self.width == 0: + handles[0].set_data(*self.h(x)) + else: + handles[0].set(center=self.h(x)) + + def draw(self, x, **kwargs): + handles, ax = self.init_plot(**kwargs) + self.update_plot(x, handles) + return handles, ax diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py new file mode 100644 index 0000000..52592b3 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py @@ -0,0 +1,23 @@ +import numpy as np +from robot import MobileRobot + + +class Omnidirectional(MobileRobot): + + def __init__(self, width, vel_max, name='robot'): + vel_max = vel_max * np.ones(2) + self.vmax = np.linalg.norm(vel_max) + super().__init__(nu=2, nx=2, width=width, name=name, u_min=(-vel_max).tolist(), u_max=(vel_max).tolist()) + + def f(self, x, u): + return [u[0], + u[1]] + + def h(self, x): + return x[:2] + + def vel_min(self): + return self.u_min + + def vel_max(self): + return self.u_max diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py new file mode 100644 index 0000000..2891df2 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py @@ -0,0 +1,38 @@ +import numpy as np +from robot import MobileRobot + + +class Unicycle(MobileRobot): + + def __init__(self, width, vel_min=None, vel_max=None, name='robot'): + self.vmax = vel_max[0] + super().__init__(nu=2, nx=3, width=width, name=name, u_min=vel_min, u_max=vel_max) + + def f(self, x, u): + return [u[0] * np.cos(x[2]), + u[0] * np.sin(x[2]), + u[1]] + + def h(self, x): + return x[:2] + + def vel_min(self): + return self.u_min + + def vel_max(self): + return self.u_max + + def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10): + handles, ax = super(Unicycle, self).init_plot(ax=ax, color=color, alpha=alpha) + handles += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color) + handles += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w') + return handles, ax + + def update_plot(self, x, handles): + super(Unicycle, self).update_plot(x, handles) + handles[1].set_data(x[0], x[1]) + handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2))) + handles[1].set_markersize(handles[1].get_markersize()) + handles[2].set_data(x[0], x[1]) + handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2))) + handles[2].set_markersize(handles[2].get_markersize()) diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py new file mode 100644 index 0000000..821dd31 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py @@ -0,0 +1,15 @@ +from setuptools import setup, find_packages + +setup(name='starworld_tunnel_mpc', + version='1.0', + packages=find_packages(), + install_requires=[ + 'pyyaml', + 'numpy', + 'scipy', + 'matplotlib', + 'shapely', + 'casadi', + 'opengen' + ] +) diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py new file mode 100755 index 0000000..c596487 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py @@ -0,0 +1,75 @@ +import numpy as np +import matplotlib.pyplot as plt +from config.load_config import load_config +from visualization import TunnelMPCGUI + +ctrl_param_file = 'tunnel_mpc_params.yaml' +ctrl_param_file = 'tunnel_mpc_convergence_params.yaml' +scene_id = None +verbosity = 0 + +scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=1, scene_id=scene_id, verbosity=verbosity) +gui = TunnelMPCGUI(robot, scene.obstacles, x0, scene.pg, scene.xlim, scene.ylim, + mpc_horizon=controller.params['N'], rho0=controller.params['rho0'], # Comment out to skip mpc solution figure + robot_alpha=0., mpc_path_linestyle='--', mpc_path_linewidth=4, target_path_linewidth=2, + goal_color='k', robot_color='orange') + +# Initialize +T_max = 30 +dt = controller.params['dt'] +t = 0. +x = x0 +u_prev = np.zeros(robot.nu) +convergence_threshold = 0.05 +converged = False +timing = {'workspace': [], 'target': [], 'mpc': [], 'tot': []} + +while gui.fig_open and not converged: + + if t < T_max and (not gui.paused or gui.step_once): + gui.step_once = False + p = robot.h(x) + # Move obstacles + scene.step(dt, np.array([p]), robot.width/2) + # Compute mpc + u = controller.compute_u(x, scene.pg, scene.obstacles) + + # Update timing + tot_time = 0 + for k, v in controller.timing.items(): + timing[k] += [v] + tot_time += v + timing['tot'] += [tot_time] + + # Update plots + mpc_path = [el for x in controller.solution['x'] for el in robot.h(x)] + e_max = controller.rho - max(controller.epsilon) + s_kappa = controller.s_kappa if hasattr(controller,'s_kappa') else None + gui.update(robot_state=x, obstacles_star=controller.obstacles_star, time=t, target_path=controller.target_path, + mpc_path=mpc_path, e_max=e_max, timing=controller.timing, e=controller.solution['e'], + s=controller.solution['s'], u=u_prev.tolist() + controller.solution['u'], rho=controller.rho, s_kappa=s_kappa) + + # Integrate robot state with new control signal + x, _ = robot.move(x, u, dt) + u_prev = u + t += dt + + converged = np.linalg.norm(robot.h(x)-scene.pg) < convergence_threshold + + if t >= T_max or converged: + gui.ax.set_title("Time: {:.1f} s. Finished".format(t)) + gui.fig.canvas.draw() + + plt.pause(0.005) + +gui.update(robot_state=x, obstacles_star=controller.obstacles_star) + +# fig, ax = plt.subplots() +# ax.plot(timing['workspace']) +# ax.plot(timing['target']) +# ax.plot(timing['mpc']) +# ax.plot(timing['tot']) +# ax.legend(['Workspace modification', 'Target path generation', 'MPC', 'Total']) + +# Wait until figure closed when converged +plt.show() diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py new file mode 100755 index 0000000..0b5f404 --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py @@ -0,0 +1,176 @@ +import numpy as np +import matplotlib.pyplot as plt +# from config.load_config import load_config +# from videos.video_writer import VideoWriter +from motion_control.soads import SoadsController +from config.load_config import load_config + +make_video = 0 + +ctrl_param_file = 'soads_ctrl_params.yaml' +scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=0) +controller.verbose = True + +# Initialize +T_max = 30 +dt = controller.params['dt'] +K = int(T_max / dt) +u = np.zeros((robot.nu, K)) +x = np.zeros((x0.size, K+1)) +x[:, 0] = x0 +pg = scene.pg +timing_history = {'obstacle': [], 'control': []} +previous_path = [] +convergence_threshold = 0.01 +converged = False +paused = True +step_once = False +i = 0 + +# Init video writing +if make_video: + video_name = "mpc_" + str(scene.id) + "_" + robot.__class__.__name__ + video_name = input("Video file name: ") + video_writer = VideoWriter(video_name) + paused = False + +# Init plotting +fig_scene, ax_scene = plt.subplots() +ax_scene.set_xlabel('x1 [m]', fontsize=15) +ax_scene.set_ylabel('x2 [m]', fontsize=15) +travelled_path_handle = ax_scene.plot([], [], 'k-', linewidth=2)[0] +goal_handle = ax_scene.plot(*scene.pg, 'g*', markersize=16)[0] +obstacle_handles, _ = scene.init_plot(ax=ax_scene, draw_p0=0, draw_pg=0, draw_streamlines=0, show_obs_name=0) +scene.update_plot(obstacle_handles) +robot.width = 0 +robot_handles, _ = robot.init_plot(ax=ax_scene, color='y', alpha=1, markersize=16) +robot.update_plot(x0, robot_handles) +obstacle_star_handles = [] +color_list = plt.cm.gist_ncar(np.linspace(0, 1, len(scene.obstacles))) + +streamplot_handle = None + +fig_open = True + +def on_close(event): + global fig_open + fig_open = False + +def on_press(event): + global paused, step_once, streamplot_handle + if event.key == ' ': + paused = not paused + elif event.key == 'right': + step_once = True + paused = True + elif event.key == 't': + fig_timing, ax = plt.subplots() + ax.plot(timing_history['obstacle'], '-o', label='obstacle') + ax.plot(timing_history['control'], '-o', label='control') + fig_timing.canvas.draw() + elif event.key == 'w': + ax_scene.axis('off') + ax_scene.title.set_visible(False) + file_name = input("-------------\nFile name: ") + fig_scene.savefig("utils/" + file_name, transparent=True) + ax_scene.axis('on') + ax_scene.title.set_visible(True) + elif event.key == 'a': + from motion_control.path_generator import plot_vector_field + n = int(input("-------------\nStreamplot resolution: ")) + streamplot_handle = plot_vector_field(scene.pg, controller.obstacles_star, ax_scene, n=n, color='orange') + else: + print(event.key) + +def remove_local_plots(): + global streamplot_handle + # Remove streamplot + if streamplot_handle is not None: + from matplotlib.patches import FancyArrowPatch + streamplot_handle.lines.remove() + for art in ax_scene.get_children(): + if not isinstance(art, FancyArrowPatch): + continue + art.remove() # Method 1 + streamplot_handle = None + +fig_scene.canvas.mpl_connect('close_event', on_close) +fig_scene.canvas.mpl_connect('key_press_event', on_press) + +ls = [] + +while fig_open and not converged: + + if i < K and (not paused or step_once): + p = robot.h(x[:, i]) + step_once = False + # Move obstacles + scene.step(dt, np.array([p]), 1e-2) + # Compute mpc + u[:, i] = controller.compute_u(x[:, i], pg, scene.obstacles) + # Integrate robot state with new control signal + x[:, i+1], _ = robot.move(x[:, i], u[:, i], dt) + # Add timing to history + for k in timing_history.keys(): + timing_history[k] += [controller.timing[k]] + + # Update plots + robot.update_plot(x[:, i], robot_handles) + scene.update_plot(obstacle_handles) + [h.remove() for h in obstacle_star_handles if h is not None] + obstacle_star_handles = [] + for j, cl in enumerate(controller.obstacle_clusters): + for o in cl.obstacles: + lh, _ = o.draw(ax=ax_scene, fc='lightgrey', show_reference=False) + obstacle_star_handles += lh + for o in controller.obstacles_star: + lh, _ = o.draw(ax=ax_scene, fc='red', show_reference=False, alpha=0.9, zorder=0) + obstacle_star_handles += lh + lh = ax_scene.plot(*o.xr(), 'gd', markersize=8) + obstacle_star_handles += lh + + # if ls: + # [l.remove() for l in ls] + # ls = [] + # for o in controller.obstacles_star: + # b = o.boundary_mapping(x[:, i]) + # dx_o = f(x[:, i], pg, [o]) + # ls += ax_scene.plot(*b, 'yx') + # ls += [ax_scene.quiver(*b, *dx_o, zorder=3, color='y')] + + travelled_path_handle.set_data(x[0, :i+1], x[1, :i+1]) + + ax_scene.set_title("Time: {:.1f} s".format(i*dt)) + + i += 1 + + if make_video: + video_writer.add_frame(fig_scene) + if not i % 10: + print(f'[VideoWriter] wrote frame {i}/{K}') + else: + fig_scene.canvas.draw() + + converged = np.linalg.norm(robot.h(x[:, i])-pg) < convergence_threshold + + if i == K or converged: + if make_video: + # close video writer + video_writer.release() + print("Finished") + fig_open = False + else: + ax_scene.set_title("Time: {:.1f} s. Finished".format(i * dt)) + fig_scene.canvas.draw() + + if not make_video: + plt.pause(0.005) + + +ot = timing_history['obstacle'] +print("Timing\n-----\nMean: {:.2f}\nMax: {:.2f}\nStdDev: {:.2f}".format(np.mean(ot), np.max(ot), np.std(ot))) + +# Wait until figure closed when converged +# if fig_open: +if not make_video: + plt.show() diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py new file mode 100644 index 0000000..2a52f2a --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py @@ -0,0 +1 @@ +from .scene_gui import SceneGUI, SoadsGUI, TunnelMPCGUI \ No newline at end of file diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py new file mode 100644 index 0000000..bc811eb --- /dev/null +++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py @@ -0,0 +1,182 @@ +import matplotlib.pyplot as plt +import shapely +import numpy as np + + +class SceneGUI: + + def __init__(self, robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis=False, + robot_color='k', robot_markersize=14, robot_alpha=0.7, + obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False, + goal_color='y', goal_marker='*', goal_markersize=16, + travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2): + self.obstacles = obstacles + self.robot = robot + self.fig, self.ax = plt.subplots() + self.ax.set_xlim(xlim), self.ax.set_ylim(ylim) + if not show_axis: + self.ax.set_axis_off() + self.goal_handle = self.ax.plot(*goal, color=goal_color, marker=goal_marker, markersize=goal_markersize)[0] + self.robot_handles, _ = robot.init_plot(ax=self.ax, color=robot_color, markersize=robot_markersize, alpha=robot_alpha) + self.obstacle_handles = [] + for o in obstacles: + lh, _ = o.init_plot(ax=self.ax, fc=obstacle_color, ec=obstacle_edge_color, show_name=show_obs_name, + show_reference=False) + self.obstacle_handles.append(lh) + self.travelled_path_handle = self.ax.plot([], [], color=travelled_path_color, linestyle=travelled_path_linestyle, linewidth=travelled_path_linewidth, zorder=0)[0] + # Simulation ctrl + self.fig_open = True + self.paused = True + self.step_once = False + self.fig.canvas.mpl_connect('close_event', self.on_close) + self.fig.canvas.mpl_connect('key_press_event', self.on_press) + # Memory + self.travelled_path = [] + # + # self.update(init_robot_state, goal, 0) + + def update(self, robot_state=None, goal=None, time=None): + if time: + self.ax.set_title("Time: {:.1f} s".format(time)) + + # Obstacles + [oh[0].update_plot(oh[1]) for oh in zip(self.obstacles, self.obstacle_handles)] + + # Robot and goal position + if robot_state is not None: + self.travelled_path += list(self.robot.h(robot_state)) + self.robot.update_plot(robot_state, self.robot_handles) + self.travelled_path_handle.set_data(self.travelled_path[::2], self.travelled_path[1::2]) + if goal is not None: + self.goal_handle.set_data(*goal) + + + def on_close(self, event): + self.fig_open = False + + def on_press(self, event): + if event.key == ' ': + self.paused = not self.paused + elif event.key == 'right': + self.step_once = True + self.paused = True + else: + print(event.key) + + +class SoadsGUI(SceneGUI): + def __init__(self, robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis=False, + robot_color='k', robot_markersize=14, robot_alpha=0.7, + obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False, + goal_color='y', goal_marker='*', goal_markersize=16, + travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2, + obstacles_star_color='r', obstacles_star_show_reference=True, obstacles_star_alpha=0.1): + self.obstacle_star_handles = [] + self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference, + 'alpha': obstacles_star_alpha, 'zorder': 0} + super().__init__(robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis, robot_color, robot_markersize, + robot_alpha, obstacle_color, obstacle_edge_color, show_obs_name, goal_color, goal_marker, + goal_markersize, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth) + + def update(self, robot_state=None, goal=None, time=None, obstacles_star=None): + # SceneFig update + super().update(robot_state, goal, time) + # Star obstacles + [h.remove() for h in self.obstacle_star_handles if h is not None] + self.obstacle_star_handles = [] + if obstacles_star is not None: + for o in obstacles_star: + lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options) + self.obstacle_star_handles += lh + +class TunnelMPCGUI(SceneGUI): + + def __init__(self, robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis=False, + robot_color='y', robot_markersize=14, robot_alpha=0.7, + obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False, + goal_color='g', goal_marker='*', goal_markersize=16, + travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2, + target_path_color='g', target_path_linestyle='-', target_path_linewidth=4, target_path_marker=None, + mpc_path_color='k', mpc_path_linestyle=':', mpc_path_linewidth=4, + mpc_tunnel_color='r', mpc_tunnel_linestyle='--', mpc_tunnel_linewidth=2, + obstacles_star_color='r', obstacles_star_show_reference=False, obstacles_star_alpha=0.1, + mpc_horizon=None, rho0=None): + self.obstacle_star_handles = [] + self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference, + 'alpha': obstacles_star_alpha, 'zorder': 0} + super().__init__(robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis, robot_color, robot_markersize, + robot_alpha, obstacle_color, obstacle_edge_color, show_obs_name, goal_color, goal_marker, + goal_markersize, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth) + self.target_path_handle = self.ax.plot([], [], color=target_path_color, linestyle=target_path_linestyle, linewidth=target_path_linewidth, marker=target_path_marker, zorder=0)[0] + self.mpc_path_handle = self.ax.plot([], [], color=mpc_path_color, linestyle=mpc_path_linestyle, linewidth=mpc_path_linewidth, zorder=0, dashes=(0.8, 0.8))[0] + self.mpc_tunnel_handle = self.ax.plot([], [], color=mpc_tunnel_color, linestyle=mpc_tunnel_linestyle, linewidth=mpc_tunnel_linewidth, zorder=0)[0] + + self.fig_mpc_solution = None + if mpc_horizon: + self.fig_mpc_solution, self.ax_mpc_solution = plt.subplots(2, 2) + self.s_handle = self.ax_mpc_solution[0, 0].plot(np.linspace(1, mpc_horizon, mpc_horizon), [None] * mpc_horizon, '-o')[0] + self.skappa_handle = self.ax_mpc_solution[0, 0].plot([0, mpc_horizon], [None, None], 'r--')[0] + self.ax_mpc_solution[0, 0].plot([0, mpc_horizon], [mpc_horizon, mpc_horizon], 'r--') + self.ax_mpc_solution[0, 0].set_ylim(0, 1.1*mpc_horizon) + self.rho_handle = self.ax_mpc_solution[1, 0].plot([0, mpc_horizon], [rho0, rho0], 'k--')[0] + self.emax_handle = self.ax_mpc_solution[1, 0].plot([0, mpc_horizon], [None, None], 'r--')[0] + self.e_handle = self.ax_mpc_solution[1, 0].plot(np.linspace(0, mpc_horizon, mpc_horizon+1, '-o'), [None] * (mpc_horizon+1), '-o')[0] + self.ax_mpc_solution[1, 0].set_ylim(0, 1.1*rho0) + # Assumes 2 control signals + self.u1_handle = self.ax_mpc_solution[0, 1].plot(np.linspace(0, mpc_horizon, mpc_horizon+1), [None] * (mpc_horizon + 1), '-o')[0] + self.ax_mpc_solution[0, 1].plot([0, mpc_horizon], [robot.u_min[0], robot.u_min[0]], 'r--') + self.ax_mpc_solution[0, 1].plot([0, mpc_horizon], [robot.u_max[0], robot.u_max[0]], 'r--') + self.u2_handle = self.ax_mpc_solution[1, 1].plot(np.linspace(0, mpc_horizon, mpc_horizon+1), [None] * (mpc_horizon + 1), '-o')[0] + self.ax_mpc_solution[1, 1].plot([0, mpc_horizon], [robot.u_min[1], robot.u_min[1]], 'r--') + self.ax_mpc_solution[1, 1].plot([0, mpc_horizon], [robot.u_max[1], robot.u_max[1]], 'r--') + + self.update(init_robot_state, goal, 0) + + def update(self, robot_state=None, goal=None, time=None, obstacles_star=None, target_path=None, mpc_path=None, + s=None, u=None, e=None, rho=None, e_max=None, s_kappa=None, timing=None): + # SceneFig update + super().update(robot_state, goal, time) + + if timing: + if time: + self.ax.set_title("Time: {:.1f} s\nCompute Time ({:.1f} / {:.1f} / {:.1f})".format(time, timing['workspace'], timing['target'], timing['mpc'])) + else: + self.ax.set_title("Compute Time ({:.1f} / {:.1f} / {:.1f})".format(timing['workspace'], timing['target'], timing['mpc'])) + + # Star obstacles + [h.remove() for h in self.obstacle_star_handles if h is not None] + self.obstacle_star_handles = [] + if obstacles_star is not None: + for o in obstacles_star: + lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options) + self.obstacle_star_handles += lh + + # Trajectories + if mpc_path is not None: + self.mpc_path_handle.set_data(mpc_path[::2], mpc_path[1::2]) + if target_path is not None: + self.target_path_handle.set_data(target_path[::2], target_path[1::2]) + if e_max is not None: + tunnel_polygon = shapely.geometry.LineString(list(zip(target_path[::2], target_path[1::2]))).buffer( + e_max) + if tunnel_polygon.geom_type == 'Polygon': + self.mpc_tunnel_handle.set_data(*tunnel_polygon.exterior.xy) + else: + print("[SceneFig]: Tunnel polygon not polygon.") + print(tunnel_polygon) + + # MPC solution plot + if self.fig_mpc_solution is not None: + if s is not None: + self.s_handle.set_ydata(s[1:]) + if s_kappa is not None: + self.skappa_handle.set_ydata([s_kappa, s_kappa]) + if e is not None: + self.e_handle.set_ydata(e) + if e_max is not None: + self.emax_handle.set_ydata([e_max, e_max]) + if rho is not None: + self.rho_handle.set_ydata([rho, rho]) + if u is not None: + self.u1_handle.set_ydata(u[::2]) + self.u2_handle.set_ydata(u[1::2]) -- GitLab