Benchmarking Example/Visualizer for Sets of Scenes/Requests (#76)

* Added scene scripts and scene request files * Visualizing state with attached objects * Attaching object correctly. * Added get goalConfiguration function Co-authored-by: Constantinos Chamzas <chamzask@gmail.com>
KavrakiLab · Jul 13, 2020 · 4d517fe · 4d517fe
1 parent 903bbf5
commit 4d517fe
Show file tree

Hide file tree

Showing 23 changed files with 2,630 additions and 0 deletions.
diff --git a/robowflex_library/CMakeLists.txt b/robowflex_library/CMakeLists.txt
@@ -106,6 +106,8 @@ add_script(ur5_io)
 add_script(ur5_pool)
 add_script(ur5_visualization)
 add_script(fetch_test)
+add_script(fetch_scenes_visualize)
+add_script(fetch_scenes_benchmark)
 add_script(fetch_benchmark)
 add_script(fetch_visualization)
 add_script(r2_test)

diff --git a/robowflex_library/scripts/fetch_scenes_benchmark.cpp b/robowflex_library/scripts/fetch_scenes_benchmark.cpp
@@ -0,0 +1,77 @@
+/* Author: Constantinos Chamzas */
+
+// Robowflex
+#include <robowflex_library/util.h>
+#include <robowflex_library/geometry.h>
+#include <robowflex_library/robot.h>
+#include <robowflex_library/scene.h>
+#include <robowflex_library/planning.h>
+#include <robowflex_library/builder.h>
+#include <robowflex_library/benchmarking.h>
+#include <robowflex_library/detail/fetch.h>
+
+using namespace robowflex;
+
+static const std::string GROUP = "arm_with_torso";
+
+int main(int argc, char **argv)
+{
+    // Startup ROS
+    ROS ros(argc, argv);
+
+    // Create the default Fetch robot.
+    auto fetch = std::make_shared<FetchRobot>();
+    fetch->initialize();
+
+    // Setup a benchmarking request for the joint and pose motion plan requests.
+    Benchmarker benchmark;
+
+    int start = 1;
+    int end = 10;
+    for (int i = start; i <= end; i++)
+    {
+        std::stringstream ss;
+        // (pre-appending 4 leading zeros)
+        ss << std::setw(4) << std::setfill('0') << i;
+        std::string index = ss.str();
+
+        auto fscene = "package://robowflex_library/yaml/fetch_scenes/scene_vicon" + index + ".yaml";
+
+        // Create an empty Scene.
+        auto scene = std::make_shared<Scene>(fetch);
+
+        if (!scene->fromYAMLFile(fscene))
+        {
+            ROS_ERROR("Failed to read file: %s for scene", fscene.c_str());
+            continue;
+        }
+
+        // Create the default planner for the Fetch.
+        auto planner = std::make_shared<OMPL::FetchOMPLPipelinePlanner>(fetch, "default");
+        planner->initialize();
+
+        // Create an empty motion planning request.
+        auto request = std::make_shared<robowflex::MotionRequestBuilder>(planner, GROUP);
+
+        auto frequest = "package://robowflex_library/yaml/fetch_scenes/request" + index + ".yaml";
+
+        if (!request->fromYAMLFile(frequest))
+        {
+            ROS_ERROR("Failed to read file: %s for request", frequest.c_str());
+            continue;
+        }
+        // modify the planning request here
+        request->setNumPlanningAttempts(1);
+        request->setAllowedPlanningTime(10);
+
+        // Add benchmarking request
+        benchmark.addBenchmarkingRequest("result" + index, scene, planner, request);
+    }
+
+    // How many times to solve each problem
+    int reps = 2;
+    // 0b001101 disables costly metrics(some crash if no plan is found)
+    auto options = Benchmarker::Options(reps, 0b001101);
+    std::string bpath = ros::package::getPath("robowflex_library") + "/yaml/benchmark/";
+    benchmark.benchmark({std::make_shared<OMPLBenchmarkOutputter>(bpath)}, options);
+}
diff --git a/robowflex_library/scripts/fetch_scenes_visualize.cpp b/robowflex_library/scripts/fetch_scenes_visualize.cpp
@@ -0,0 +1,88 @@
+/* Author: Constantinos Chamzas */
+
+// Robowflex
+#include <robowflex_library/util.h>
+#include <robowflex_library/geometry.h>
+#include <robowflex_library/robot.h>
+#include <robowflex_library/scene.h>
+#include <robowflex_library/planning.h>
+#include <robowflex_library/io/visualization.h>
+#include <robowflex_library/builder.h>
+#include <robowflex_library/detail/fetch.h>
+
+using namespace robowflex;
+
+static const std::string GROUP = "arm_with_torso";
+
+int main(int argc, char **argv)
+{
+    // Startup ROS
+    ROS ros(argc, argv);
+
+    // Create the default Fetch robot.
+    auto fetch = std::make_shared<FetchRobot>();
+    fetch->initialize();
+
+    // Create an RViz visualization helper.
+    // Publishes all topics and parameter under `/robowflex` by default.
+    IO::RVIZHelper rviz(fetch);
+
+    ROS_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)...");
+    std::cin.get();
+
+    int start = 1;
+    int end = 10;
+    for (int i = start; i <= end; i++)
+    {
+        std::stringstream ss;
+        // (pre-appending 4 leading zeros)
+        ss << std::setw(4) << std::setfill('0') << i;
+        std::string index = ss.str();
+
+        auto fscene = "package://robowflex_library/yaml/fetch_scenes/scene_vicon" + index + ".yaml";
+
+        // Create an empty Scene.
+        auto scene = std::make_shared<Scene>(fetch);
+
+        if (!scene->fromYAMLFile(fscene))
+        {
+            ROS_ERROR("Failed to read file: %s for scene", fscene.c_str());
+            continue;
+        }
+
+        // Create the default planner for the Fetch.
+        auto planner = std::make_shared<OMPL::FetchOMPLPipelinePlanner>(fetch, "default");
+        planner->initialize();
+
+        // Create an empty motion planning request.
+        auto request = std::make_shared<robowflex::MotionRequestBuilder>(planner, GROUP);
+
+        auto frequest = "package://robowflex_library/yaml/fetch_scenes/request" + index + ".yaml";
+
+        if (!request->fromYAMLFile(frequest))
+        {
+            ROS_ERROR("Failed to read file: %s for request", frequest.c_str());
+            continue;
+        }
+
+        // Visualize the scene.
+        rviz.updateScene(scene);
+        rviz.updateMarkers();
+
+        ROS_INFO("Scene displayed! Press enter to plan...");
+        std::cin.get();
+
+        // Do motion planning!
+        planning_interface::MotionPlanResponse res = planner->plan(scene, request->getRequest());
+        if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
+            return 1;
+
+        // Publish the trajectory to a topic to display in RViz
+        rviz.updateTrajectory(res);
+
+        ROS_INFO("Press enter to remove the scene.");
+        std::cin.get();
+
+        rviz.removeScene();
+    }
+}
diff --git a/robowflex_library/yaml/fetch_scenes/request0001.yaml b/robowflex_library/yaml/fetch_scenes/request0001.yaml
@@ -0,0 +1,35 @@
+start_state:
+  joint_state:
+    position: [-0.001301890656828952, 0.001095591422250131, 0, 0.006638057602043759, 7.905552577014419e-07, 0.002398346642077165, 1.319999501869205, 1.39998238413443, -0.1999861759683759, 1.719957206645441, 9.227849400161858e-07, 1.660019041691295, 2.456531061234557e-06, 0.04992857891283312, 0.04995543695423495]
+    header:
+      frame_id: base_link
+    name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint]
+goal_constraints:
+  - joint_constraints:
+      - joint_name: torso_lift_joint
+        position: 0.3813146885304878
+      - joint_name: shoulder_pan_joint
+        position: -1.00151795317852
+      - joint_name: shoulder_lift_joint
+        position: 0.7259824827996758
+      - joint_name: upperarm_roll_joint
+        position: -2.247113267302562
+      - joint_name: elbow_flex_joint
+        position: 1.385221349117354
+      - joint_name: forearm_roll_joint
+        position: -2.738178902248606
+      - joint_name: wrist_flex_joint
+        position: 0.6328322327387069
+      - joint_name: wrist_roll_joint
+        position: -0.9584235758850088
+planner_id: RRTConnectkConfigDefault
+group_name: arm_with_torso
+num_planning_attempts: 3
+allowed_planning_time: 10
+max_velocity_scaling_factor: 0
+max_acceleration_scaling_factor: 0
+workspace_parameters:
+  header:
+    frame_id: ""
+  max_corner: [1, 1, 1]
+  min_corner: [-1, -1, -1]
diff --git a/robowflex_library/yaml/fetch_scenes/request0002.yaml b/robowflex_library/yaml/fetch_scenes/request0002.yaml
@@ -0,0 +1,35 @@
+workspace_parameters:
+  header:
+    frame_id: ""
+  max_corner: [1, 1, 1]
+  min_corner: [-1, -1, -1]
+goal_constraints:
+  - joint_constraints:
+      - joint_name: torso_lift_joint
+        position: 0.001383420381405535
+      - joint_name: shoulder_pan_joint
+        position: 0.4648722840775982
+      - joint_name: shoulder_lift_joint
+        position: -0.5074878033675915
+      - joint_name: upperarm_roll_joint
+        position: 1.778365204990892
+      - joint_name: elbow_flex_joint
+        position: 1.399065046316606
+      - joint_name: forearm_roll_joint
+        position: -2.465591618081946
+      - joint_name: wrist_flex_joint
+        position: 0.9340101381804361
+      - joint_name: wrist_roll_joint
+        position: 1.259637205870057
+planner_id: RRTConnectkConfigDefault
+group_name: arm_with_torso
+num_planning_attempts: 3
+allowed_planning_time: 10
+max_velocity_scaling_factor: 0
+max_acceleration_scaling_factor: 0
+start_state:
+  joint_state:
+    position: [-0.001216658772094981, 0.005465717010629589, 0.0001944184190247067, 0.006739301443282752, -1.57, 0.06100790031371073, 1.320009175556049, 1.399961007146933, -0.1999865151776357, 1.719984239537961, 5.04440564874642e-07, 1.65998376638543, -6.490962181082693e-06, 0.04996258843986121, 0.04998905699031816]
+    header:
+      frame_id: base_link
+    name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint]
diff --git a/robowflex_library/yaml/fetch_scenes/request0003.yaml b/robowflex_library/yaml/fetch_scenes/request0003.yaml
@@ -0,0 +1,35 @@
+goal_constraints:
+  - joint_constraints:
+      - joint_name: torso_lift_joint
+        position: 0
+      - joint_name: shoulder_pan_joint
+        position: -0.3219457710162654
+      - joint_name: shoulder_lift_joint
+        position: -1.031237409034907
+      - joint_name: upperarm_roll_joint
+        position: -0.5238098155892518
+      - joint_name: elbow_flex_joint
+        position: 1.248264881847507
+      - joint_name: forearm_roll_joint
+        position: 2.514573833120962
+      - joint_name: wrist_flex_joint
+        position: 0.2453362368243334
+      - joint_name: wrist_roll_joint
+        position: -2.251217315718256
+start_state:
+  joint_state:
+    position: [-0.001562896033541428, 0.009571000190605083, 0.000149862967199013, 0.006733143270455835, 0.1296726620695008, 0.05959118989887102, 1.320012968934249, 1.39988060011404, -0.1999708197304848, 1.72038053399174, -1.901323256703336e-05, 1.6596845021088, 1.903639478317842e-05, 0.04986566348681871, 0.04989687234441947]
+    name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint]
+    header:
+      frame_id: base_link
+workspace_parameters:
+  header:
+    frame_id: ""
+  max_corner: [1, 1, 1]
+  min_corner: [-1, -1, -1]
+planner_id: RRTConnectkConfigDefault
+group_name: arm_with_torso
+num_planning_attempts: 3
+allowed_planning_time: 10
+max_velocity_scaling_factor: 0
+max_acceleration_scaling_factor: 0
diff --git a/robowflex_library/yaml/fetch_scenes/request0004.yaml b/robowflex_library/yaml/fetch_scenes/request0004.yaml
@@ -0,0 +1,35 @@
+planner_id: RRTConnectkConfigDefault
+group_name: arm_with_torso
+num_planning_attempts: 3
+allowed_planning_time: 10
+max_velocity_scaling_factor: 0
+max_acceleration_scaling_factor: 0
+start_state:
+  joint_state:
+    name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint]
+    header:
+      frame_id: base_link
+    position: [-0.0007849328624542906, 0.01518767700433266, 0.0002210966487781457, 0.006770516995822764, -1.456004262010633, 0.06927821557024672, 1.319992589578521, 1.40005654689207, -0.2000057364221055, 1.719647223770044, 1.545727283236431e-05, 1.660256418083981, -1.775061979358838e-05, 0.04986774346086155, 0.04989131080041784]
+goal_constraints:
+  - joint_constraints:
+      - joint_name: torso_lift_joint
+        position: 0.2970653046338838
+      - joint_name: shoulder_pan_joint
+        position: 0.8268250225140396
+      - joint_name: shoulder_lift_joint
+        position: 0.2149171972880675
+      - joint_name: upperarm_roll_joint
+        position: -1.442793157354775
+      - joint_name: elbow_flex_joint
+        position: -1.406203255911677
+      - joint_name: forearm_roll_joint
+        position: -0.175685296938704
+      - joint_name: wrist_flex_joint
+        position: 0.9390853579292325
+      - joint_name: wrist_roll_joint
+        position: 1.478886118706163
+workspace_parameters:
+  header:
+    frame_id: ""
+  min_corner: [-1, -1, -1]
+  max_corner: [1, 1, 1]
diff --git a/robowflex_library/yaml/fetch_scenes/request0005.yaml b/robowflex_library/yaml/fetch_scenes/request0005.yaml
@@ -0,0 +1,35 @@
+goal_constraints:
+  - joint_constraints:
+      - joint_name: torso_lift_joint
+        position: 8.267346270450965e-05
+      - joint_name: shoulder_pan_joint
+        position: -0.7390899331497579
+      - joint_name: shoulder_lift_joint
+        position: -1.146840886899364
+      - joint_name: upperarm_roll_joint
+        position: 2.173463541140414
+      - joint_name: elbow_flex_joint
+        position: -1.548607351735822
+      - joint_name: forearm_roll_joint
+        position: -0.9368171321369169
+      - joint_name: wrist_flex_joint
+        position: 0.6847151250403862
+      - joint_name: wrist_roll_joint
+        position: -1.907179170364188
+start_state:
+  joint_state:
+    header:
+      frame_id: base_link
+    name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint]
+    position: [-0.00117985636496698, 0.01819934087407304, 0.000216298199833016, 0.006745437057228717, 0.346159108759629, 0.06220715250170183, 1.320024019859318, 1.399989855275235, -0.1999744819550378, 1.719963485404481, 6.291687152604197e-06, 1.659964506788162, -9.409996287068623e-06, 0.04983253857063437, 0.04985824435648394]
+planner_id: RRTConnectkConfigDefault
+group_name: arm_with_torso
+num_planning_attempts: 3
+allowed_planning_time: 10
+max_velocity_scaling_factor: 0
+max_acceleration_scaling_factor: 0
+workspace_parameters:
+  min_corner: [-1, -1, -1]
+  header:
+    frame_id: ""
+  max_corner: [1, 1, 1]
diff --git a/robowflex_library/yaml/fetch_scenes/request0006.yaml b/robowflex_library/yaml/fetch_scenes/request0006.yaml
@@ -0,0 +1,35 @@
+start_state:
+  joint_state:
+    name: [l_wheel_joint, r_wheel_joint, torso_lift_joint, bellows_joint, head_pan_joint, head_tilt_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint, l_gripper_finger_joint, r_gripper_finger_joint]
+    position: [-0.0007513192310932837, 0.02299412389440647, 0.0002172780170763, 0.006728862970468036, -1.57, 0.06739120431890111, 1.319991266344941, 1.399973211217874, -0.1999901678891369, 1.719987616872583, 4.924826370711344e-06, 1.659980714834496, 7.422737644091626e-06, 0.04989747207375791, 0.04992557815134854]
+    header:
+      frame_id: base_link
+planner_id: RRTConnectkConfigDefault
+group_name: arm_with_torso
+num_planning_attempts: 3
+allowed_planning_time: 10
+max_velocity_scaling_factor: 0
+max_acceleration_scaling_factor: 0
+goal_constraints:
+  - joint_constraints:
+      - joint_name: torso_lift_joint
+        position: 5.817591087453589e-08
+      - joint_name: shoulder_pan_joint
+        position: 0.09492901534222749
+      - joint_name: shoulder_lift_joint
+        position: -1.206952814771632
+      - joint_name: upperarm_roll_joint
+        position: -1.255509487338636
+      - joint_name: elbow_flex_joint
+        position: 1.726402749304374
+      - joint_name: forearm_roll_joint
+        position: -0.4336693715661215
+      - joint_name: wrist_flex_joint
+        position: -0.365113537340298
+      - joint_name: wrist_roll_joint
+        position: 0.7728088779874197
+workspace_parameters:
+  max_corner: [1, 1, 1]
+  header:
+    frame_id: ""
+  min_corner: [-1, -1, -1]