From f4673325308c6eacebbba460c6e20126a80415ba Mon Sep 17 00:00:00 2001 From: kpbaks Date: Fri, 14 Jun 2024 14:21:01 +0200 Subject: [PATCH] feat!: allow formations with 0 robots --- config/simulations/Bad/config.toml | 103 ----- config/simulations/Bad/environment.yaml | 76 ---- config/simulations/Bad/formation.yaml | 29 -- .../Collaborative GP High Qin/config.toml | 119 ------ .../environment.yaml | 18 - .../Collaborative GP High Qin/formation.yaml | 356 ----------------- .../Collaborative GP Low Qin/environment.yaml | 18 - .../Collaborative GP Low Qin/formation.yaml | 364 ------------------ config/simulations/Complex/config.toml | 116 ------ config/simulations/Complex/environment.yaml | 18 - config/simulations/Complex/formation.yaml | 118 ------ config/simulations/Intersection/config.toml | 111 ------ .../Junction Experiment/config.toml | 123 +++--- .../Junction Experiment/formation.yaml | 124 +----- config/simulations/Malthe/config.toml | 95 ----- config/simulations/Malthe/environment.yaml | 31 -- config/simulations/Malthe/formation.yaml | 31 -- .../config.toml | 0 .../environment.yaml | 0 .../formation.yaml | 2 +- config/simulations/Solo GP/config.toml | 122 +++--- config/simulations/Solo GP/formation.yaml | 2 +- .../config.toml | 56 +-- .../environment.yaml | 0 .../formation.yaml | 0 crates/gbp_config/src/formation.rs | 23 +- crates/magics/src/main.rs | 8 +- crates/magics/src/planner/robot.rs | 32 +- crates/magics/src/planner/spawner.rs | 27 +- 29 files changed, 213 insertions(+), 1909 deletions(-) delete mode 100644 config/simulations/Bad/config.toml delete mode 100644 config/simulations/Bad/environment.yaml delete mode 100644 config/simulations/Bad/formation.yaml delete mode 100644 config/simulations/Collaborative GP High Qin/config.toml delete mode 100644 config/simulations/Collaborative GP High Qin/environment.yaml delete mode 100644 config/simulations/Collaborative GP High Qin/formation.yaml delete mode 100644 config/simulations/Collaborative GP Low Qin/environment.yaml delete mode 100644 config/simulations/Collaborative GP Low Qin/formation.yaml delete mode 100644 config/simulations/Complex/config.toml delete mode 100644 config/simulations/Complex/environment.yaml delete mode 100644 config/simulations/Complex/formation.yaml delete mode 100644 config/simulations/Intersection/config.toml delete mode 100644 config/simulations/Malthe/config.toml delete mode 100644 config/simulations/Malthe/environment.yaml delete mode 100644 config/simulations/Malthe/formation.yaml rename config/simulations/{test => Obstacle Shapes Showcase}/config.toml (100%) rename config/simulations/{test => Obstacle Shapes Showcase}/environment.yaml (100%) rename config/simulations/{test => Obstacle Shapes Showcase}/formation.yaml (98%) rename config/simulations/{Collaborative GP Low Qin => Tracking Factor Showcase}/config.toml (67%) rename config/simulations/{Intersection => Tracking Factor Showcase}/environment.yaml (100%) rename config/simulations/{Intersection => Tracking Factor Showcase}/formation.yaml (100%) diff --git a/config/simulations/Bad/config.toml b/config/simulations/Bad/config.toml deleted file mode 100644 index 68eed37e..00000000 --- a/config/simulations/Bad/config.toml +++ /dev/null @@ -1,103 +0,0 @@ -# environment = "junction" -environment_image = "circle_cluttered" -environment = "./config/simulations/Circle/environment.yaml" -formation_group = "./config/simulations/Circle/formation.ron" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 250.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 300.0 - -[visualisation.height] -objects = 0.5 -height-map = 1.0 - -[visualisation.draw] -robots = true -communication-graph = false -predicted-trajectories = true -waypoints = true -uncertainty = true -paths = true -generated-map = true -height-map = false -sdf = false -communication-radius = false -obstacle-factors = false -tracking = false -interrobot-factors = false -interrobot-factors-safety-distance = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = true -robot-environment-collisions = true - -[gbp] -sigma-pose-fixed = 0.000000000000001 -sigma-factor-dynamics = 0.1 -sigma-factor-interrobot = 0.01 -sigma-factor-obstacle = 0.01 -sigma-factor-tracking = 0.1 -# iterations-per-timestep = 10 -lookahead-multiple = 3 - - -[gbp.iteration-schedule] -internal = 10 -external = 10 -schedule = "interleave-evenly" - -[robot] -planning-horizon = 5.0 -target-speed = 4.0 -inter-robot-safety-distance-multiplier = 2.2 - -[robot.radius] -min = 2.0 -max = 3.0 - -[robot.communication] -radius = 20.0 -failure-rate = 0.2 - -[simulation] -# t0 = 0.25 -max-time = 10000.0 -time-scale = 1.0 -manual-step-factor = 1 -hz = 60.0 -# world-size = 100.0 -prng-seed = 0 -pause-on-spawn = false -start-paused = false -despawn-robot-when-final-waypoint-reached = false - -[rrt] -max-iterations = 1000000 -step-size = 0.5 -collision-radius = 0.1 -neighbourhood-radius = 10.0 - -[rrt.smoothing] -enabled = true -max-iterations = 500 -step-size = 0.5 - -[graphviz] -export-location = "./assets/export/" - -[graphviz.interrobot.active] -style = "dashed" -len = 8.0 -color = "red" - -[graphviz.interrobot.inactive] -style = "dashed" -len = 8.0 -color = "gray" - -[manual] -timesteps-per-step = 1 diff --git a/config/simulations/Bad/environment.yaml b/config/simulations/Bad/environment.yaml deleted file mode 100644 index 4199abaa..00000000 --- a/config/simulations/Bad/environment.yaml +++ /dev/null @@ -1,76 +0,0 @@ -tiles: - grid: - - █ - settings: - tile-size: 100.0 - path-width: 0.1 - obstacle-height: 1.0 - sdf: - resolution: 200 - expansion: 0.1 - blur: 0.01 -obstacles: -- shape: !regular-polygon - sides: 4 - radius: 0.0525 - translation: - x: 0.625 - y: 0.60125 - rotation: 0.0 - tile-coordinates: - row: 0 - col: 0 -- shape: !regular-polygon - sides: 4 - radius: 0.035 - translation: - x: 0.44125 - y: 0.57125 - rotation: 0.0 - tile-coordinates: - row: 0 - col: 0 -- shape: !regular-polygon - sides: 4 - radius: 0.0225 - translation: - x: 0.4835 - y: 0.428 - rotation: 0.0 - tile-coordinates: - row: 0 - col: 0 -- shape: !rectangle - width: 0.0875 - height: 0.035 - translation: - x: 0.589 - y: 0.3965 - rotation: 0.0 - tile-coordinates: - row: 0 - col: 0 -- shape: !triangle - angles: - A: 0.5235987755982988 - B: 0.5235987755982988 - radius: 0.05 - rotation: 0.0 - translation: - x: 0.5575 - y: 0.5145 - tile-coordinates: - row: 0 - col: 0 -- shape: !triangle - angles: - A: 1.9198621771937625 - B: 0.6981317007977318 - radius: 0.03 - rotation: 5.225 - translation: - x: 0.38 - y: 0.432 - tile-coordinates: - row: 0 - col: 0 diff --git a/config/simulations/Bad/formation.yaml b/config/simulations/Bad/formation.yaml deleted file mode 100644 index 8837bc8e..00000000 --- a/config/simulations/Bad/formation.yaml +++ /dev/null @@ -1,29 +0,0 @@ -formations: -- repeat-every: - secs: 8 - nanos: 0 - delay: - secs: 2 - nanos: 0 - robots: 1 - planning-strategy: only-local - initial-position: - shape: !line-segment - - x: 0.45 - y: 0.0 - - x: 0.55 - y: 0.0 - placement-strategy: equal - waypoints: - - shape: !line-segment - - x: 0.45 - y: 1.25 - - x: 0.55 - y: 1.25 - projection-strategy: identity - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: robot-radius - intersects-with: horizon diff --git a/config/simulations/Collaborative GP High Qin/config.toml b/config/simulations/Collaborative GP High Qin/config.toml deleted file mode 100644 index 42eebcdd..00000000 --- a/config/simulations/Collaborative GP High Qin/config.toml +++ /dev/null @@ -1,119 +0,0 @@ -# environment = "junction" -environment_image = "junction_twoway" -environment = "./config/simulations/Collaborative GP/environment.yaml" -formation_group = "./config/simulations/Collaborative GP/formation.ron" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 300.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 300.0 - -[visualisation.height] -objects = 0.5 -height-map = 2.0 - -[visualisation.draw] -robots = true -communication-graph = true -predicted-trajectories = true -waypoints = true -uncertainty = false -paths = true -generated-map = true -height-map = false -sdf = false -communication-radius = false -obstacle-factors = false -tracking = false -interrobot-factors = false -interrobot-factors-safety-distance = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = true -robot-environment-collisions = true - -[gbp.factors-enabled] -tracking = false -obstacle = true -dynamic = true -interrobot = true - -[gbp] -sigma-pose-fixed = 1e-15 -sigma-factor-dynamics = 0.5 -sigma-factor-interrobot = 0.01 -sigma-factor-obstacle = 0.01 -sigma-factor-tracking = 0.3 -lookahead-multiple = 3 - -[gbp.iteration-schedule] -internal = 10 -external = 10 -schedule = "interleave-evenly" - -[gbp.tracking] -attraction-distance = 1.0 -switch-padding = 5.0 - -[robot] -planning-horizon = 5.0 -target-speed = 7.0 -inter-robot-safety-distance-multiplier = 3.2 - -[robot.radius] -min = 2.0 -max = 2.0 - -[robot.communication] -radius = 20.0 -failure-rate = 0.0 - -[simulation] -# t0 = 0.25 -max-time = 10000.0 -time-scale = 1.0 -manual-step-factor = 1 -hz = 10.0 -# world-size = 100.0 -prng-seed = 2 -pause-on-spawn = false -despawn-robot-when-final-waypoint-reached = true - -[rrt] -max-iterations = 5000000 -step-size = 5.0 -collision-radius = 3.0 -neighbourhood-radius = 8.0 - -[rrt.smoothing] -enabled = true -max-iterations = 500 -step-size = 0.5 - - -[graphviz.interrobot.active] -style = "dashed" -len = 8.0 -color = "red" - -[graphviz.interrobot.inactive] -style = "dashed" -len = 8.0 -color = "gray" - -[graphviz] -export-location = "./assets/export/" - -[manual] -timesteps-per-step = 1 - -[debug.on-variable-clicked] -tracking = true -obstacle = true -dynamic = false -interrobot = false -variable = false -inbox = true diff --git a/config/simulations/Collaborative GP High Qin/environment.yaml b/config/simulations/Collaborative GP High Qin/environment.yaml deleted file mode 100644 index b4deb215..00000000 --- a/config/simulations/Collaborative GP High Qin/environment.yaml +++ /dev/null @@ -1,18 +0,0 @@ -tiles: - grid: - - ' ╷ ╷ ' - - ' ┌─┼─┼─┐┌╴' - - '╶┼─┘┌┼┬┼┘ ' - - '╶┴┬─┴┼┘│ ' - - ' ┌┴┐┌┼─┴┬╴' - - ' ├─┴┘├──┘ ' - - ' ╵ ╵ ' - settings: - tile-size: 25.0 - path-width: 0.5 - obstacle-height: 2.25 - sdf: - resolution: 200 - expansion: 0.2 - blur: 0.02 -obstacles: [] diff --git a/config/simulations/Collaborative GP High Qin/formation.yaml b/config/simulations/Collaborative GP High Qin/formation.yaml deleted file mode 100644 index 6f2f9ce0..00000000 --- a/config/simulations/Collaborative GP High Qin/formation.yaml +++ /dev/null @@ -1,356 +0,0 @@ -formations: - -# xmin = 0.075 -# ymin = 0.1 -# xmax = 0.925 -# ymax = 0.9 - -# Formation 1: brown east->north -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.925 - y: 0.357 - - x: 0.925 - y: 0.357 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.55 - y: 0.9 - - x: 0.55 - y: 0.9 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 2: yellow east->west -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.925 - y: 0.357 - - x: 0.925 - y: 0.357 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.15 - y: 0.1 - - x: 0.15 - y: 0.1 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 3: purple south->north-east -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.55 - y: 0.1 - - x: 0.55 - y: 0.1 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.925 - y: 0.786 - - x: 0.925 - y: 0.786 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 4: red south->north -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.55 - y: 0.1 - - x: 0.55 - y: 0.1 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.55 - y: 0.9 - - x: 0.55 - y: 0.9 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 5: blue south->north-west -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 10 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.55 - y: 0.1 - - x: 0.55 - y: 0.1 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.075 - y: 0.643 - - x: 0.075 - y: 0.643 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 6: orange west->north-east -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.075 - y: 0.5 - - x: 0.075 - y: 0.5 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.925 - y: 0.786 - - x: 0.925 - y: 0.786 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 7: green west->north -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.075 - y: 0.5 - - x: 0.075 - y: 0.5 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.55 - y: 0.9 - - x: 0.55 - y: 0.9 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 8: yellow north2->north-west -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.35 - y: 0.9 - - x: 0.35 - y: 0.9 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.075 - y: 0.643 - - x: 0.075 - y: 0.643 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 9: dark purple north2->west -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.35 - y: 0.9 - - x: 0.35 - y: 0.9 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.15 - y: 0.1 - - x: 0.15 - y: 0.1 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 - -# Formation 10: light blue north2->north-east -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 10 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.35 - y: 0.9 - - x: 0.35 - y: 0.9 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.925 - y: 0.786 - - x: 0.925 - y: 0.786 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 50 - intersects-with: !variable 5 diff --git a/config/simulations/Collaborative GP Low Qin/environment.yaml b/config/simulations/Collaborative GP Low Qin/environment.yaml deleted file mode 100644 index b4deb215..00000000 --- a/config/simulations/Collaborative GP Low Qin/environment.yaml +++ /dev/null @@ -1,18 +0,0 @@ -tiles: - grid: - - ' ╷ ╷ ' - - ' ┌─┼─┼─┐┌╴' - - '╶┼─┘┌┼┬┼┘ ' - - '╶┴┬─┴┼┘│ ' - - ' ┌┴┐┌┼─┴┬╴' - - ' ├─┴┘├──┘ ' - - ' ╵ ╵ ' - settings: - tile-size: 25.0 - path-width: 0.5 - obstacle-height: 2.25 - sdf: - resolution: 200 - expansion: 0.2 - blur: 0.02 -obstacles: [] diff --git a/config/simulations/Collaborative GP Low Qin/formation.yaml b/config/simulations/Collaborative GP Low Qin/formation.yaml deleted file mode 100644 index 6e647272..00000000 --- a/config/simulations/Collaborative GP Low Qin/formation.yaml +++ /dev/null @@ -1,364 +0,0 @@ -formations: - -# xmin = 0.075 -# ymin = 0.1 -# xmax = 0.925 -# ymax = 0.9 - -# Formation 1: brown east->north -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.925 - y: 0.357 - - x: 0.925 - y: 0.357 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.55 - y: 0.9 - - x: 0.55 - y: 0.9 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 10 - intersects-with: !variable 5 - -# Formation 2: yellow east->west -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.925 - y: 0.357 - - x: 0.925 - y: 0.357 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.15 - y: 0.1 - - x: 0.15 - y: 0.1 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 10 - intersects-with: !variable 5 - -# Formation 3: purple south->north-east -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.55 - y: 0.1 - - x: 0.55 - y: 0.1 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.925 - y: 0.786 - - x: 0.925 - y: 0.786 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 10 - intersects-with: current - -# Formation 4: red south->north -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.55 - y: 0.1 - - x: 0.55 - y: 0.1 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.55 - y: 0.9 - - x: 0.55 - y: 0.9 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 10 - intersects-with: current - -# Formation 5: blue south->north-west -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 10 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.55 - y: 0.1 - - x: 0.55 - y: 0.1 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.075 - y: 0.643 - - x: 0.075 - y: 0.643 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 10 - intersects-with: current - -# Formation 6: orange west->north-east -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.075 - y: 0.5 - - x: 0.075 - y: 0.5 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.925 - y: 0.786 - - x: 0.925 - y: 0.786 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: !meter 10 - intersects-with: current - -# Formation 7: green west->north -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.075 - y: 0.5 - - x: 0.075 - y: 0.5 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.55 - y: 0.9 - - x: 0.55 - y: 0.9 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - # distance: !meter 10 - # intersects-with: !variable 5 - distance: !meter 10 - intersects-with: current - -# Formation 8: yellow north2->north-west -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 0 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.35 - y: 0.9 - - x: 0.35 - y: 0.9 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.075 - y: 0.643 - - x: 0.075 - y: 0.643 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - # distance: !meter 10 - # intersects-with: !variable 5 - distance: !meter 10 - intersects-with: current - -# Formation 9: dark purple north2->west -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 5 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.35 - y: 0.9 - - x: 0.35 - y: 0.9 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.15 - y: 0.1 - - x: 0.15 - y: 0.1 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - # distance: !meter 10 - # intersects-with: !variable 5 - distance: !meter 10 - intersects-with: current - -# Formation 10: light blue north2->north-east -- repeat: - every: - secs: 15 - nanos: 0 - times: !finite 10 - delay: - secs: 10 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.35 - y: 0.9 - - x: 0.35 - y: 0.9 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.925 - y: 0.786 - - x: 0.925 - y: 0.786 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - # finished-when-intersects: !variable 5 - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - # distance: !meter 10 - # intersects-with: !variable 5 - distance: !meter 10 - intersects-with: current diff --git a/config/simulations/Complex/config.toml b/config/simulations/Complex/config.toml deleted file mode 100644 index 17875ee5..00000000 --- a/config/simulations/Complex/config.toml +++ /dev/null @@ -1,116 +0,0 @@ -environment_image = "junction_twoway" -environment = "./config/simulations/Complex/environment.yaml" -formation_group = "./config/simulations/Complex/formation.ron" - -[visualisation.height] -objects = 0.5 -height-map = 2.0 - -[visualisation.draw] -robots = false -communication-graph = true -predicted-trajectories = true -waypoints = true -uncertainty = false -paths = true -communication-radius = false -obstacle-factors = false -tracking = true -interrobot-factors = false -interrobot-factors-safety-distance = false -generated-map = true -sdf = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = true -robot-environment-collisions = true - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 300.0 - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 300.0 - -[gbp] -sigma-pose-fixed = 0.0000000000000010000000036274937 -sigma-factor-dynamics = 0.5 -sigma-factor-interrobot = 0.009999999776482582 -sigma-factor-obstacle = 0.009999999776482582 -sigma-factor-tracking = 0.30000001192092896 -lookahead-multiple = 3 -variables = 10 - -[gbp.tracking] -switch-padding = 5.0 -attraction-distance = 1.0 - -[gbp.iteration-schedule] -internal = 10 -external = 10 -schedule = "interleave-evenly" - -[gbp.factors-enabled] -dynamic = true -interrobot = true -obstacle = true -tracking = false - -[robot] -planning-horizon = 5.0 -target-speed = 7.0 -inter-robot-safety-distance-multiplier = 3.200000047683716 - -[robot.radius] -min = 2.0 -max = 2.0 - -[robot.communication] -radius = 20.0 -failure-rate = 0.0 - -[simulation] -max-time = 10000.0 -time-scale = 1.0 -manual-step-factor = 1 -hz = 10.0 -prng-seed = 2 -pause-on-spawn = true -despawn-robot-when-final-waypoint-reached = true -exit-application-on-scenario-finished = false - -[rrt] -max-iterations = 5000000 -step-size = 5.0 -collision-radius = 3.0 -neighbourhood-radius = 8.0 - -[rrt.smoothing] -enabled = true -max-iterations = 500 -step-size = 0.5 - -[graphviz] -export-location = "./assets/export/" - -[graphviz.interrobot.active] -style = "dashed" -len = 8.0 -color = "red" - -[graphviz.interrobot.inactive] -style = "dashed" -len = 8.0 -color = "gray" - -[manual] -timesteps-per-step = 1 - -[debug.on-variable-clicked] -obstacle = true -dynamic = false -interrobot = false -tracking = true -variable = true -inbox = true diff --git a/config/simulations/Complex/environment.yaml b/config/simulations/Complex/environment.yaml deleted file mode 100644 index 00b0a3c3..00000000 --- a/config/simulations/Complex/environment.yaml +++ /dev/null @@ -1,18 +0,0 @@ -tiles: - grid: - - ' ╷ ' - - ' ┌─┼─┬─┐┌╴' - - '╶┼─┘┌┼┬┼┘ ' - - '╶┴┬─┴┼┘│ ' - - ' ┌┴┐┌┼─┴┬╴' - - ' ├─┴┘└──┘ ' - - ' ╵ ' - settings: - tile-size: 25.0 - path-width: 0.5 - obstacle-height: 2.25 - sdf: - resolution: 200 - expansion: 0.2 - blur: 0.02 -obstacles: [] diff --git a/config/simulations/Complex/formation.yaml b/config/simulations/Complex/formation.yaml deleted file mode 100644 index c68889c8..00000000 --- a/config/simulations/Complex/formation.yaml +++ /dev/null @@ -1,118 +0,0 @@ -formations: - -- repeat: - every: - secs: 5 - nanos: 0 - times: !finite 1 - delay: - secs: 2 - nanos: 0 - robots: 1 - planning-strategy: rrt-star - initial-position: - shape: !line-segment - - x: 0.15 - y: 0.20 - - x: 0.15 - y: 0.2 - placement-strategy: !random - attempts: 1000 - waypoints: - - shape: !line-segment - - x: 0.85 - y: 0.80 - - x: 0.85 - y: 0.8 - projection-strategy: identity - # waypoint-reached-when-intersects: horizon - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: robot-radius - intersects-with: current - - -# - repeat: -# every: -# secs: 5 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 2 -# nanos: 0 -# robots: 1 -# planning-strategy: rrt-star -# initial-position: -# shape: !line-segment -# - x: 0.85 -# y: 0.80 -# - x: 0.85 -# y: 0.8 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.15 -# y: 0.20 -# - x: 0.15 -# y: 0.2 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon -# -# -# - repeat: -# every: -# secs: 5 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 0 -# robots: 1 -# planning-strategy: rrt-star -# initial-position: -# shape: !line-segment -# - x: 0.15 -# y: 0.80 -# - x: 0.15 -# y: 0.8 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.85 -# y: 0.2 -# - x: 0.85 -# y: 0.2 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon -# -# -# - repeat: -# every: -# secs: 5 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 0 -# robots: 1 -# planning-strategy: rrt-star -# initial-position: -# shape: !line-segment -# - x: 0.85 -# y: 0.2 -# - x: 0.85 -# y: 0.2 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.15 -# y: 0.80 -# - x: 0.15 -# y: 0.8 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon diff --git a/config/simulations/Intersection/config.toml b/config/simulations/Intersection/config.toml deleted file mode 100644 index 374a067c..00000000 --- a/config/simulations/Intersection/config.toml +++ /dev/null @@ -1,111 +0,0 @@ -# environment = "junction" -environment_image = "junction_twoway" -environment = "./config/simulations/Intersection/environment.yaml" -formation_group = "./config/simulations/Intersection/formation.ron" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 100.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 300.0 - -[visualisation.height] -objects = 0.5 -height-map = 1.0 - -[visualisation.draw] -robots = false -communication-graph = false -predicted-trajectories = true -waypoints = false -uncertainty = true -paths = true -generated-map = false -height-map = false -sdf = false -communication-radius = false -obstacle-factors = false -tracking = true -interrobot-factors = false -interrobot-factors-safety-distance = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = false -robot-environment-collisions = false - -[gbp.factors-enabled] -tracking = true -obstacle = false -dynamic = true -interrobot = false - -[gbp] -sigma-pose-fixed = 0.000000000000001 -sigma-factor-dynamics = 0.1 -sigma-factor-interrobot = 0.01 -sigma-factor-obstacle = 0.01 -sigma-factor-tracking = 0.2 -lookahead-multiple = 3 - -[gbp.tracking] -attraction-distance = 1.0 -switch-padding = 5.0 - -[gbp.iteration-schedule] -internal = 1 -external = 1 -schedule = "interleave-evenly" - -[robot] -planning-horizon = 5.0 -target-speed = 4.0 -inter-robot-safety-distance-multiplier = 2.2 - -[robot.radius] -min = 2.0 -max = 3.0 - -[robot.communication] -radius = 20.0 -failure-rate = 0.2 - -[simulation] -# t0 = 0.25 -max-time = 10000.0 -time-scale = 1.0 -manual-step-factor = 1 -hz = 60.0 -# world-size = 100.0 -prng-seed = 0 -pause-on-spawn = true -despawn-robot-when-final-waypoint-reached = false - -[rrt] -max-iterations = 1000000 -step-size = 0.5 -collision-radius = 0.1 -neighbourhood-radius = 10.0 - -[rrt.smoothing] -enabled = true -max-iterations = 500 -step-size = 0.5 - -[graphviz] -export-location = "./assets/export/" - -[graphviz.interrobot.active] -style = "dashed" -len = 8.0 -color = "red" - -[graphviz.interrobot.inactive] -style = "dashed" -len = 8.0 -color = "gray" - - -[manual] -timesteps-per-step = 1 diff --git a/config/simulations/Junction Experiment/config.toml b/config/simulations/Junction Experiment/config.toml index 3ffe8893..6cbcda33 100644 --- a/config/simulations/Junction Experiment/config.toml +++ b/config/simulations/Junction Experiment/config.toml @@ -1,101 +1,116 @@ -# environment = "junction" environment_image = "junction_twoway" -environment = "./config/simulations/Intersection/environment.yaml" -formation_group = "./config/simulations/Intersection/formation.ron" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 150.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 30.0 +environment = "./config/simulations/Intersection/environment.yaml" +formation_group = "./config/simulations/Intersection/formation.ron" [visualisation.height] -objects = 0.5 +objects = 0.5 height-map = 1.0 [visualisation.draw] -robots = true -communication-graph = false -predicted-trajectories = false -waypoints = true -uncertainty = false -paths = false -generated-map = true -height-map = false -sdf = false -communication-radius = false -obstacle-factors = false -tracking = false -interrobot-factors = false +robots = true +communication-graph = false +predicted-trajectories = false +waypoints = true +uncertainty = false +paths = false +communication-radius = false +obstacle-factors = false +tracking = false +interrobot-factors = false interrobot-factors-safety-distance = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = false -robot-environment-collisions = false +generated-map = true +sdf = false +robot-colliders = false +environment-colliders = false +robot-robot-collisions = false +robot-environment-collisions = false + +[visualisation.uncertainty] +max-radius = 2.5 +scale = 30.0 +[interaction] +ui-focus-cancels-inputs = true +default-cam-distance = 150.0 [gbp] -sigma-pose-fixed = 1e-15 -sigma-factor-dynamics = 0.5 -sigma-factor-interrobot = 0.005 -sigma-factor-obstacle = 0.005 -sigma-factor-tracking = 0.01 -lookahead-multiple = 3 +sigma-pose-fixed = 0.0000000000000010000000036274937 +sigma-factor-dynamics = 0.5 +sigma-factor-interrobot = 0.004999999888241291 +sigma-factor-obstacle = 0.004999999888241291 +sigma-factor-tracking = 0.009999999776482582 +lookahead-multiple = 3 +variables = 10 + +[gbp.tracking] +switch-padding = 1.0 +attraction-distance = 2.0 [gbp.iteration-schedule] -internal = 50 +internal = 10 external = 10 schedule = "interleave-evenly" +[gbp.factors-enabled] +dynamic = true +interrobot = true +obstacle = true +tracking = false + [robot] -planning-horizon = 2.0 -target-speed = 15.0 -inter-robot-safety-distance-multiplier = 2.2 +planning-horizon = 2.0 +target-speed = 15.0 +inter-robot-safety-distance-multiplier = 2.200000047683716 [robot.radius] min = 2.5 max = 2.5 [robot.communication] -radius = 20.0 +radius = 20.0 failure-rate = 0.0 [simulation] -max-time = 61.0 -time-scale = 1.0 +max-time = 10000.0 +time-scale = 1.0 manual-step-factor = 1 -hz = 10.0 -# world-size = 100.0 -prng-seed = 805 -pause-on-spawn = false +hz = 10.0 +prng-seed = 805 +pause-on-spawn = false despawn-robot-when-final-waypoint-reached = true +exit-application-on-scenario-finished = false [rrt] -max-iterations = 1000000 -step-size = 0.5 -collision-radius = 0.1 +max-iterations = 1000000 +step-size = 0.5 +collision-radius = 0.10000000149011612 neighbourhood-radius = 10.0 [rrt.smoothing] -enabled = true +enabled = true max-iterations = 500 -step-size = 0.5 +step-size = 0.5 [graphviz] export-location = "./assets/export/" [graphviz.interrobot.active] style = "dashed" -len = 8.0 +len = 8.0 color = "red" [graphviz.interrobot.inactive] style = "dashed" -len = 8.0 +len = 8.0 color = "gray" - [manual] timesteps-per-step = 1 + +[debug.on-variable-clicked] +obstacle = false +dynamic = false +interrobot = false +tracking = false +variable = false +inbox = false diff --git a/config/simulations/Junction Experiment/formation.yaml b/config/simulations/Junction Experiment/formation.yaml index 156e4882..ea47a83b 100644 --- a/config/simulations/Junction Experiment/formation.yaml +++ b/config/simulations/Junction Experiment/formation.yaml @@ -28,11 +28,11 @@ formations: # finished-when-intersects: horizon waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon + distance: !meter 15 + intersects-with: current finished-when-intersects: - distance: robot-radius - intersects-with: horizon + distance: !meter 15 + intersects-with: current - repeat: every: @@ -63,116 +63,8 @@ formations: # finished-when-intersects: !variable 4 # finished-when-intersects: horizon waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon + distance: !meter 15 + intersects-with: current finished-when-intersects: - distance: robot-radius - intersects-with: horizon - -# - repeat: -# every: -# secs: 1 -# nanos: 0 -# times: !finite 25 -# delay: -# secs: 0 -# nanos: 500000000 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: -0.05 -# y: 0.45 -# - x: -0.05 -# y: 0.45 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 1.3 -# y: 0.45 -# - x: 1.3 -# y: 0.45 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon -# -# - repeat: -# every: -# secs: 1 -# nanos: 0 -# times: !finite 20 -# delay: -# secs: 0 -# nanos: 0 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: -0.05 -# y: 0.55 -# - x: -0.05 -# y: 0.55 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 1.3 -# y: 0.55 -# - x: 1.3 -# y: 0.55 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon -# -# - repeat: -# every: -# secs: 1 -# nanos: 500000000 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 500000000 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: 0.45 -# y: 1.05 -# - x: 0.45 -# y: 1.05 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.45 -# y: -0.3 -# - x: 0.45 -# y: -0.3 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon -# -# - repeat: -# every: -# secs: 2 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 0 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: 0.55 -# y: 1.05 -# - x: 0.55 -# y: 1.05 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.55 -# y: -0.3 -# - x: 0.55 -# y: -0.3 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon + distance: !meter 15 + intersects-with: current diff --git a/config/simulations/Malthe/config.toml b/config/simulations/Malthe/config.toml deleted file mode 100644 index 2a689cc7..00000000 --- a/config/simulations/Malthe/config.toml +++ /dev/null @@ -1,95 +0,0 @@ -# environment = "junction" -environment_image = "junction_twoway" -environment = "./config/simulations/Intersection/environment.yaml" -formation_group = "./config/simulations/Intersection/formation.ron" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 300.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 300.0 - -[visualisation.height] -objects = 0.5 -height-map = 1.0 - -[visualisation.draw] -robots = false -communication-graph = false -predicted-trajectories = true -waypoints = false -uncertainty = true -paths = true -generated-map = true -sdf = true -communication-radius = false -obstacle-factors = false -tracking = true -interrobot-factors = false -interrobot-factors-safety-distance = false -robot-colliders = false -environment-colliders = true -robot-robot-collisions = false -robot-environment-collisions = false - -[gbp] -sigma-pose-fixed = 0.000000000000001 -sigma-factor-dynamics = 0.1 -sigma-factor-interrobot = 0.01 -sigma-factor-obstacle = 0.01 -sigma-factor-tracking = 0.01 -lookahead-multiple = 3 - -[gbp.iteration-schedule] -internal = 10 -external = 10 -schedule = "interleave-evenly" - -[robot] -planning-horizon = 5.0 -target-speed = 4.0 -inter-robot-safety-distance-multiplier = 2.2 - -[robot.radius] -min = 2.0 -max = 3.0 - -[robot.communication] -radius = 20.0 -failure-rate = 0.2 - -[simulation] -# t0 = 0.25 -max-time = 10000.0 -time-scale = 1.0 -manual-step-factor = 1 -hz = 100.0 -# world-size = 100.0 -prng-seed = 0 -pause-on-spawn = true -despawn-robot-when-final-waypoint-reached = false - -[rrt] -max-iterations = 1000000 -step-size = 0.5 -collision-radius = 0.1 -neighbourhood-radius = 10.0 - -[graphviz] -export-location = "./assets/export/" - -[graphviz.interrobot.active] -style = "dashed" -len = 8.0 -color = "red" - -[graphviz.interrobot.inactive] -style = "dashed" -len = 8.0 -color = "gray" - - -[manual] -timesteps-per-step = 1 diff --git a/config/simulations/Malthe/environment.yaml b/config/simulations/Malthe/environment.yaml deleted file mode 100644 index e2112e71..00000000 --- a/config/simulations/Malthe/environment.yaml +++ /dev/null @@ -1,31 +0,0 @@ -tiles: - grid: - - '┌┐ ┌──┐' - - '│├─┤┌─┘' - - '││ │└─┐' - - '│└─┴─ │' - - '└─────┘' - # - # - ' ' - # - ' ───┼── ' - # - ' ───┼── ' - # - ' ──┤ ' - # - ' ' - - # - ' ' - # - ' ┌─┼─┬─┐┌ ' - # - ' ┼─┘┌┼┬┼┘ ' - # - ' ┴┬─┴┼┘│ ' - # - ' ┌┴┐┌┼─┴┬ ' - # - ' ├─┴┘└──┘ ' - # - ' ' - - settings: - tile-size: 25.0 - path-width: 0.9 - obstacle-height: 1.0 - sdf: - resolution: 200 - expansion: 0.25 - blur: 0.01 -obstacles: [] diff --git a/config/simulations/Malthe/formation.yaml b/config/simulations/Malthe/formation.yaml deleted file mode 100644 index a169f021..00000000 --- a/config/simulations/Malthe/formation.yaml +++ /dev/null @@ -1,31 +0,0 @@ -formations: -- repeat: - every: - secs: 10 - nanos: 0 - times: !finite 1 - delay: - secs: 1 - nanos: 0 - robots: 3 - planning-strategy: only-local - initial-position: - shape: !circle - radius: 25.0 - center: - x: 0.5 - y: 0.5 - placement-strategy: equal - waypoints: - - shape: !circle - radius: 25.0 - center: - x: 0.5 - y: 0.5 - projection-strategy: cross - waypoint-reached-when-intersects: - distance: robot-radius - intersects-with: horizon - finished-when-intersects: - distance: robot-radius - intersects-with: current diff --git a/config/simulations/test/config.toml b/config/simulations/Obstacle Shapes Showcase/config.toml similarity index 100% rename from config/simulations/test/config.toml rename to config/simulations/Obstacle Shapes Showcase/config.toml diff --git a/config/simulations/test/environment.yaml b/config/simulations/Obstacle Shapes Showcase/environment.yaml similarity index 100% rename from config/simulations/test/environment.yaml rename to config/simulations/Obstacle Shapes Showcase/environment.yaml diff --git a/config/simulations/test/formation.yaml b/config/simulations/Obstacle Shapes Showcase/formation.yaml similarity index 98% rename from config/simulations/test/formation.yaml rename to config/simulations/Obstacle Shapes Showcase/formation.yaml index 3e3e536e..6d1874c7 100644 --- a/config/simulations/test/formation.yaml +++ b/config/simulations/Obstacle Shapes Showcase/formation.yaml @@ -7,7 +7,7 @@ formations: delay: secs: 1 nanos: 0 - robots: 2 + robots: 0 planning-strategy: only-local initial-position: shape: !circle diff --git a/config/simulations/Solo GP/config.toml b/config/simulations/Solo GP/config.toml index 8dbdf742..efc78873 100644 --- a/config/simulations/Solo GP/config.toml +++ b/config/simulations/Solo GP/config.toml @@ -1,51 +1,49 @@ environment_image = "junction_twoway" -formation_group = "./config/simulations/Solo GP/formation.ron" -environment = "./config/simulations/Solo GP/environment.yaml" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 300.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 300.0 - +environment = "./config/simulations/Solo GP/environment.yaml" +formation_group = "./config/simulations/Solo GP/formation.ron" [visualisation.height] -objects = 0.5 +objects = 0.5 height-map = 2.0 [visualisation.draw] -robots = true -communication-graph = true -predicted-trajectories = true -waypoints = true -uncertainty = false -paths = true -communication-radius = false -obstacle-factors = false -tracking = true -interrobot-factors = false +robots = true +communication-graph = true +predicted-trajectories = true +waypoints = true +uncertainty = false +paths = true +communication-radius = false +obstacle-factors = false +tracking = true +interrobot-factors = false interrobot-factors-safety-distance = false -generated-map = true -sdf = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = true -robot-environment-collisions = true +generated-map = true +sdf = false +robot-colliders = false +environment-colliders = false +robot-robot-collisions = true +robot-environment-collisions = true + +[visualisation.uncertainty] +max-radius = 2.5 +scale = 300.0 +[interaction] +ui-focus-cancels-inputs = true +default-cam-distance = 300.0 [gbp] -sigma-pose-fixed = 1e-15 -sigma-factor-dynamics = 0.2 -sigma-factor-interrobot = 0.01 -sigma-factor-obstacle = 0.01 -sigma-factor-tracking = 0.15 -lookahead-multiple = 3 -variables = 10 +sigma-pose-fixed = 0.0000000000000010000000036274937 +sigma-factor-dynamics = 0.10000000149011612 +sigma-factor-interrobot = 0.009999999776482582 +sigma-factor-obstacle = 0.009999999776482582 +sigma-factor-tracking = 0.15000000596046448 +lookahead-multiple = 3 +variables = 10 [gbp.tracking] -switch-padding = 5.0 +switch-padding = 5.0 attraction-distance = 1.0 [gbp.iteration-schedule] @@ -54,65 +52,65 @@ external = 10 schedule = "interleave-evenly" [gbp.factors-enabled] -dynamic = true +dynamic = true interrobot = true -obstacle = true -tracking = true +obstacle = true +tracking = true [robot] -planning-horizon = 5.0 -target-speed = 7.0 -inter-robot-safety-distance-multiplier = 3.2 +planning-horizon = 5.0 +target-speed = 7.0 +inter-robot-safety-distance-multiplier = 3.200000047683716 [robot.radius] min = 2.0 max = 2.0 [robot.communication] -radius = 20.0 +radius = 20.0 failure-rate = 0.0 [simulation] -max-time = 10000.0 -time-scale = 10.0 -manual-step-factor = 1 -hz = 10.0 -prng-seed = 805 -pause-on-spawn = true +max-time = 10000.0 +time-scale = 1.600000023841858 +manual-step-factor = 1 +hz = 10.0 +prng-seed = 805 +pause-on-spawn = true despawn-robot-when-final-waypoint-reached = true -exit-application-on-scenario-finished = true +exit-application-on-scenario-finished = true [rrt] -max-iterations = 5000000 -step-size = 5.0 -collision-radius = 3.0 +max-iterations = 5000000 +step-size = 5.0 +collision-radius = 3.0 neighbourhood-radius = 8.0 [rrt.smoothing] -enabled = true +enabled = true max-iterations = 500 -step-size = 0.5 +step-size = 0.5 [graphviz] export-location = "./assets/export/" [graphviz.interrobot.active] style = "dashed" -len = 8.0 +len = 8.0 color = "red" [graphviz.interrobot.inactive] style = "dashed" -len = 8.0 +len = 8.0 color = "gray" [manual] timesteps-per-step = 1 [debug.on-variable-clicked] -obstacle = false -dynamic = false +obstacle = false +dynamic = false interrobot = false -tracking = false -variable = false -inbox = true +tracking = false +variable = false +inbox = true diff --git a/config/simulations/Solo GP/formation.yaml b/config/simulations/Solo GP/formation.yaml index 6c5513a4..2ff4c77c 100644 --- a/config/simulations/Solo GP/formation.yaml +++ b/config/simulations/Solo GP/formation.yaml @@ -10,7 +10,7 @@ formations: secs: 0 nanos: 0 robots: 1 - planning-strategy: only-local + planning-strategy: rrt-star initial-position: shape: !line-segment - x: 0.15 diff --git a/config/simulations/Collaborative GP Low Qin/config.toml b/config/simulations/Tracking Factor Showcase/config.toml similarity index 67% rename from config/simulations/Collaborative GP Low Qin/config.toml rename to config/simulations/Tracking Factor Showcase/config.toml index 1db846d4..249f0b37 100644 --- a/config/simulations/Collaborative GP Low Qin/config.toml +++ b/config/simulations/Tracking Factor Showcase/config.toml @@ -1,29 +1,29 @@ environment_image = "junction_twoway" -environment = "./config/simulations/Collaborative GP/environment.yaml" -formation_group = "./config/simulations/Collaborative GP/formation.ron" +environment = "./config/simulations/Intersection/environment.yaml" +formation_group = "./config/simulations/Intersection/formation.ron" [visualisation.height] objects = 0.5 -height-map = 2.0 +height-map = 1.0 [visualisation.draw] robots = true -communication-graph = true +communication-graph = false predicted-trajectories = true waypoints = true -uncertainty = false -paths = false +uncertainty = true +paths = true communication-radius = false obstacle-factors = false tracking = true interrobot-factors = false interrobot-factors-safety-distance = false -generated-map = true +generated-map = false sdf = false robot-colliders = false environment-colliders = false -robot-robot-collisions = true -robot-environment-collisions = true +robot-robot-collisions = false +robot-environment-collisions = false [visualisation.uncertainty] max-radius = 2.5 @@ -31,7 +31,7 @@ scale = 300.0 [interaction] ui-focus-cancels-inputs = true -default-cam-distance = 300.0 +default-cam-distance = 100.0 [gbp] sigma-pose-fixed = 0.0000000000000010000000036274937 @@ -47,44 +47,44 @@ switch-padding = 5.0 attraction-distance = 1.0 [gbp.iteration-schedule] -internal = 10 -external = 10 +internal = 1 +external = 1 schedule = "interleave-evenly" [gbp.factors-enabled] dynamic = true -interrobot = true -obstacle = true +interrobot = false +obstacle = false tracking = true [robot] planning-horizon = 5.0 -target-speed = 7.0 -inter-robot-safety-distance-multiplier = 4.0 +target-speed = 4.0 +inter-robot-safety-distance-multiplier = 2.200000047683716 [robot.radius] min = 2.0 -max = 2.0 +max = 3.0 [robot.communication] radius = 20.0 -failure-rate = 0.0 +failure-rate = 0.20000000298023224 [simulation] max-time = 10000.0 -time-scale = 1.0 +time-scale = 2.0 manual-step-factor = 1 -hz = 10.0 -prng-seed = 2 +hz = 60.0 +prng-seed = 0 pause-on-spawn = false -despawn-robot-when-final-waypoint-reached = true +despawn-robot-when-final-waypoint-reached = false exit-application-on-scenario-finished = false [rrt] -max-iterations = 5000000 -step-size = 5.0 -collision-radius = 3.0 -neighbourhood-radius = 8.0 +max-iterations = 1000000 +step-size = 0.5 +collision-radius = 0.10000000149011612 +neighbourhood-radius = 10.0 [rrt.smoothing] enabled = true @@ -105,12 +105,12 @@ len = 8.0 color = "gray" [manual] -timesteps-per-step = 20 +timesteps-per-step = 1 [debug.on-variable-clicked] obstacle = false dynamic = false interrobot = false -tracking = true +tracking = false variable = false inbox = false diff --git a/config/simulations/Intersection/environment.yaml b/config/simulations/Tracking Factor Showcase/environment.yaml similarity index 100% rename from config/simulations/Intersection/environment.yaml rename to config/simulations/Tracking Factor Showcase/environment.yaml diff --git a/config/simulations/Intersection/formation.yaml b/config/simulations/Tracking Factor Showcase/formation.yaml similarity index 100% rename from config/simulations/Intersection/formation.yaml rename to config/simulations/Tracking Factor Showcase/formation.yaml diff --git a/crates/gbp_config/src/formation.rs b/crates/gbp_config/src/formation.rs index 7021f461..3391b807 100644 --- a/crates/gbp_config/src/formation.rs +++ b/crates/gbp_config/src/formation.rs @@ -218,7 +218,8 @@ pub struct Formation { /// should spawn. pub delay: Duration, /// Number of robots to spawn every iteration - pub robots: NonZeroUsize, + // pub robots: NonZeroUsize, + pub robots: usize, /// Planning strategy pub planning_strategy: PlanningStrategy, /// Where to spawn the formation @@ -265,15 +266,8 @@ impl Formation { RepeatTimes::Infinite => usize::MAX, RepeatTimes::Finite(times) => times, }); - self.robots.get().saturating_mul(times) - // self.robots.get().saturating_mul(rhs) - // // FIXME(kpbaks): handle of 1 by in repeat count - // dbg!(&self.robots); - // self.robots.get() - // * (self.repeat.map_or(0, |repeat| match repeat.times { - // RepeatTimes::Infinite => unreachable!("ehh ..."), - // RepeatTimes::Finite(times) => times, - // })) + // self.robots.get().saturating_mul(times) + self.robots.saturating_mul(times) } /// Return a new `Formation` matching the used in the **gbpplanner** paper @@ -349,7 +343,8 @@ impl Formation { } }?; - assert_eq!(lerp_amounts.len(), self.robots.get()); + // assert_eq!(lerp_amounts.len(), self.robots.get()); + assert_eq!(lerp_amounts.len(), self.robots); let initial_positions: Vec<_> = lerp_amounts .iter() @@ -399,8 +394,8 @@ impl Formation { // dbg!(¢er); let angles: Vec = match self.initial_position.placement_strategy { InitialPlacementStrategy::Equal => { - let angle = 2.0 * PI / self.robots.get() as f32; - let angles = (0..self.robots.get()).map(|i| i as f32 * angle).collect(); + let angle = 2.0 * PI / self.robots as f32; + let angles = (0..self.robots).map(|i| i as f32 * angle).collect(); Some(angles) } InitialPlacementStrategy::Random { attempts } => { @@ -418,7 +413,7 @@ impl Formation { // ) } }?; - assert_eq!(angles.len(), self.robots.get()); + assert_eq!(angles.len(), self.robots); let initial_positions: Vec = angles .iter() diff --git a/crates/magics/src/main.rs b/crates/magics/src/main.rs index deb53955..a526e8f8 100644 --- a/crates/magics/src/main.rs +++ b/crates/magics/src/main.rs @@ -26,11 +26,11 @@ pub mod export; pub(crate) mod escape_codes; pub(crate) mod macros; -#[cfg(feature = "dhat-heap")] -#[global_allocator] -static ALLOC: dhat::Alloc = dhat::Alloc; +// #[cfg(feature = "dhat-heap")] +// #[global_allocator] +// static ALLOC: dhat::Alloc = dhat::Alloc; -#[cfg(not(feature = "dhat-heap"))] +// #[cfg(not(feature = "dhat-heap"))] #[global_allocator] static ALLOC: mimalloc::MiMalloc = mimalloc::MiMalloc; diff --git a/crates/magics/src/planner/robot.rs b/crates/magics/src/planner/robot.rs index e0270d86..ced87877 100644 --- a/crates/magics/src/planner/robot.rs +++ b/crates/magics/src/planner/robot.rs @@ -2100,25 +2100,21 @@ fn reached_waypoint( }; let r_sq = r.0 * r.0; - // if mission.next_waypoint_is_last() { - // let - // mission.advance_to_next_waypoint(&time); - // assert!(matches!(mission.state, MissionState::Completed)); - //} - - let reached = if mission.next_waypoint_is_last() { - let next_waypoint = mission.next_waypoint().unwrap(); - let distance = match mission.finished_when_intersects.distance { - IntersectionDistance::RobotRadius => r.0, - IntersectionDistance::Meter(meter) => meter, - }; - let reached = next_waypoint - .position() - .distance(transform.translation.xz()) - < distance; - reached - } else { + let reached = { + // if mission.next_waypoint_is_last() { + // let next_waypoint = mission.next_waypoint().unwrap(); + // let distance = match mission.finished_when_intersects.distance { + // IntersectionDistance::RobotRadius => r.0, + // IntersectionDistance::Meter(meter) => meter, + // }; + + // let reached = next_waypoint + // .position() + // .distance(transform.translation.xz()) + // < distance; + // reached + // } else { use CheckIntersectionWith::{Current, Horizon, Variable}; let when_intersects = if mission.next_waypoint_is_last() { mission.finished_when_intersects diff --git a/crates/magics/src/planner/spawner.rs b/crates/magics/src/planner/spawner.rs index 4cea782a..621cc0c9 100644 --- a/crates/magics/src/planner/spawner.rs +++ b/crates/magics/src/planner/spawner.rs @@ -445,7 +445,7 @@ fn spawn_formation( let max_placement_attempts = NonZeroUsize::new(1000).expect("1000 is not zero"); - let radii = (0..formation.robots.get()) + let radii = (0..formation.robots) .map(|_| prng.gen_range(config.robot.radius.range())) .collect::>(); @@ -498,15 +498,26 @@ fn spawn_formation( }) .collect(); - let min_radius = radii - .iter() - .copied() - .map(ordered_float::OrderedFloat) - .min() - .expect("not empty"); + #[rustfmt::skip] + let Some(min_radius) = radii.iter().copied().map(ordered_float::OrderedFloat).min() else { + return; + }; + // let min_radius = radii + // .iter() + // .copied() + // .map(ordered_float::OrderedFloat) + // .min() + // .expect("not empty"); + + // #[rustfmt::skip] + // let max_radius = + // radii.iter().copied().map(ordered_float::OrderedFloat).max().expect("not + // empty"); #[rustfmt::skip] - let max_radius = radii.iter().copied().map(ordered_float::OrderedFloat).max().expect("not empty"); + let Some(max_radius) = radii.iter().copied().map(ordered_float::OrderedFloat).max() else { + return; + }; for (i, initial_pose) in initial_pose_for_each_robot.iter().enumerate() { let mut waypoints: Vec = waypoint_poses_for_each_robot