From 0ddf909482705e65bebb2d59379c78eb9d5d58ff Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Tue, 21 Jun 2022 13:58:08 -0500 Subject: [PATCH 01/16] Removed trainining scripts --- launch/benchmark.launch | 2 - scripts/benchmark.cpp | 3 +- scripts/train.cpp | 136 ---------------------------------------- 3 files changed, 1 insertion(+), 140 deletions(-) delete mode 100644 scripts/train.cpp diff --git a/launch/benchmark.launch b/launch/benchmark.launch index 28f39d3..2141158 100644 --- a/launch/benchmark.launch +++ b/launch/benchmark.launch @@ -9,7 +9,6 @@ - @@ -31,7 +30,6 @@ - [$(arg planners)] diff --git a/scripts/benchmark.cpp b/scripts/benchmark.cpp index 8f9136d..273885d 100644 --- a/scripts/benchmark.cpp +++ b/scripts/benchmark.cpp @@ -28,7 +28,7 @@ int main(int argc, char **argv) std::string dataset, results, exp_name; std::vector planners; double time, runs; - bool sensed, train; + bool sensed; int start, end; @@ -46,7 +46,6 @@ int main(int argc, char **argv) error += !parser::get(exec_name, node, "end", end); error += !parser::get(exec_name, node, "runs", runs); error += !parser::get(exec_name, node, "sensed", sensed); - error += !parser::get(exec_name, node, "train", train); parser::shutdownIfError(exec_name, error); diff --git a/scripts/train.cpp b/scripts/train.cpp deleted file mode 100644 index 6494796..0000000 --- a/scripts/train.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* Author: Constantinos Chamzas */ - -// ROS -#include -#include - -// Robowflex dataset -#include -#include - -// Robowflex library -#include -#include -#include -#include - -// Robowflex ompl -#include - -#include -#include - -using namespace robowflex; - -void prerunCallback(PlanningQuery &query) -{ - const auto planner = dynamic_cast(query.planner.get()); - if (planner) - { - // Get the name of the planner. - const auto &planner_name = query.request.planner_id; - if (planner_name == "thunder") - { - const auto &ss = dynamic_cast(planner->getLastSimpleSetup().get()); - if (ss) - { - ss->setFilePath("thunder.db"); - ss->setup(); - } - else - ROS_ERROR("Failed to typecast Thunder simple Setup!"); - } - else if (planner_name == "lightning") - { - const auto &ss = dynamic_cast(planner->getLastSimpleSetup().get()); - if (ss) - { - ss->setFilePath("lightning.db"); - ss->setup(); - } - else - ROS_ERROR("Failed to typecast Thunder simple Setup!"); - } - else - { - ROS_ERROR("Underlying planners is not lightning or thunder"); - } - } -} - -int main(int argc, char **argv) -{ - // Enables multiple instances running with the same name - ros::init(argc, argv, "benchmark"); - ros::NodeHandle node("~"); - - std::string dataset, results, exp_name; - std::vector planners; - double time, runs; - bool sensed; - - int start, end; - - std::string exec_name = "benchmark"; - - // Load ROS params - size_t error = 0; - // Parsing the parametes from the param server - error += !parser::get(exec_name, node, "dataset", dataset); - error += !parser::get(exec_name, node, "results", results); - error += !parser::get(exec_name, node, "exp_name", exp_name); - error += !parser::get(exec_name, node, "time", time); - error += !parser::get(exec_name, node, "planners", planners); - error += !parser::get(exec_name, node, "start", start); - error += !parser::get(exec_name, node, "end", end); - error += !parser::get(exec_name, node, "runs", runs); - error += !parser::get(exec_name, node, "sensed", sensed); - - parser::shutdownIfError(exec_name, error); - - auto setup = std::make_shared(dataset); - auto robot = setup->getRobot(); - - // Benchmarker - Profiler::Options options; - options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::SMOOTHNESS; - - Experiment experiment(exp_name, options, time, runs); - auto settings = OMPL::Settings(); - settings.simplify_solutions = false; - settings.interpolate_solutions = false; - - auto planner = setup->createPlanner("planner_name", settings); - - int numSamples = (end > 1) ? end : setup->getNumSamples(); - for (int i = start; i <= numSamples; i++) - { - const auto &scene = std::make_shared(robot); - if (sensed) - setup->loadSensedScene(i, scene); - else - setup->loadGeometricScene(i, scene); - - for (const auto &planner_name : planners) - { - const auto &request = std::make_shared(planner, setup->getGroup()); - setup->loadRequest(i, request); - request->setAllowedPlanningTime(time); - request->setNumPlanningAttempts(1); - - // Set the name of the planner - if (!request->setConfig(planner_name)) - ROS_ERROR("Did not find planner %s", planner_name.c_str()); - - experiment.setPreRunCallback(prerunCallback) - experiment.addQuery(planner_name, scene, planner, request); - } - } - - auto results_data = experiment.benchmark(1); - - OMPLPlanDataSetOutputter output(IO::resolveParent(results)); - output.dump(*results_data); - - return 0; -} From 4a15937ff98db2147e072a4f06b4f63285effd7d Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Tue, 21 Jun 2022 14:58:13 -0500 Subject: [PATCH 02/16] WIP --- docker/Dockerfile | 24 ++++++++++++++---------- docker/build-docker.sh | 2 +- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index b6b7707..0961449 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -17,16 +17,20 @@ RUN apt-get update && \ ENV NVIDIA_VISIBLE_DEVICES all ENV NVIDIA_DRIVER_CAPABILITIES all -# Build dependencies +# Create catkin workspace WORKDIR $ROS_UNDERLAY/.. +RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release + +# Main MotionBenchMakerLibrary +RUN mkdir -p ./src/motion_bench_maker +ADD CMakeLists.txt ./src/ # Clone MotionBenchMaker and dependencies -RUN git clone --depth 1 https://github.com/KavrakiLab/motion_bench_maker ./src/motion_bench_maker && \ - git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \ - rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \ - git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \ - git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \ - rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ - catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \ - catkin build --limit-status-rate 0.001 --no-notify && \ - sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh +#RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \ +# rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \ +# git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \ +# git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \ +# git clone --depth 1 https://github.com/TAMS-Group/bio_ik ./src/bio_ik && \ +# rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ +# catkin build --limit-status-rate 0.001 --no-notify && \ +# sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh diff --git a/docker/build-docker.sh b/docker/build-docker.sh index 0c0f706..3c9fa92 100755 --- a/docker/build-docker.sh +++ b/docker/build-docker.sh @@ -1,4 +1,4 @@ #!/bin/bash cd "${0%/*}" -docker build -t motion_bench_maker . +docker build -f Dockerfile -t motion_bench_maker ../ From e564efd239f17b73b20e8405f8e07dca04405533 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Tue, 21 Jun 2022 20:15:24 +0000 Subject: [PATCH 03/16] Made Dockerfile to use current folder --- docker/Dockerfile | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 0961449..a7f951b 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -21,16 +21,19 @@ ENV NVIDIA_DRIVER_CAPABILITIES all WORKDIR $ROS_UNDERLAY/.. RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release -# Main MotionBenchMakerLibrary +# Copy the current motion benchmaker files to the workspace RUN mkdir -p ./src/motion_bench_maker -ADD CMakeLists.txt ./src/ +ADD ./ ./src/motion_bench_maker/ + +RUN ls ./src/motion_bench_maker +RUN ls ./src/ # Clone MotionBenchMaker and dependencies -#RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \ -# rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \ -# git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \ -# git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \ -# git clone --depth 1 https://github.com/TAMS-Group/bio_ik ./src/bio_ik && \ -# rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ -# catkin build --limit-status-rate 0.001 --no-notify && \ -# sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh +RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \ + rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \ + git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \ + git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \ + git clone --depth 1 https://github.com/TAMS-Group/bio_ik ./src/bio_ik && \ + rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ + catkin build --limit-status-rate 0.001 --no-notify && \ + sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh From 3c1df09f890ee244c76c954c1b5cee2b8c6ca249 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Tue, 21 Jun 2022 21:17:35 +0000 Subject: [PATCH 04/16] Updated ompl config files --- configs/ompl/ompl_planning_fetch.yaml | 482 ++-------------------- configs/ompl/ompl_planning_fetch_old.yaml | 446 ++++++++++++++++++++ configs/ompl/ompl_planning_ur5.yaml | 6 - 3 files changed, 486 insertions(+), 448 deletions(-) create mode 100644 configs/ompl/ompl_planning_fetch_old.yaml diff --git a/configs/ompl/ompl_planning_fetch.yaml b/configs/ompl/ompl_planning_fetch.yaml index fd5c344..b8b5f21 100644 --- a/configs/ompl/ompl_planning_fetch.yaml +++ b/configs/ompl/ompl_planning_fetch.yaml @@ -1,446 +1,44 @@ planner_configs: - BKPIECE_BOOKSHELF_SMALL: - type: geometric::BKPIECE - range: 0.4209 - BKPIECE_BOOKSHELF_TALL: - type: geometric::BKPIECE - range: 0.4727 - BKPIECE_BOOKSHELF_THIN: - type: geometric::BKPIECE - range: 0.4635 - BKPIECE_BOX: - type: geometric::BKPIECE - range: 0.9723 - BKPIECE_CAGE: - type: geometric::BKPIECE - range: 0.3644 - BKPIECE_KITCHEN: - type: geometric::BKPIECE - range: 0.5914 - BKPIECE_TABLE_PICK: - type: geometric::BKPIECE - range: 0.9257 - BKPIECE_DEFAULT: - type: geometric::BKPIECE - RRTConnect_BOOKSHELF_SMALL: - type: geometric::RRTConnect - range: 0.2142 - RRTConnect_BOOKSHELF_TALL: - type: geometric::RRTConnect - range: 0.7611 - RRTConnect_BOOKSHELF_THIN: - type: geometric::RRTConnect - range: 0.5080 - RRTConnect_BOX: - type: geometric::RRTConnect - range: 4.2123 - RRTConnect_CAGE: - type: geometric::RRTConnect - range: 0.2167 - RRTConnect_KITCHEN: - type: geometric::RRTConnect - range: 4.2357 - RRTConnect_TABLE_PICK: - type: geometric::RRTConnect - range: 2.7199 - RRTConnect_DEFAULT: - type: geometric::RRTConnect - BiEST_BOOKSHELF_SMALL: - type: geometric::BiEST - range: 1.149 - BiEST_BOOKSHELF_TALL: - type: geometric::BiEST - range: 1.499 - BiEST_BOOKSHELF_THIN: - type: geometric::BiEST - range: 1.205 - BiEST_BOX: - type: geometric::BiEST - range: 14.157 - BiEST_CAGE: - type: geometric::BiEST - range: 1.4018 - BiEST_KITCHEN: - type: geometric::BiEST - range: 14.248 - BiEST_TABLE_PICK: - type: geometric::BiEST - range: 14.116 - BiEST_DEFAULT: - type: geometric::BiEST - BiEST_0_05: - type: geometric::BiEST - range: 0.05 - BiEST_0_25: - type: geometric::BiEST - range: 0.25 - BiEST_0_50: - type: geometric::BiEST - range: 0.50 - BiEST_0_75: - type: geometric::BiEST - range: 0.75 - BiEST_1_00: - type: geometric::BiEST - range: 1.00 - BiEST_1_25: - type: geometric::BiEST - range: 1.25 - BiEST_1_50: - type: geometric::BiEST - range: 1.50 - BiEST_1_75: - type: geometric::BiEST - range: 1.75 - BiEST_2_00: - type: geometric::BiEST - range: 2.00 - BiEST_2_25: - type: geometric::BiEST - range: 2.25 - BiEST_2_50: - type: geometric::BiEST - range: 2.50 - BiEST_2_75: - type: geometric::BiEST - range: 2.75 - BiEST_3_00: - type: geometric::BiEST - range: 3.00 - BiEST_3_25: - type: geometric::BiEST - range: 3.25 - BiEST_3_50: - type: geometric::BiEST - range: 3.50 - BiEST_3_75: - type: geometric::BiEST - range: 3.75 - BiEST_4_00: - type: geometric::BiEST - range: 4.00 - BiEST_4_25: - type: geometric::BiEST - range: 4.25 - BiEST_4_50: - type: geometric::BiEST - range: 4.50 - BiEST_4_75: - type: geometric::BiEST - range: 4.75 - BiEST_5_00: - type: geometric::BiEST - range: 5.00 - BiEST_5_25: - type: geometric::BiEST - range: 5.25 - BiEST_5_50: - type: geometric::BiEST - range: 5.50 - BiEST_5_75: - type: geometric::BiEST - range: 5.75 - BiEST_6_00: - type: geometric::BiEST - range: 6.00 - BiEST_6_25: - type: geometric::BiEST - range: 6.25 - BiEST_6_50: - type: geometric::BiEST - range: 6.50 - BiEST_6_75: - type: geometric::BiEST - range: 6.75 - BiEST_7_00: - type: geometric::BiEST - range: 7.00 - BKPIECE_0_05: - type: geometric::BKPIECE - range: 0.05 - BKPIECE_0_25: - type: geometric::BKPIECE - range: 0.25 - BKPIECE_0_50: - type: geometric::BKPIECE - range: 0.50 - BKPIECE_0_75: - type: geometric::BKPIECE - range: 0.75 - BKPIECE_1_00: - type: geometric::BKPIECE - range: 1.00 - BKPIECE_1_25: - type: geometric::BKPIECE - range: 1.25 - BKPIECE_1_50: - type: geometric::BKPIECE - range: 1.50 - BKPIECE_1_75: - type: geometric::BKPIECE - range: 1.75 - BKPIECE_2_00: - type: geometric::BKPIECE - range: 2.00 - BKPIECE_2_25: - type: geometric::BKPIECE - range: 2.25 - BKPIECE_2_50: - type: geometric::BKPIECE - range: 2.50 - BKPIECE_2_75: - type: geometric::BKPIECE - range: 2.75 - BKPIECE_3_00: - type: geometric::BKPIECE - range: 3.00 - BKPIECE_3_25: - type: geometric::BKPIECE - range: 3.25 - BKPIECE_3_50: - type: geometric::BKPIECE - range: 3.50 - BKPIECE_3_75: - type: geometric::BKPIECE - range: 3.75 - BKPIECE_4_00: - type: geometric::BKPIECE - range: 4.00 - BKPIECE_4_25: - type: geometric::BKPIECE - range: 4.25 - BKPIECE_4_50: - type: geometric::BKPIECE - range: 4.50 - BKPIECE_4_75: - type: geometric::BKPIECE - range: 4.75 - BKPIECE_5_00: - type: geometric::BKPIECE - range: 5.00 - BKPIECE_5_25: - type: geometric::BKPIECE - range: 5.25 - BKPIECE_5_50: - type: geometric::BKPIECE - range: 5.50 - BKPIECE_5_75: - type: geometric::BKPIECE - range: 5.75 - BKPIECE_6_00: - type: geometric::BKPIECE - range: 6.00 - BKPIECE_6_25: - type: geometric::BKPIECE - range: 6.25 - BKPIECE_6_50: - type: geometric::BKPIECE - range: 6.50 - BKPIECE_6_75: - type: geometric::BKPIECE - range: 6.75 - BKPIECE_7_00: - type: geometric::BKPIECE - range: 7.00 - RRTConnect_0_05: - type: geometric::RRTConnect - range: 0.05 - RRTConnect_0_25: - type: geometric::RRTConnect - range: 0.25 - RRTConnect_0_50: - type: geometric::RRTConnect - range: 0.50 - RRTConnect_0_75: - type: geometric::RRTConnect - range: 0.75 - RRTConnect_1_00: - type: geometric::RRTConnect - range: 1.00 - RRTConnect_1_25: - type: geometric::RRTConnect - range: 1.25 - RRTConnect_1_50: - type: geometric::RRTConnect - range: 1.50 - RRTConnect_1_75: - type: geometric::RRTConnect - range: 1.75 - RRTConnect_2_00: - type: geometric::RRTConnect - range: 2.00 - RRTConnect_2_25: - type: geometric::RRTConnect - range: 2.25 - RRTConnect_2_50: - type: geometric::RRTConnect - range: 2.50 - RRTConnect_2_75: - type: geometric::RRTConnect - range: 2.75 - RRTConnect_3_00: - type: geometric::RRTConnect - range: 3.00 - RRTConnect_3_25: - type: geometric::RRTConnect - range: 3.25 - RRTConnect_3_50: - type: geometric::RRTConnect - range: 3.50 - RRTConnect_3_75: - type: geometric::RRTConnect - range: 3.75 - RRTConnect_4_00: - type: geometric::RRTConnect - range: 4.00 - RRTConnect_4_25: - type: geometric::RRTConnect - range: 4.25 - RRTConnect_4_50: - type: geometric::RRTConnect - range: 4.50 - RRTConnect_4_75: - type: geometric::RRTConnect - range: 4.75 - RRTConnect_5_00: - type: geometric::RRTConnect - range: 5.00 - RRTConnect_5_25: - type: geometric::RRTConnect - range: 5.25 - RRTConnect_5_50: - type: geometric::RRTConnect - range: 5.50 - RRTConnect_5_75: - type: geometric::RRTConnect - range: 5.75 - RRTConnect_6_00: - type: geometric::RRTConnect - range: 6.00 - RRTConnect_6_25: - type: geometric::RRTConnect - range: 6.25 - RRTConnect_6_50: - type: geometric::RRTConnect - range: 6.50 - RRTConnect_6_75: - type: geometric::RRTConnect - range: 6.75 - RRTConnect_7_00: - type: geometric::RRTConnect - range: 7.00 + SBLkConfigDefault: + type: geometric::SBL + ESTkConfigDefault: + type: geometric::EST + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + BKPIECEkConfigDefault: + type: geometric::BKPIECE + KPIECEkConfigDefault: + type: geometric::KPIECE + RRTkConfigDefault: + type: geometric::RRT + RRTConnectkConfigDefault: + type: geometric::RRTConnect + RRTstarkConfigDefault: + type: geometric::RRTstar + TRRTkConfigDefault: + type: geometric::TRRT + PRMkConfigDefault: + type: geometric::PRM + PRMstarkConfigDefault: + type: geometric::PRMstar + BITstarkConfigDefault: + type: geometric::BITstar + AITstarkConfigDefault: + type: geometric::AITstar arm_with_torso: planner_configs: - - BKPIECE_BOOKSHELF_SMALL - - BKPIECE_BOOKSHELF_TALL - - BKPIECE_BOOKSHELF_THIN - - BKPIECE_BOX - - BKPIECE_CAGE - - BKPIECE_KITCHEN - - BKPIECE_TABLE_PICK - - BKPIECE_DEFAULT - - RRTConnect_BOOKSHELF_SMALL - - RRTConnect_BOOKSHELF_TALL - - RRTConnect_BOOKSHELF_THIN - - RRTConnect_BOX - - RRTConnect_CAGE - - RRTConnect_KITCHEN - - RRTConnect_TABLE_PICK - - RRTConnect_DEFAULT - - BiEST_BOOKSHELF_SMALL - - BiEST_BOOKSHELF_TALL - - BiEST_BOOKSHELF_THIN - - BiEST_BOX - - BiEST_CAGE - - BiEST_KITCHEN - - BiEST_TABLE_PICK - - BiEST_DEFAULT - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(torso_lift_joint,shoulder_pan_joint) - longest_valid_segment_fraction: 0.005 + longest_valid_segment_fraction: 0.005 diff --git a/configs/ompl/ompl_planning_fetch_old.yaml b/configs/ompl/ompl_planning_fetch_old.yaml new file mode 100644 index 0000000..fd5c344 --- /dev/null +++ b/configs/ompl/ompl_planning_fetch_old.yaml @@ -0,0 +1,446 @@ +planner_configs: + BKPIECE_BOOKSHELF_SMALL: + type: geometric::BKPIECE + range: 0.4209 + BKPIECE_BOOKSHELF_TALL: + type: geometric::BKPIECE + range: 0.4727 + BKPIECE_BOOKSHELF_THIN: + type: geometric::BKPIECE + range: 0.4635 + BKPIECE_BOX: + type: geometric::BKPIECE + range: 0.9723 + BKPIECE_CAGE: + type: geometric::BKPIECE + range: 0.3644 + BKPIECE_KITCHEN: + type: geometric::BKPIECE + range: 0.5914 + BKPIECE_TABLE_PICK: + type: geometric::BKPIECE + range: 0.9257 + BKPIECE_DEFAULT: + type: geometric::BKPIECE + RRTConnect_BOOKSHELF_SMALL: + type: geometric::RRTConnect + range: 0.2142 + RRTConnect_BOOKSHELF_TALL: + type: geometric::RRTConnect + range: 0.7611 + RRTConnect_BOOKSHELF_THIN: + type: geometric::RRTConnect + range: 0.5080 + RRTConnect_BOX: + type: geometric::RRTConnect + range: 4.2123 + RRTConnect_CAGE: + type: geometric::RRTConnect + range: 0.2167 + RRTConnect_KITCHEN: + type: geometric::RRTConnect + range: 4.2357 + RRTConnect_TABLE_PICK: + type: geometric::RRTConnect + range: 2.7199 + RRTConnect_DEFAULT: + type: geometric::RRTConnect + BiEST_BOOKSHELF_SMALL: + type: geometric::BiEST + range: 1.149 + BiEST_BOOKSHELF_TALL: + type: geometric::BiEST + range: 1.499 + BiEST_BOOKSHELF_THIN: + type: geometric::BiEST + range: 1.205 + BiEST_BOX: + type: geometric::BiEST + range: 14.157 + BiEST_CAGE: + type: geometric::BiEST + range: 1.4018 + BiEST_KITCHEN: + type: geometric::BiEST + range: 14.248 + BiEST_TABLE_PICK: + type: geometric::BiEST + range: 14.116 + BiEST_DEFAULT: + type: geometric::BiEST + BiEST_0_05: + type: geometric::BiEST + range: 0.05 + BiEST_0_25: + type: geometric::BiEST + range: 0.25 + BiEST_0_50: + type: geometric::BiEST + range: 0.50 + BiEST_0_75: + type: geometric::BiEST + range: 0.75 + BiEST_1_00: + type: geometric::BiEST + range: 1.00 + BiEST_1_25: + type: geometric::BiEST + range: 1.25 + BiEST_1_50: + type: geometric::BiEST + range: 1.50 + BiEST_1_75: + type: geometric::BiEST + range: 1.75 + BiEST_2_00: + type: geometric::BiEST + range: 2.00 + BiEST_2_25: + type: geometric::BiEST + range: 2.25 + BiEST_2_50: + type: geometric::BiEST + range: 2.50 + BiEST_2_75: + type: geometric::BiEST + range: 2.75 + BiEST_3_00: + type: geometric::BiEST + range: 3.00 + BiEST_3_25: + type: geometric::BiEST + range: 3.25 + BiEST_3_50: + type: geometric::BiEST + range: 3.50 + BiEST_3_75: + type: geometric::BiEST + range: 3.75 + BiEST_4_00: + type: geometric::BiEST + range: 4.00 + BiEST_4_25: + type: geometric::BiEST + range: 4.25 + BiEST_4_50: + type: geometric::BiEST + range: 4.50 + BiEST_4_75: + type: geometric::BiEST + range: 4.75 + BiEST_5_00: + type: geometric::BiEST + range: 5.00 + BiEST_5_25: + type: geometric::BiEST + range: 5.25 + BiEST_5_50: + type: geometric::BiEST + range: 5.50 + BiEST_5_75: + type: geometric::BiEST + range: 5.75 + BiEST_6_00: + type: geometric::BiEST + range: 6.00 + BiEST_6_25: + type: geometric::BiEST + range: 6.25 + BiEST_6_50: + type: geometric::BiEST + range: 6.50 + BiEST_6_75: + type: geometric::BiEST + range: 6.75 + BiEST_7_00: + type: geometric::BiEST + range: 7.00 + BKPIECE_0_05: + type: geometric::BKPIECE + range: 0.05 + BKPIECE_0_25: + type: geometric::BKPIECE + range: 0.25 + BKPIECE_0_50: + type: geometric::BKPIECE + range: 0.50 + BKPIECE_0_75: + type: geometric::BKPIECE + range: 0.75 + BKPIECE_1_00: + type: geometric::BKPIECE + range: 1.00 + BKPIECE_1_25: + type: geometric::BKPIECE + range: 1.25 + BKPIECE_1_50: + type: geometric::BKPIECE + range: 1.50 + BKPIECE_1_75: + type: geometric::BKPIECE + range: 1.75 + BKPIECE_2_00: + type: geometric::BKPIECE + range: 2.00 + BKPIECE_2_25: + type: geometric::BKPIECE + range: 2.25 + BKPIECE_2_50: + type: geometric::BKPIECE + range: 2.50 + BKPIECE_2_75: + type: geometric::BKPIECE + range: 2.75 + BKPIECE_3_00: + type: geometric::BKPIECE + range: 3.00 + BKPIECE_3_25: + type: geometric::BKPIECE + range: 3.25 + BKPIECE_3_50: + type: geometric::BKPIECE + range: 3.50 + BKPIECE_3_75: + type: geometric::BKPIECE + range: 3.75 + BKPIECE_4_00: + type: geometric::BKPIECE + range: 4.00 + BKPIECE_4_25: + type: geometric::BKPIECE + range: 4.25 + BKPIECE_4_50: + type: geometric::BKPIECE + range: 4.50 + BKPIECE_4_75: + type: geometric::BKPIECE + range: 4.75 + BKPIECE_5_00: + type: geometric::BKPIECE + range: 5.00 + BKPIECE_5_25: + type: geometric::BKPIECE + range: 5.25 + BKPIECE_5_50: + type: geometric::BKPIECE + range: 5.50 + BKPIECE_5_75: + type: geometric::BKPIECE + range: 5.75 + BKPIECE_6_00: + type: geometric::BKPIECE + range: 6.00 + BKPIECE_6_25: + type: geometric::BKPIECE + range: 6.25 + BKPIECE_6_50: + type: geometric::BKPIECE + range: 6.50 + BKPIECE_6_75: + type: geometric::BKPIECE + range: 6.75 + BKPIECE_7_00: + type: geometric::BKPIECE + range: 7.00 + RRTConnect_0_05: + type: geometric::RRTConnect + range: 0.05 + RRTConnect_0_25: + type: geometric::RRTConnect + range: 0.25 + RRTConnect_0_50: + type: geometric::RRTConnect + range: 0.50 + RRTConnect_0_75: + type: geometric::RRTConnect + range: 0.75 + RRTConnect_1_00: + type: geometric::RRTConnect + range: 1.00 + RRTConnect_1_25: + type: geometric::RRTConnect + range: 1.25 + RRTConnect_1_50: + type: geometric::RRTConnect + range: 1.50 + RRTConnect_1_75: + type: geometric::RRTConnect + range: 1.75 + RRTConnect_2_00: + type: geometric::RRTConnect + range: 2.00 + RRTConnect_2_25: + type: geometric::RRTConnect + range: 2.25 + RRTConnect_2_50: + type: geometric::RRTConnect + range: 2.50 + RRTConnect_2_75: + type: geometric::RRTConnect + range: 2.75 + RRTConnect_3_00: + type: geometric::RRTConnect + range: 3.00 + RRTConnect_3_25: + type: geometric::RRTConnect + range: 3.25 + RRTConnect_3_50: + type: geometric::RRTConnect + range: 3.50 + RRTConnect_3_75: + type: geometric::RRTConnect + range: 3.75 + RRTConnect_4_00: + type: geometric::RRTConnect + range: 4.00 + RRTConnect_4_25: + type: geometric::RRTConnect + range: 4.25 + RRTConnect_4_50: + type: geometric::RRTConnect + range: 4.50 + RRTConnect_4_75: + type: geometric::RRTConnect + range: 4.75 + RRTConnect_5_00: + type: geometric::RRTConnect + range: 5.00 + RRTConnect_5_25: + type: geometric::RRTConnect + range: 5.25 + RRTConnect_5_50: + type: geometric::RRTConnect + range: 5.50 + RRTConnect_5_75: + type: geometric::RRTConnect + range: 5.75 + RRTConnect_6_00: + type: geometric::RRTConnect + range: 6.00 + RRTConnect_6_25: + type: geometric::RRTConnect + range: 6.25 + RRTConnect_6_50: + type: geometric::RRTConnect + range: 6.50 + RRTConnect_6_75: + type: geometric::RRTConnect + range: 6.75 + RRTConnect_7_00: + type: geometric::RRTConnect + range: 7.00 +arm_with_torso: + planner_configs: + - BKPIECE_BOOKSHELF_SMALL + - BKPIECE_BOOKSHELF_TALL + - BKPIECE_BOOKSHELF_THIN + - BKPIECE_BOX + - BKPIECE_CAGE + - BKPIECE_KITCHEN + - BKPIECE_TABLE_PICK + - BKPIECE_DEFAULT + - RRTConnect_BOOKSHELF_SMALL + - RRTConnect_BOOKSHELF_TALL + - RRTConnect_BOOKSHELF_THIN + - RRTConnect_BOX + - RRTConnect_CAGE + - RRTConnect_KITCHEN + - RRTConnect_TABLE_PICK + - RRTConnect_DEFAULT + - BiEST_BOOKSHELF_SMALL + - BiEST_BOOKSHELF_TALL + - BiEST_BOOKSHELF_THIN + - BiEST_BOX + - BiEST_CAGE + - BiEST_KITCHEN + - BiEST_TABLE_PICK + - BiEST_DEFAULT + - RRTConnect_0_05 + - RRTConnect_0_25 + - RRTConnect_0_50 + - RRTConnect_0_75 + - RRTConnect_1_00 + - RRTConnect_1_25 + - RRTConnect_1_50 + - RRTConnect_1_75 + - RRTConnect_2_00 + - RRTConnect_2_25 + - RRTConnect_2_50 + - RRTConnect_2_75 + - RRTConnect_3_00 + - RRTConnect_3_25 + - RRTConnect_3_50 + - RRTConnect_3_75 + - RRTConnect_4_00 + - RRTConnect_4_25 + - RRTConnect_4_50 + - RRTConnect_4_75 + - RRTConnect_5_00 + - RRTConnect_5_25 + - RRTConnect_5_50 + - RRTConnect_5_75 + - RRTConnect_6_00 + - RRTConnect_6_25 + - RRTConnect_6_50 + - RRTConnect_6_75 + - RRTConnect_7_00 + - BiEST_0_05 + - BiEST_0_25 + - BiEST_0_50 + - BiEST_0_75 + - BiEST_1_00 + - BiEST_1_25 + - BiEST_1_50 + - BiEST_1_75 + - BiEST_2_00 + - BiEST_2_25 + - BiEST_2_50 + - BiEST_2_75 + - BiEST_3_00 + - BiEST_3_25 + - BiEST_3_50 + - BiEST_3_75 + - BiEST_4_00 + - BiEST_4_25 + - BiEST_4_50 + - BiEST_4_75 + - BiEST_5_00 + - BiEST_5_25 + - BiEST_5_50 + - BiEST_5_75 + - BiEST_6_00 + - BiEST_6_25 + - BiEST_6_50 + - BiEST_6_75 + - BiEST_7_00 + - BKPIECE_0_05 + - BKPIECE_0_25 + - BKPIECE_0_50 + - BKPIECE_0_75 + - BKPIECE_1_00 + - BKPIECE_1_25 + - BKPIECE_1_50 + - BKPIECE_1_75 + - BKPIECE_2_00 + - BKPIECE_2_25 + - BKPIECE_2_50 + - BKPIECE_2_75 + - BKPIECE_3_00 + - BKPIECE_3_25 + - BKPIECE_3_50 + - BKPIECE_3_75 + - BKPIECE_4_00 + - BKPIECE_4_25 + - BKPIECE_4_50 + - BKPIECE_4_75 + - BKPIECE_5_00 + - BKPIECE_5_25 + - BKPIECE_5_50 + - BKPIECE_5_75 + - BKPIECE_6_00 + - BKPIECE_6_25 + - BKPIECE_6_50 + - BKPIECE_6_75 + - BKPIECE_7_00 + projection_evaluator: joints(torso_lift_joint,shoulder_pan_joint) + longest_valid_segment_fraction: 0.005 diff --git a/configs/ompl/ompl_planning_ur5.yaml b/configs/ompl/ompl_planning_ur5.yaml index 9f7f07b..6dd6737 100644 --- a/configs/ompl/ompl_planning_ur5.yaml +++ b/configs/ompl/ompl_planning_ur5.yaml @@ -46,12 +46,6 @@ planner_configs: type: geometric::BITstar AITstarkConfigDefault: type: geometric::AITstar - ThunderConfig: - type: geometric::RRTConnect - experience: "thunder" - LightningConfig: - type: geometric::RRTConnect - experience: "lightning" TRRTkConfigDefault: type: geometric::TRRT range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() From bc3de1e629489ccb8aa6893c5738f6ce6e2693f3 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Tue, 21 Jun 2022 21:34:48 +0000 Subject: [PATCH 05/16] refactored ompl config files --- configs/ompl/ompl_planning_baxter.yaml | 539 +------------------------ configs/ompl/ompl_planning_fetch.yaml | 3 + configs/ompl/ompl_planning_panda.yaml | 440 -------------------- configs/ompl/ompl_planning_ur5.yaml | 350 ---------------- docker/Dockerfile | 3 - 5 files changed, 13 insertions(+), 1322 deletions(-) diff --git a/configs/ompl/ompl_planning_baxter.yaml b/configs/ompl/ompl_planning_baxter.yaml index ed53087..85b01c0 100644 --- a/configs/ompl/ompl_planning_baxter.yaml +++ b/configs/ompl/ompl_planning_baxter.yaml @@ -52,272 +52,9 @@ planner_configs: max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 PRMstarkConfigDefault: type: geometric::PRMstar - ThunderConfig: - type: geometric::RRTConnect - range: 0.5 - experience: "thunder" - BiEST_0_05: - type: geometric::BiEST - range: 0.05 - BiEST_0_25: - type: geometric::BiEST - range: 0.25 - BiEST_0_50: - type: geometric::BiEST - range: 0.50 - BiEST_0_75: - type: geometric::BiEST - range: 0.75 - BiEST_1_00: - type: geometric::BiEST - range: 1.00 - BiEST_1_25: - type: geometric::BiEST - range: 1.25 - BiEST_1_50: - type: geometric::BiEST - range: 1.50 - BiEST_1_75: - type: geometric::BiEST - range: 1.75 - BiEST_2_00: - type: geometric::BiEST - range: 2.00 - BiEST_2_25: - type: geometric::BiEST - range: 2.25 - BiEST_2_50: - type: geometric::BiEST - range: 2.50 - BiEST_2_75: - type: geometric::BiEST - range: 2.75 - BiEST_3_00: - type: geometric::BiEST - range: 3.00 - BiEST_3_25: - type: geometric::BiEST - range: 3.25 - BiEST_3_50: - type: geometric::BiEST - range: 3.50 - BiEST_3_75: - type: geometric::BiEST - range: 3.75 - BiEST_4_00: - type: geometric::BiEST - range: 4.00 - BiEST_4_25: - type: geometric::BiEST - range: 4.25 - BiEST_4_50: - type: geometric::BiEST - range: 4.50 - BiEST_4_75: - type: geometric::BiEST - range: 4.75 - BiEST_5_00: - type: geometric::BiEST - range: 5.00 - BiEST_5_25: + BiESTkConfigDefault: type: geometric::BiEST - range: 5.25 - BiEST_5_50: - type: geometric::BiEST - range: 5.50 - BiEST_5_75: - type: geometric::BiEST - range: 5.75 - BiEST_6_00: - type: geometric::BiEST - range: 6.00 - BiEST_6_25: - type: geometric::BiEST - range: 6.25 - BiEST_6_50: - type: geometric::BiEST - range: 6.50 - BiEST_6_75: - type: geometric::BiEST - range: 6.75 - BiEST_7_00: - type: geometric::BiEST - range: 7.00 - BKPIECE_0_05: - type: geometric::BKPIECE - range: 0.05 - BKPIECE_0_25: - type: geometric::BKPIECE - range: 0.25 - BKPIECE_0_50: - type: geometric::BKPIECE - range: 0.50 - BKPIECE_0_75: - type: geometric::BKPIECE - range: 0.75 - BKPIECE_1_00: - type: geometric::BKPIECE - range: 1.00 - BKPIECE_1_25: - type: geometric::BKPIECE - range: 1.25 - BKPIECE_1_50: - type: geometric::BKPIECE - range: 1.50 - BKPIECE_1_75: - type: geometric::BKPIECE - range: 1.75 - BKPIECE_2_00: - type: geometric::BKPIECE - range: 2.00 - BKPIECE_2_25: - type: geometric::BKPIECE - range: 2.25 - BKPIECE_2_50: - type: geometric::BKPIECE - range: 2.50 - BKPIECE_2_75: - type: geometric::BKPIECE - range: 2.75 - BKPIECE_3_00: - type: geometric::BKPIECE - range: 3.00 - BKPIECE_3_25: - type: geometric::BKPIECE - range: 3.25 - BKPIECE_3_50: - type: geometric::BKPIECE - range: 3.50 - BKPIECE_3_75: - type: geometric::BKPIECE - range: 3.75 - BKPIECE_4_00: - type: geometric::BKPIECE - range: 4.00 - BKPIECE_4_25: - type: geometric::BKPIECE - range: 4.25 - BKPIECE_4_50: - type: geometric::BKPIECE - range: 4.50 - BKPIECE_4_75: - type: geometric::BKPIECE - range: 4.75 - BKPIECE_5_00: - type: geometric::BKPIECE - range: 5.00 - BKPIECE_5_25: - type: geometric::BKPIECE - range: 5.25 - BKPIECE_5_50: - type: geometric::BKPIECE - range: 5.50 - BKPIECE_5_75: - type: geometric::BKPIECE - range: 5.75 - BKPIECE_6_00: - type: geometric::BKPIECE - range: 6.00 - BKPIECE_6_25: - type: geometric::BKPIECE - range: 6.25 - BKPIECE_6_50: - type: geometric::BKPIECE - range: 6.50 - BKPIECE_6_75: - type: geometric::BKPIECE - range: 6.75 - BKPIECE_7_00: - type: geometric::BKPIECE - range: 7.00 - RRTConnect_0_05: - type: geometric::RRTConnect - range: 0.05 - RRTConnect_0_25: - type: geometric::RRTConnect - range: 0.25 - RRTConnect_0_50: - type: geometric::RRTConnect - range: 0.50 - RRTConnect_0_75: - type: geometric::RRTConnect - range: 0.75 - RRTConnect_1_00: - type: geometric::RRTConnect - range: 1.00 - RRTConnect_1_25: - type: geometric::RRTConnect - range: 1.25 - RRTConnect_1_50: - type: geometric::RRTConnect - range: 1.50 - RRTConnect_1_75: - type: geometric::RRTConnect - range: 1.75 - RRTConnect_2_00: - type: geometric::RRTConnect - range: 2.00 - RRTConnect_2_25: - type: geometric::RRTConnect - range: 2.25 - RRTConnect_2_50: - type: geometric::RRTConnect - range: 2.50 - RRTConnect_2_75: - type: geometric::RRTConnect - range: 2.75 - RRTConnect_3_00: - type: geometric::RRTConnect - range: 3.00 - RRTConnect_3_25: - type: geometric::RRTConnect - range: 3.25 - RRTConnect_3_50: - type: geometric::RRTConnect - range: 3.50 - RRTConnect_3_75: - type: geometric::RRTConnect - range: 3.75 - RRTConnect_4_00: - type: geometric::RRTConnect - range: 4.00 - RRTConnect_4_25: - type: geometric::RRTConnect - range: 4.25 - RRTConnect_4_50: - type: geometric::RRTConnect - range: 4.50 - RRTConnect_4_75: - type: geometric::RRTConnect - range: 4.75 - RRTConnect_5_00: - type: geometric::RRTConnect - range: 5.00 - RRTConnect_5_25: - type: geometric::RRTConnect - range: 5.25 - RRTConnect_5_50: - type: geometric::RRTConnect - range: 5.50 - RRTConnect_5_75: - type: geometric::RRTConnect - range: 5.75 - RRTConnect_6_00: - type: geometric::RRTConnect - range: 6.00 - RRTConnect_6_25: - type: geometric::RRTConnect - range: 6.25 - RRTConnect_6_50: - type: geometric::RRTConnect - range: 6.50 - RRTConnect_6_75: - type: geometric::RRTConnect - range: 6.75 - RRTConnect_7_00: - type: geometric::RRTConnect - range: 7.00 - + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() left_arm: planner_configs: - SBLkConfigDefault @@ -331,94 +68,7 @@ left_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault - - ThunderConfig - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 + - BiESTkConfigDefault projection_evaluator: joints(left_s0,left_s1) longest_valid_segment_fraction: 0.005 right_arm: @@ -434,94 +84,7 @@ right_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault - - ThunderConfig - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 + - BiESTkConfigDefault projection_evaluator: joints(right_s0,right_s1) longest_valid_segment_fraction: 0.005 right_3dof: @@ -537,6 +100,7 @@ right_3dof: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault + - BiESTkConfigDefault projection_evaluator: joints(right_w0,right_w1) longest_valid_segment_fraction: 0.005 right_4dof: @@ -552,6 +116,7 @@ right_4dof: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault + - BiESTkConfigDefault projection_evaluator: joints(right_e1,right_w0) longest_valid_segment_fraction: 0.005 right_5dof: @@ -567,6 +132,7 @@ right_5dof: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault + - BiESTkConfigDefault projection_evaluator: joints(right_e0,right_e1) longest_valid_segment_fraction: 0.005 both_arms: @@ -582,94 +148,7 @@ both_arms: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault - - ThunderConfig - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 + - BiESTkConfigDefault projection_evaluator: joints(left_s0,left_s1) longest_valid_segment_fraction: 0.005 left_hand: @@ -685,6 +164,7 @@ left_hand: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault + - BiESTkConfigDefault right_hand: planner_configs: - SBLkConfigDefault @@ -698,3 +178,4 @@ right_hand: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault + - BiESTkConfigDefault diff --git a/configs/ompl/ompl_planning_fetch.yaml b/configs/ompl/ompl_planning_fetch.yaml index b8b5f21..1fabfd5 100644 --- a/configs/ompl/ompl_planning_fetch.yaml +++ b/configs/ompl/ompl_planning_fetch.yaml @@ -17,6 +17,8 @@ planner_configs: type: geometric::RRTstar TRRTkConfigDefault: type: geometric::TRRT + BiESTkConfigDefault: + type: geometric::BiEST PRMkConfigDefault: type: geometric::PRM PRMstarkConfigDefault: @@ -36,6 +38,7 @@ arm_with_torso: - RRTConnectkConfigDefault - RRTstarkConfigDefault - TRRTkConfigDefault + - BiESTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault - BITstarkConfigDefault diff --git a/configs/ompl/ompl_planning_panda.yaml b/configs/ompl/ompl_planning_panda.yaml index 20df9af..522d3cc 100644 --- a/configs/ompl/ompl_planning_panda.yaml +++ b/configs/ompl/ompl_planning_panda.yaml @@ -124,271 +124,6 @@ planner_configs: sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 - TrajOptDefault: - type: geometric::TrajOpt - BiEST_0_05: - type: geometric::BiEST - range: 0.05 - BiEST_0_25: - type: geometric::BiEST - range: 0.25 - BiEST_0_50: - type: geometric::BiEST - range: 0.50 - BiEST_0_75: - type: geometric::BiEST - range: 0.75 - BiEST_1_00: - type: geometric::BiEST - range: 1.00 - BiEST_1_25: - type: geometric::BiEST - range: 1.25 - BiEST_1_50: - type: geometric::BiEST - range: 1.50 - BiEST_1_75: - type: geometric::BiEST - range: 1.75 - BiEST_2_00: - type: geometric::BiEST - range: 2.00 - BiEST_2_25: - type: geometric::BiEST - range: 2.25 - BiEST_2_50: - type: geometric::BiEST - range: 2.50 - BiEST_2_75: - type: geometric::BiEST - range: 2.75 - BiEST_3_00: - type: geometric::BiEST - range: 3.00 - BiEST_3_25: - type: geometric::BiEST - range: 3.25 - BiEST_3_50: - type: geometric::BiEST - range: 3.50 - BiEST_3_75: - type: geometric::BiEST - range: 3.75 - BiEST_4_00: - type: geometric::BiEST - range: 4.00 - BiEST_4_25: - type: geometric::BiEST - range: 4.25 - BiEST_4_50: - type: geometric::BiEST - range: 4.50 - BiEST_4_75: - type: geometric::BiEST - range: 4.75 - BiEST_5_00: - type: geometric::BiEST - range: 5.00 - BiEST_5_25: - type: geometric::BiEST - range: 5.25 - BiEST_5_50: - type: geometric::BiEST - range: 5.50 - BiEST_5_75: - type: geometric::BiEST - range: 5.75 - BiEST_6_00: - type: geometric::BiEST - range: 6.00 - BiEST_6_25: - type: geometric::BiEST - range: 6.25 - BiEST_6_50: - type: geometric::BiEST - range: 6.50 - BiEST_6_75: - type: geometric::BiEST - range: 6.75 - BiEST_7_00: - type: geometric::BiEST - range: 7.00 - BKPIECE_0_05: - type: geometric::BKPIECE - range: 0.05 - BKPIECE_0_25: - type: geometric::BKPIECE - range: 0.25 - BKPIECE_0_50: - type: geometric::BKPIECE - range: 0.50 - BKPIECE_0_75: - type: geometric::BKPIECE - range: 0.75 - BKPIECE_1_00: - type: geometric::BKPIECE - range: 1.00 - BKPIECE_1_25: - type: geometric::BKPIECE - range: 1.25 - BKPIECE_1_50: - type: geometric::BKPIECE - range: 1.50 - BKPIECE_1_75: - type: geometric::BKPIECE - range: 1.75 - BKPIECE_2_00: - type: geometric::BKPIECE - range: 2.00 - BKPIECE_2_25: - type: geometric::BKPIECE - range: 2.25 - BKPIECE_2_50: - type: geometric::BKPIECE - range: 2.50 - BKPIECE_2_75: - type: geometric::BKPIECE - range: 2.75 - BKPIECE_3_00: - type: geometric::BKPIECE - range: 3.00 - BKPIECE_3_25: - type: geometric::BKPIECE - range: 3.25 - BKPIECE_3_50: - type: geometric::BKPIECE - range: 3.50 - BKPIECE_3_75: - type: geometric::BKPIECE - range: 3.75 - BKPIECE_4_00: - type: geometric::BKPIECE - range: 4.00 - BKPIECE_4_25: - type: geometric::BKPIECE - range: 4.25 - BKPIECE_4_50: - type: geometric::BKPIECE - range: 4.50 - BKPIECE_4_75: - type: geometric::BKPIECE - range: 4.75 - BKPIECE_5_00: - type: geometric::BKPIECE - range: 5.00 - BKPIECE_5_25: - type: geometric::BKPIECE - range: 5.25 - BKPIECE_5_50: - type: geometric::BKPIECE - range: 5.50 - BKPIECE_5_75: - type: geometric::BKPIECE - range: 5.75 - BKPIECE_6_00: - type: geometric::BKPIECE - range: 6.00 - BKPIECE_6_25: - type: geometric::BKPIECE - range: 6.25 - BKPIECE_6_50: - type: geometric::BKPIECE - range: 6.50 - BKPIECE_6_75: - type: geometric::BKPIECE - range: 6.75 - BKPIECE_7_00: - type: geometric::BKPIECE - range: 7.00 - RRTConnect_0_05: - type: geometric::RRTConnect - range: 0.05 - RRTConnect_0_25: - type: geometric::RRTConnect - range: 0.25 - RRTConnect_0_50: - type: geometric::RRTConnect - range: 0.50 - RRTConnect_0_75: - type: geometric::RRTConnect - range: 0.75 - RRTConnect_1_00: - type: geometric::RRTConnect - range: 1.00 - RRTConnect_1_25: - type: geometric::RRTConnect - range: 1.25 - RRTConnect_1_50: - type: geometric::RRTConnect - range: 1.50 - RRTConnect_1_75: - type: geometric::RRTConnect - range: 1.75 - RRTConnect_2_00: - type: geometric::RRTConnect - range: 2.00 - RRTConnect_2_25: - type: geometric::RRTConnect - range: 2.25 - RRTConnect_2_50: - type: geometric::RRTConnect - range: 2.50 - RRTConnect_2_75: - type: geometric::RRTConnect - range: 2.75 - RRTConnect_3_00: - type: geometric::RRTConnect - range: 3.00 - RRTConnect_3_25: - type: geometric::RRTConnect - range: 3.25 - RRTConnect_3_50: - type: geometric::RRTConnect - range: 3.50 - RRTConnect_3_75: - type: geometric::RRTConnect - range: 3.75 - RRTConnect_4_00: - type: geometric::RRTConnect - range: 4.00 - RRTConnect_4_25: - type: geometric::RRTConnect - range: 4.25 - RRTConnect_4_50: - type: geometric::RRTConnect - range: 4.50 - RRTConnect_4_75: - type: geometric::RRTConnect - range: 4.75 - RRTConnect_5_00: - type: geometric::RRTConnect - range: 5.00 - RRTConnect_5_25: - type: geometric::RRTConnect - range: 5.25 - RRTConnect_5_50: - type: geometric::RRTConnect - range: 5.50 - RRTConnect_5_75: - type: geometric::RRTConnect - range: 5.75 - RRTConnect_6_00: - type: geometric::RRTConnect - range: 6.00 - RRTConnect_6_25: - type: geometric::RRTConnect - range: 6.25 - RRTConnect_6_50: - type: geometric::RRTConnect - range: 6.50 - RRTConnect_6_75: - type: geometric::RRTConnect - range: 6.75 - RRTConnect_7_00: - type: geometric::RRTConnect - range: 7.00 - - panda_arm: planner_configs: - SBLkConfigDefault @@ -417,93 +152,6 @@ panda_arm: - TrajOptDefault - BITstarkConfigDefault - AITstarkConfigDefault - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 projection_evaluator: link(panda_link8) panda_arm_hand: planner_configs: @@ -530,96 +178,8 @@ panda_arm_hand: - LazyPRMstarkConfigDefault - SPARSkConfigDefault - SPARStwokConfigDefault - - TrajOptDefault - BITstarkConfigDefault - AITstarkConfigDefault - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 projection_evaluator: link(panda_link8) hand: planner_configs: diff --git a/configs/ompl/ompl_planning_ur5.yaml b/configs/ompl/ompl_planning_ur5.yaml index 6dd6737..4b2a33e 100644 --- a/configs/ompl/ompl_planning_ur5.yaml +++ b/configs/ompl/ompl_planning_ur5.yaml @@ -130,267 +130,6 @@ planner_configs: sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 - BiEST_0_05: - type: geometric::BiEST - range: 0.05 - BiEST_0_25: - type: geometric::BiEST - range: 0.25 - BiEST_0_50: - type: geometric::BiEST - range: 0.50 - BiEST_0_75: - type: geometric::BiEST - range: 0.75 - BiEST_1_00: - type: geometric::BiEST - range: 1.00 - BiEST_1_25: - type: geometric::BiEST - range: 1.25 - BiEST_1_50: - type: geometric::BiEST - range: 1.50 - BiEST_1_75: - type: geometric::BiEST - range: 1.75 - BiEST_2_00: - type: geometric::BiEST - range: 2.00 - BiEST_2_25: - type: geometric::BiEST - range: 2.25 - BiEST_2_50: - type: geometric::BiEST - range: 2.50 - BiEST_2_75: - type: geometric::BiEST - range: 2.75 - BiEST_3_00: - type: geometric::BiEST - range: 3.00 - BiEST_3_25: - type: geometric::BiEST - range: 3.25 - BiEST_3_50: - type: geometric::BiEST - range: 3.50 - BiEST_3_75: - type: geometric::BiEST - range: 3.75 - BiEST_4_00: - type: geometric::BiEST - range: 4.00 - BiEST_4_25: - type: geometric::BiEST - range: 4.25 - BiEST_4_50: - type: geometric::BiEST - range: 4.50 - BiEST_4_75: - type: geometric::BiEST - range: 4.75 - BiEST_5_00: - type: geometric::BiEST - range: 5.00 - BiEST_5_25: - type: geometric::BiEST - range: 5.25 - BiEST_5_50: - type: geometric::BiEST - range: 5.50 - BiEST_5_75: - type: geometric::BiEST - range: 5.75 - BiEST_6_00: - type: geometric::BiEST - range: 6.00 - BiEST_6_25: - type: geometric::BiEST - range: 6.25 - BiEST_6_50: - type: geometric::BiEST - range: 6.50 - BiEST_6_75: - type: geometric::BiEST - range: 6.75 - BiEST_7_00: - type: geometric::BiEST - range: 7.00 - BKPIECE_0_05: - type: geometric::BKPIECE - range: 0.05 - BKPIECE_0_25: - type: geometric::BKPIECE - range: 0.25 - BKPIECE_0_50: - type: geometric::BKPIECE - range: 0.50 - BKPIECE_0_75: - type: geometric::BKPIECE - range: 0.75 - BKPIECE_1_00: - type: geometric::BKPIECE - range: 1.00 - BKPIECE_1_25: - type: geometric::BKPIECE - range: 1.25 - BKPIECE_1_50: - type: geometric::BKPIECE - range: 1.50 - BKPIECE_1_75: - type: geometric::BKPIECE - range: 1.75 - BKPIECE_2_00: - type: geometric::BKPIECE - range: 2.00 - BKPIECE_2_25: - type: geometric::BKPIECE - range: 2.25 - BKPIECE_2_50: - type: geometric::BKPIECE - range: 2.50 - BKPIECE_2_75: - type: geometric::BKPIECE - range: 2.75 - BKPIECE_3_00: - type: geometric::BKPIECE - range: 3.00 - BKPIECE_3_25: - type: geometric::BKPIECE - range: 3.25 - BKPIECE_3_50: - type: geometric::BKPIECE - range: 3.50 - BKPIECE_3_75: - type: geometric::BKPIECE - range: 3.75 - BKPIECE_4_00: - type: geometric::BKPIECE - range: 4.00 - BKPIECE_4_25: - type: geometric::BKPIECE - range: 4.25 - BKPIECE_4_50: - type: geometric::BKPIECE - range: 4.50 - BKPIECE_4_75: - type: geometric::BKPIECE - range: 4.75 - BKPIECE_5_00: - type: geometric::BKPIECE - range: 5.00 - BKPIECE_5_25: - type: geometric::BKPIECE - range: 5.25 - BKPIECE_5_50: - type: geometric::BKPIECE - range: 5.50 - BKPIECE_5_75: - type: geometric::BKPIECE - range: 5.75 - BKPIECE_6_00: - type: geometric::BKPIECE - range: 6.00 - BKPIECE_6_25: - type: geometric::BKPIECE - range: 6.25 - BKPIECE_6_50: - type: geometric::BKPIECE - range: 6.50 - BKPIECE_6_75: - type: geometric::BKPIECE - range: 6.75 - BKPIECE_7_00: - type: geometric::BKPIECE - range: 7.00 - RRTConnect_0_05: - type: geometric::RRTConnect - range: 0.05 - RRTConnect_0_25: - type: geometric::RRTConnect - range: 0.25 - RRTConnect_0_50: - type: geometric::RRTConnect - range: 0.50 - RRTConnect_0_75: - type: geometric::RRTConnect - range: 0.75 - RRTConnect_1_00: - type: geometric::RRTConnect - range: 1.00 - RRTConnect_1_25: - type: geometric::RRTConnect - range: 1.25 - RRTConnect_1_50: - type: geometric::RRTConnect - range: 1.50 - RRTConnect_1_75: - type: geometric::RRTConnect - range: 1.75 - RRTConnect_2_00: - type: geometric::RRTConnect - range: 2.00 - RRTConnect_2_25: - type: geometric::RRTConnect - range: 2.25 - RRTConnect_2_50: - type: geometric::RRTConnect - range: 2.50 - RRTConnect_2_75: - type: geometric::RRTConnect - range: 2.75 - RRTConnect_3_00: - type: geometric::RRTConnect - range: 3.00 - RRTConnect_3_25: - type: geometric::RRTConnect - range: 3.25 - RRTConnect_3_50: - type: geometric::RRTConnect - range: 3.50 - RRTConnect_3_75: - type: geometric::RRTConnect - range: 3.75 - RRTConnect_4_00: - type: geometric::RRTConnect - range: 4.00 - RRTConnect_4_25: - type: geometric::RRTConnect - range: 4.25 - RRTConnect_4_50: - type: geometric::RRTConnect - range: 4.50 - RRTConnect_4_75: - type: geometric::RRTConnect - range: 4.75 - RRTConnect_5_00: - type: geometric::RRTConnect - range: 5.00 - RRTConnect_5_25: - type: geometric::RRTConnect - range: 5.25 - RRTConnect_5_50: - type: geometric::RRTConnect - range: 5.50 - RRTConnect_5_75: - type: geometric::RRTConnect - range: 5.75 - RRTConnect_6_00: - type: geometric::RRTConnect - range: 6.00 - RRTConnect_6_25: - type: geometric::RRTConnect - range: 6.25 - RRTConnect_6_50: - type: geometric::RRTConnect - range: 6.50 - RRTConnect_6_75: - type: geometric::RRTConnect - range: 6.75 - RRTConnect_7_00: - type: geometric::RRTConnect - range: 7.00 manipulator: planner_configs: - SBLkConfigDefault @@ -421,94 +160,5 @@ manipulator: - SPARStwokConfigDefault - BITstarkConfigDefault - AITstarkConfigDefault - - ThunderConfig - - LightningConfig - - RRTConnect_0_05 - - RRTConnect_0_25 - - RRTConnect_0_50 - - RRTConnect_0_75 - - RRTConnect_1_00 - - RRTConnect_1_25 - - RRTConnect_1_50 - - RRTConnect_1_75 - - RRTConnect_2_00 - - RRTConnect_2_25 - - RRTConnect_2_50 - - RRTConnect_2_75 - - RRTConnect_3_00 - - RRTConnect_3_25 - - RRTConnect_3_50 - - RRTConnect_3_75 - - RRTConnect_4_00 - - RRTConnect_4_25 - - RRTConnect_4_50 - - RRTConnect_4_75 - - RRTConnect_5_00 - - RRTConnect_5_25 - - RRTConnect_5_50 - - RRTConnect_5_75 - - RRTConnect_6_00 - - RRTConnect_6_25 - - RRTConnect_6_50 - - RRTConnect_6_75 - - RRTConnect_7_00 - - BiEST_0_05 - - BiEST_0_25 - - BiEST_0_50 - - BiEST_0_75 - - BiEST_1_00 - - BiEST_1_25 - - BiEST_1_50 - - BiEST_1_75 - - BiEST_2_00 - - BiEST_2_25 - - BiEST_2_50 - - BiEST_2_75 - - BiEST_3_00 - - BiEST_3_25 - - BiEST_3_50 - - BiEST_3_75 - - BiEST_4_00 - - BiEST_4_25 - - BiEST_4_50 - - BiEST_4_75 - - BiEST_5_00 - - BiEST_5_25 - - BiEST_5_50 - - BiEST_5_75 - - BiEST_6_00 - - BiEST_6_25 - - BiEST_6_50 - - BiEST_6_75 - - BiEST_7_00 - - BKPIECE_0_05 - - BKPIECE_0_25 - - BKPIECE_0_50 - - BKPIECE_0_75 - - BKPIECE_1_00 - - BKPIECE_1_25 - - BKPIECE_1_50 - - BKPIECE_1_75 - - BKPIECE_2_00 - - BKPIECE_2_25 - - BKPIECE_2_50 - - BKPIECE_2_75 - - BKPIECE_3_00 - - BKPIECE_3_25 - - BKPIECE_3_50 - - BKPIECE_3_75 - - BKPIECE_4_00 - - BKPIECE_4_25 - - BKPIECE_4_50 - - BKPIECE_4_75 - - BKPIECE_5_00 - - BKPIECE_5_25 - - BKPIECE_5_50 - - BKPIECE_5_75 - - BKPIECE_6_00 - - BKPIECE_6_25 - - BKPIECE_6_50 - - BKPIECE_6_75 - - BKPIECE_7_00 projection_evaluator: link(ee_link) longest_valid_segment_fraction: 0.001 diff --git a/docker/Dockerfile b/docker/Dockerfile index a7f951b..9092edd 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -25,9 +25,6 @@ RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE= RUN mkdir -p ./src/motion_bench_maker ADD ./ ./src/motion_bench_maker/ -RUN ls ./src/motion_bench_maker -RUN ls ./src/ - # Clone MotionBenchMaker and dependencies RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \ rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \ From 224fcc3a14aa498d630354029c43e11fa3842a29 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Tue, 28 Jun 2022 19:58:09 -0500 Subject: [PATCH 06/16] Minor fixes --- include/motion_bench_maker/octomap_generator.h | 1 + src/octomap_generator.cpp | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/include/motion_bench_maker/octomap_generator.h b/include/motion_bench_maker/octomap_generator.h index a7dc758..48d5384 100644 --- a/include/motion_bench_maker/octomap_generator.h +++ b/include/motion_bench_maker/octomap_generator.h @@ -72,6 +72,7 @@ namespace robowflex const IO::RVIZHelperPtr &rviz = nullptr); CloudXYZPtr getLastPointCloud(); + occupancy_map_monitor::OccMapTreePtr getLastOctomapTree(); private: gds::Mesh geomToMesh(const GeometryPtr &geom, const std::string &name); diff --git a/src/octomap_generator.cpp b/src/octomap_generator.cpp index cf76579..a0edfc4 100644 --- a/src/octomap_generator.cpp +++ b/src/octomap_generator.cpp @@ -95,11 +95,14 @@ CloudXYZPtr OctomapGenerator::generateCloud(const RobotPose &cam_pose) gds::toPointCloudXYZ(props_, depth_img, cloud); auto tr = cam_pose.translation(); + auto qr = cam_pose.rotation(); // cloud.sensor_origin_ = tr; cloud.sensor_origin_.x() = tr.x(); cloud.sensor_origin_.y() = tr.y(); cloud.sensor_origin_.z() = tr.z(); + cloud.sensor_orientation_ = qr.cast(); + return std::make_shared(cloud); } @@ -223,3 +226,8 @@ CloudXYZPtr OctomapGenerator::getLastPointCloud() { return fullCloud_; } + +occupancy_map_monitor::OccMapTreePtr OctomapGenerator::getLastOctomapTree() +{ + return tree_; +} From c52443f5b430729fa689b250b7808a08a1128956 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Wed, 29 Jun 2022 18:23:36 -0500 Subject: [PATCH 07/16] Fixed clipping --- .../motion_bench_maker/octomap_generator.h | 8 ++- src/octomap_generator.cpp | 50 ++++++++++++++----- 2 files changed, 44 insertions(+), 14 deletions(-) diff --git a/include/motion_bench_maker/octomap_generator.h b/include/motion_bench_maker/octomap_generator.h index 48d5384..c5a7898 100644 --- a/include/motion_bench_maker/octomap_generator.h +++ b/include/motion_bench_maker/octomap_generator.h @@ -62,6 +62,11 @@ namespace robowflex // @brief loads a scene to the void loadScene(const SceneConstPtr &scene); + // Creates a pointcloud with points at max range. This can help create an octomap + // with correct free cells. + + void setClipAtMaxRange(bool clip_at_max_range); + void clear(); // pose The position of the camera in ROS standard coordinates (+Z down camera LoS) CloudXYZPtr generateCloud(const RobotPose &cam_pose); @@ -79,13 +84,12 @@ namespace robowflex void parseCameraProporties(const std::string &config); gds::CameraProperties props_; Sensors sensors_; + bool clip_at_max_range_; ScenePtr cloudToOctomap(const RobotPose &cam_pose, pcl::PointCloud); std::shared_ptr sim_; occupancy_map_monitor::OccMapTreePtr tree_; CloudXYZPtr fullCloud_; - - octomap::KeyRay key_ray_; }; } // namespace robowflex diff --git a/src/octomap_generator.cpp b/src/octomap_generator.cpp index a0edfc4..49f718e 100644 --- a/src/octomap_generator.cpp +++ b/src/octomap_generator.cpp @@ -36,6 +36,9 @@ OctomapGenerator::OctomapGenerator(const std::string &config) tree_ = std::make_shared(sensors_.resolution); sim_ = std::make_shared(props_); fullCloud_ = std::make_shared(); + + // The default clip_at_max_range behaviour is false. + clip_at_max_range_ = false; }; RobotPose OctomapGenerator::lookat(const Eigen::Vector3d &eye, const Eigen::Vector3d &origin) @@ -84,14 +87,32 @@ void OctomapGenerator::loadScene(const SceneConstPtr &scene) { const auto &obj = scene->getObjectGeometry(name); const auto &mesh = geomToMesh(obj, name); + // TODO: does this need to be cleared everytime? sim_->add(name, mesh, toMatrix(scene->getObjectPose(name))); } } +void OctomapGenerator::setClipAtMaxRange(bool clip_at_max_range) +{ + clip_at_max_range_ = clip_at_max_range; +} + +void OctomapGenerator::clear() +{ + tree_->clear(); + fullCloud_->clear(); +} + CloudXYZPtr OctomapGenerator::generateCloud(const RobotPose &cam_pose) { CloudXYZ cloud; - const auto &depth_img = sim_->render(toMatrix(cam_pose)); + auto depth_img = sim_->render(toMatrix(cam_pose)); + + // Use z_far for areas with no objects + if (clip_at_max_range_) + for (auto &d : depth_img.data) + d = d == 0 ? props_.z_far : d; + gds::toPointCloudXYZ(props_, depth_img, cloud); auto tr = cam_pose.translation(); @@ -109,8 +130,7 @@ CloudXYZPtr OctomapGenerator::generateCloud(const RobotPose &cam_pose) bool OctomapGenerator::geomToSensed(const ScenePtr &geometric, const ScenePtr &sensed, const IO::RVIZHelperPtr &rviz) { - tree_->clear(); - fullCloud_->clear(); + clear(); auto start = ros::WallTime::now(); loadScene(geometric); @@ -155,7 +175,8 @@ bool OctomapGenerator::updateOctoMap(const CloudXYZPtr &cloud, const RobotPose & // sensor origin is the cloud origin octomap::point3d sensor_origin(cso.x(), cso.y(), cso.z()); - octomap::KeySet free_cells, occupied_cells; + octomap::KeySet free_cells, occupied_cells, clipped_cells; + octomap::KeyRay key_ray_; tree_->lockRead(); @@ -169,23 +190,28 @@ bool OctomapGenerator::updateOctoMap(const CloudXYZPtr &cloud, const RobotPose & if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) { // transform to camera frame - auto point = cam_pose * Eigen::Vector3d{p.x, p.y, p.z}; - occupied_cells.insert(tree_->coordToKey(point.x(), point.y(), point.z())); - fullCloud_->push_back(pcl::PointXYZ(point.x(), point.y(), point.z())); + auto vec = Eigen::Vector3d{p.x, p.y, p.z}; + auto point = cam_pose * vec; + // 0.999 is to account for numerical errors + if (vec.norm() < props_.z_far * (0.999)) + { + occupied_cells.insert(tree_->coordToKey(point.x(), point.y(), point.z())); + fullCloud_->push_back(pcl::PointXYZ(point.x(), point.y(), point.z())); + } + else + clipped_cells.insert(tree_->coordToKey(point.x(), point.y(), point.z())); } } - // TODO: Maybe I should remove this completelely /* compute the free cells along each ray that ends at an occupied cell */ for (auto it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it) if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_)) free_cells.insert(key_ray_.begin(), key_ray_.end()); /* compute the free cells along each ray that ends at a clipped cell */ - // for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_cells.end(); it != end; - // ++it) - // if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_)) - // free_cells.insert(key_ray_.begin(), key_ray_.end()); + for (auto it = clipped_cells.begin(), end = clipped_cells.end(); it != end; ++it) + if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_)) + free_cells.insert(key_ray_.begin(), key_ray_.end()); } catch (...) From 8838c913d9768b9f1a6feaf356efb79a836fd173 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Wed, 6 Jul 2022 16:29:52 -0500 Subject: [PATCH 08/16] Added helper function --- include/motion_bench_maker/octomap_generator.h | 1 + src/octomap_generator.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/include/motion_bench_maker/octomap_generator.h b/include/motion_bench_maker/octomap_generator.h index c5a7898..981fbf6 100644 --- a/include/motion_bench_maker/octomap_generator.h +++ b/include/motion_bench_maker/octomap_generator.h @@ -78,6 +78,7 @@ namespace robowflex CloudXYZPtr getLastPointCloud(); occupancy_map_monitor::OccMapTreePtr getLastOctomapTree(); + const occupancy_map_monitor::OccMapTreeConstPtr getLastOctomapTreeConst(); private: gds::Mesh geomToMesh(const GeometryPtr &geom, const std::string &name); diff --git a/src/octomap_generator.cpp b/src/octomap_generator.cpp index 49f718e..bbc1c1f 100644 --- a/src/octomap_generator.cpp +++ b/src/octomap_generator.cpp @@ -257,3 +257,8 @@ occupancy_map_monitor::OccMapTreePtr OctomapGenerator::getLastOctomapTree() { return tree_; } + +const occupancy_map_monitor::OccMapTreeConstPtr OctomapGenerator::getLastOctomapTreeConst() +{ + return collision_detection::OccMapTreeConstPtr(tree_); +} From 20a4361e591b2484adfec8484ad34fd31e5408c4 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Fri, 8 Jul 2022 16:50:43 -0500 Subject: [PATCH 09/16] added something: --- include/motion_bench_maker/octomap_generator.h | 1 + src/octomap_generator.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/include/motion_bench_maker/octomap_generator.h b/include/motion_bench_maker/octomap_generator.h index 981fbf6..bef8adc 100644 --- a/include/motion_bench_maker/octomap_generator.h +++ b/include/motion_bench_maker/octomap_generator.h @@ -77,6 +77,7 @@ namespace robowflex const IO::RVIZHelperPtr &rviz = nullptr); CloudXYZPtr getLastPointCloud(); + const gds::CameraProperties getCameraProperties(); occupancy_map_monitor::OccMapTreePtr getLastOctomapTree(); const occupancy_map_monitor::OccMapTreeConstPtr getLastOctomapTreeConst(); diff --git a/src/octomap_generator.cpp b/src/octomap_generator.cpp index bbc1c1f..3dd6dc9 100644 --- a/src/octomap_generator.cpp +++ b/src/octomap_generator.cpp @@ -253,6 +253,11 @@ CloudXYZPtr OctomapGenerator::getLastPointCloud() return fullCloud_; } +const gds::CameraProperties OctomapGenerator::getCameraProperties() +{ + return props_; +} + occupancy_map_monitor::OccMapTreePtr OctomapGenerator::getLastOctomapTree() { return tree_; From abd40cc9de63ae9b4ea54e3ff00b50804cc26cab Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Sat, 23 Jul 2022 17:16:31 +0800 Subject: [PATCH 10/16] Added AITstar/Bitstar int he config files --- configs/ompl/ompl_planning_baxter.yaml | 20 ++++++++++++++++++++ configs/ompl/ompl_planning_shadow.yaml | 12 ++++++++---- 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/configs/ompl/ompl_planning_baxter.yaml b/configs/ompl/ompl_planning_baxter.yaml index 85b01c0..0d0d785 100644 --- a/configs/ompl/ompl_planning_baxter.yaml +++ b/configs/ompl/ompl_planning_baxter.yaml @@ -55,6 +55,10 @@ planner_configs: BiESTkConfigDefault: type: geometric::BiEST range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + BITstarkConfigDefault: + type: geometric::BITstar + AITstarkConfigDefault: + type: geometric::AITstar left_arm: planner_configs: - SBLkConfigDefault @@ -69,6 +73,8 @@ left_arm: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(left_s0,left_s1) longest_valid_segment_fraction: 0.005 right_arm: @@ -85,6 +91,8 @@ right_arm: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(right_s0,right_s1) longest_valid_segment_fraction: 0.005 right_3dof: @@ -101,6 +109,8 @@ right_3dof: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(right_w0,right_w1) longest_valid_segment_fraction: 0.005 right_4dof: @@ -117,6 +127,8 @@ right_4dof: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(right_e1,right_w0) longest_valid_segment_fraction: 0.005 right_5dof: @@ -133,6 +145,8 @@ right_5dof: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(right_e0,right_e1) longest_valid_segment_fraction: 0.005 both_arms: @@ -149,6 +163,8 @@ both_arms: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(left_s0,left_s1) longest_valid_segment_fraction: 0.005 left_hand: @@ -165,6 +181,8 @@ left_hand: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault right_hand: planner_configs: - SBLkConfigDefault @@ -179,3 +197,5 @@ right_hand: - PRMkConfigDefault - PRMstarkConfigDefault - BiESTkConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault diff --git a/configs/ompl/ompl_planning_shadow.yaml b/configs/ompl/ompl_planning_shadow.yaml index 44813f4..1218852 100644 --- a/configs/ompl/ompl_planning_shadow.yaml +++ b/configs/ompl/ompl_planning_shadow.yaml @@ -120,8 +120,10 @@ planner_configs: sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 - projection_evaluator: joints(FFJ1) - longest_valid_segment_fraction: 0.005 + BITstarkConfigDefault: + type: geometric::BITstar + AITstarkConfigDefault: + type: geometric::AITstar all_fingers: planner_configs: - SBLkConfigDefault @@ -147,6 +149,8 @@ all_fingers: - LazyPRMstarkConfigDefault - SPARSkConfigDefault - SPARStwokConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(FFJ1) longest_valid_segment_fraction: 0.005 ff_finger: @@ -174,7 +178,7 @@ ff_finger: - LazyPRMstarkConfigDefault - SPARSkConfigDefault - SPARStwokConfigDefault + - BITstarkConfigDefault + - AITstarkConfigDefault projection_evaluator: joints(FFJ1) longest_valid_segment_fraction: 0.005 - - From be9252dde47e7d4e3ebb5bb75465096df5b37e98 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Sun, 24 Jul 2022 15:53:02 +0800 Subject: [PATCH 11/16] Added new docker --- docker/noetic-src/Dockerfile | 49 +++++++++++++++++++++++++++++++ docker/noetic-src/build-docker.sh | 4 +++ docker/noetic-src/run-docker.sh | 12 ++++++++ 3 files changed, 65 insertions(+) create mode 100644 docker/noetic-src/Dockerfile create mode 100755 docker/noetic-src/build-docker.sh create mode 100755 docker/noetic-src/run-docker.sh diff --git a/docker/noetic-src/Dockerfile b/docker/noetic-src/Dockerfile new file mode 100644 index 0000000..c9bb7e4 --- /dev/null +++ b/docker/noetic-src/Dockerfile @@ -0,0 +1,49 @@ +FROM ros:noetic-ros-base +MAINTAINER Constantinos Chamzas chamzas@rice.edu + +ENV ROS_UNDERLAY /ws/devel +ENV CATKIN_WS $(realpath $ROS_UNDERLAY/..) + +# Dependencies +RUN apt-get update && \ + DEBIAN_FRONTEND=noninteractive apt-get install -y build-essential git python3-catkin-tools libhdf5-dev libpcl-dev zip wget \ + ros-${ROS_DISTRO}-eigen-conversions \ + ros-${ROS_DISTRO}-tf-conversions \ + ros-${ROS_DISTRO}-pcl-conversions \ + ros-${ROS_DISTRO}-fcl + +# Needed to generate pointclouds +ENV NVIDIA_VISIBLE_DEVICES all +ENV NVIDIA_DRIVER_CAPABILITIES all + +# Create catkin workspace +WORKDIR $ROS_UNDERLAY/.. +RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release + +# Get & Install OMPL +WORKDIR /ws/src +RUN wget -O - https://github.com/ompl/ompl/archive/1.5.0.tar.gz | tar zxf - +RUN catkin build ompl --start-with ompl --limit-status-rate 0.001 --no-notify + +# Get & Install MoveIt from Source +WORKDIR /ws +RUN wstool init src && \ + wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/noetic-devel/moveit.rosinstall && \ + wstool update -t src && \ + rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} + +RUN catkin build --limit-status-rate 0.001 --no-notify + +# Copy the current motion benchmaker files to the workspace +RUN mkdir -p ./src/motion_bench_maker +ADD ./ ./src/motion_bench_maker/ + +# Clone MotionBenchMaker and dependencies +RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \ + rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \ + git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \ + git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \ + git clone --depth 1 https://github.com/TAMS-Group/bio_ik ./src/bio_ik && \ + rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ + catkin build --limit-status-rate 0.001 --no-notify && \ + sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh diff --git a/docker/noetic-src/build-docker.sh b/docker/noetic-src/build-docker.sh new file mode 100755 index 0000000..649be8b --- /dev/null +++ b/docker/noetic-src/build-docker.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +cd "${0%/*}" +docker build -f Dockerfile -t motion_bench_maker_moveit_src ../../ diff --git a/docker/noetic-src/run-docker.sh b/docker/noetic-src/run-docker.sh new file mode 100755 index 0000000..b70e449 --- /dev/null +++ b/docker/noetic-src/run-docker.sh @@ -0,0 +1,12 @@ +#Run with gpu support + +if [ "$1" = "gpu" ] ; then + docker run --rm -it --gpus all -v /usr/share/glvnd/egl_vendor.d:/usr/share/glvnd/egl_vendor.d motion_bench_maker +elif [ "$1" = "no-gpu" ] ; then + docker run --rm -it motion_bench_maker +else + echo "You need to specify if you want gpu support or not. Syntax: ./run-docker.sh {gpu, no-gpu}" + exit +fi + +#Run without gpu support From 547645eb71f60b6e7ac0199f4eb576dfe1b6dbd3 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Sun, 24 Jul 2022 16:04:02 +0800 Subject: [PATCH 12/16] Updated DockerFile --- docker/noetic-src/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docker/noetic-src/Dockerfile b/docker/noetic-src/Dockerfile index c9bb7e4..69d80e7 100644 --- a/docker/noetic-src/Dockerfile +++ b/docker/noetic-src/Dockerfile @@ -30,7 +30,9 @@ WORKDIR /ws RUN wstool init src && \ wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/noetic-devel/moveit.rosinstall && \ wstool update -t src && \ - rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} + rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} && \ + rm -r ./src/moveit_core && \ + git clone -b noetic-devel-add-bitstar-aitstar --depth 1 https://github.com/KavrakiLab/moveit_core ./src/moveit_core RUN catkin build --limit-status-rate 0.001 --no-notify From e8f635348b2887b481f5f7bdb7de0dfdcf6dda8b Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Sun, 24 Jul 2022 11:59:20 +0000 Subject: [PATCH 13/16] Moved docker file in top level directory --- docker/noetic-src/build-docker.sh | 4 ---- {docker/noetic-src => noetic-src}/Dockerfile | 5 +++-- noetic-src/build-docker.sh | 4 ++++ {docker/noetic-src => noetic-src}/run-docker.sh | 0 4 files changed, 7 insertions(+), 6 deletions(-) delete mode 100755 docker/noetic-src/build-docker.sh rename {docker/noetic-src => noetic-src}/Dockerfile (92%) create mode 100755 noetic-src/build-docker.sh rename {docker/noetic-src => noetic-src}/run-docker.sh (100%) diff --git a/docker/noetic-src/build-docker.sh b/docker/noetic-src/build-docker.sh deleted file mode 100755 index 649be8b..0000000 --- a/docker/noetic-src/build-docker.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -cd "${0%/*}" -docker build -f Dockerfile -t motion_bench_maker_moveit_src ../../ diff --git a/docker/noetic-src/Dockerfile b/noetic-src/Dockerfile similarity index 92% rename from docker/noetic-src/Dockerfile rename to noetic-src/Dockerfile index 69d80e7..0b6c584 100644 --- a/docker/noetic-src/Dockerfile +++ b/noetic-src/Dockerfile @@ -7,6 +7,7 @@ ENV CATKIN_WS $(realpath $ROS_UNDERLAY/..) # Dependencies RUN apt-get update && \ DEBIAN_FRONTEND=noninteractive apt-get install -y build-essential git python3-catkin-tools libhdf5-dev libpcl-dev zip wget \ + libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ ros-${ROS_DISTRO}-eigen-conversions \ ros-${ROS_DISTRO}-tf-conversions \ ros-${ROS_DISTRO}-pcl-conversions \ @@ -31,8 +32,8 @@ RUN wstool init src && \ wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/noetic-devel/moveit.rosinstall && \ wstool update -t src && \ rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} && \ - rm -r ./src/moveit_core && \ - git clone -b noetic-devel-add-bitstar-aitstar --depth 1 https://github.com/KavrakiLab/moveit_core ./src/moveit_core + rm -r ./src/moveit/ && \ + git clone -b noetic-devel-add-bitstar-aitstar --depth 1 https://github.com/ChamzasKonstantinos/moveit ./src/moveit RUN catkin build --limit-status-rate 0.001 --no-notify diff --git a/noetic-src/build-docker.sh b/noetic-src/build-docker.sh new file mode 100755 index 0000000..42af553 --- /dev/null +++ b/noetic-src/build-docker.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +cd "${0%/*}" +docker build -f Dockerfile -t motion_bench_maker_moveit_src ../ diff --git a/docker/noetic-src/run-docker.sh b/noetic-src/run-docker.sh similarity index 100% rename from docker/noetic-src/run-docker.sh rename to noetic-src/run-docker.sh From 398339d67d0c1fe9733f8e642ad30d51f27fc604 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Mon, 25 Jul 2022 02:22:09 +0000 Subject: [PATCH 14/16] Fixed docker source file --- CMakeLists.txt | 5 +++++ noetic-src/Dockerfile | 3 +-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e44c1f..b298fb7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,9 +12,13 @@ list(APPEND CATKIN_SOURCES ) find_package(catkin REQUIRED COMPONENTS ${CATKIN_SOURCES}) + #Octomap is not a a catkin package, a.k.a. it has to be found like this find_package(octomap) +#We need boost to build succesfully +find_package(Boost REQUIRED filesystem serialization) + # needed to convert octomaps SET(OpenGL_GL_PREFERENCE "GLVND") find_package(gl_depth_sim) @@ -51,6 +55,7 @@ list(APPEND LIBRARIES gl_depth_sim::glad gl_depth_sim::gl_depth_sim_interfaces yaml-cpp + Boost::serialization ) diff --git a/noetic-src/Dockerfile b/noetic-src/Dockerfile index 0b6c584..fb40a6e 100644 --- a/noetic-src/Dockerfile +++ b/noetic-src/Dockerfile @@ -6,8 +6,7 @@ ENV CATKIN_WS $(realpath $ROS_UNDERLAY/..) # Dependencies RUN apt-get update && \ - DEBIAN_FRONTEND=noninteractive apt-get install -y build-essential git python3-catkin-tools libhdf5-dev libpcl-dev zip wget \ - libboost-all-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ + DEBIAN_FRONTEND=noninteractive apt-get install -y build-essential git python3-catkin-tools libhdf5-dev libpcl-dev zip wget libboost-all-dev libboost-serialization-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \ ros-${ROS_DISTRO}-eigen-conversions \ ros-${ROS_DISTRO}-tf-conversions \ ros-${ROS_DISTRO}-pcl-conversions \ From 7496d1d33b09aac0702942cb29b1fd301c659b95 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Mon, 25 Jul 2022 02:39:51 +0000 Subject: [PATCH 15/16] Moved docker back to the lower level --- {noetic-src => docker/noetic-src}/Dockerfile | 0 docker/noetic-src/build-docker.sh | 4 ++++ {noetic-src => docker/noetic-src}/run-docker.sh | 0 noetic-src/build-docker.sh | 4 ---- 4 files changed, 4 insertions(+), 4 deletions(-) rename {noetic-src => docker/noetic-src}/Dockerfile (100%) create mode 100755 docker/noetic-src/build-docker.sh rename {noetic-src => docker/noetic-src}/run-docker.sh (100%) delete mode 100755 noetic-src/build-docker.sh diff --git a/noetic-src/Dockerfile b/docker/noetic-src/Dockerfile similarity index 100% rename from noetic-src/Dockerfile rename to docker/noetic-src/Dockerfile diff --git a/docker/noetic-src/build-docker.sh b/docker/noetic-src/build-docker.sh new file mode 100755 index 0000000..649be8b --- /dev/null +++ b/docker/noetic-src/build-docker.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +cd "${0%/*}" +docker build -f Dockerfile -t motion_bench_maker_moveit_src ../../ diff --git a/noetic-src/run-docker.sh b/docker/noetic-src/run-docker.sh similarity index 100% rename from noetic-src/run-docker.sh rename to docker/noetic-src/run-docker.sh diff --git a/noetic-src/build-docker.sh b/noetic-src/build-docker.sh deleted file mode 100755 index 42af553..0000000 --- a/noetic-src/build-docker.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -cd "${0%/*}" -docker build -f Dockerfile -t motion_bench_maker_moveit_src ../ From 69a889744ae9b2def55ab6d8be2398111150d810 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Mon, 25 Jul 2022 02:53:25 +0000 Subject: [PATCH 16/16] Fixed shadow --- configs/ompl/ompl_planning_shadow.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configs/ompl/ompl_planning_shadow.yaml b/configs/ompl/ompl_planning_shadow.yaml index 1218852..a6e7ebb 100644 --- a/configs/ompl/ompl_planning_shadow.yaml +++ b/configs/ompl/ompl_planning_shadow.yaml @@ -153,7 +153,7 @@ all_fingers: - AITstarkConfigDefault projection_evaluator: joints(FFJ1) longest_valid_segment_fraction: 0.005 -ff_finger: +finger_ff: planner_configs: - SBLkConfigDefault - ESTkConfigDefault