Skip to content

Commit 4d517fe

Browse files
zkingstonChamzasKonstantinos
zkingston
and
ChamzasKonstantinos
authored
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>
1 parent 903bbf5 commit 4d517fe

23 files changed

+2630
-0
lines changed

‎robowflex_library/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,8 @@ add_script(ur5_io)
106106
add_script(ur5_pool)
107107
add_script(ur5_visualization)
108108
add_script(fetch_test)
109+
add_script(fetch_scenes_visualize)
110+
add_script(fetch_scenes_benchmark)
109111
add_script(fetch_benchmark)
110112
add_script(fetch_visualization)
111113
add_script(r2_test)

‎robowflex_library/scripts/fetch_scenes_benchmark.cpp‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
/* Author: Constantinos Chamzas */
2+
3+
// Robowflex
4+
#include <robowflex_library/util.h>
5+
#include <robowflex_library/geometry.h>
6+
#include <robowflex_library/robot.h>
7+
#include <robowflex_library/scene.h>
8+
#include <robowflex_library/planning.h>
9+
#include <robowflex_library/builder.h>
10+
#include <robowflex_library/benchmarking.h>
11+
#include <robowflex_library/detail/fetch.h>
12+
13+
using namespace robowflex;
14+
15+
static const std::string GROUP = "arm_with_torso";
16+
17+
int main(int argc, char **argv)
18+
{
19+
// Startup ROS
20+
ROS ros(argc, argv);
21+
22+
// Create the default Fetch robot.
23+
auto fetch = std::make_shared<FetchRobot>();
24+
fetch->initialize();
25+
26+
// Setup a benchmarking request for the joint and pose motion plan requests.
27+
Benchmarker benchmark;
28+
29+
int start = 1;
30+
int end = 10;
31+
for (int i = start; i <= end; i++)
32+
{
33+
std::stringstream ss;
34+
// (pre-appending 4 leading zeros)
35+
ss << std::setw(4) << std::setfill('0') << i;
36+
std::string index = ss.str();
37+
38+
auto fscene = "package://robowflex_library/yaml/fetch_scenes/scene_vicon" + index + ".yaml";
39+
40+
// Create an empty Scene.
41+
auto scene = std::make_shared<Scene>(fetch);
42+
43+
if (!scene->fromYAMLFile(fscene))
44+
{
45+
ROS_ERROR("Failed to read file: %s for scene", fscene.c_str());
46+
continue;
47+
}
48+
49+
// Create the default planner for the Fetch.
50+
auto planner = std::make_shared<OMPL::FetchOMPLPipelinePlanner>(fetch, "default");
51+
planner->initialize();
52+
53+
// Create an empty motion planning request.
54+
auto request = std::make_shared<robowflex::MotionRequestBuilder>(planner, GROUP);
55+
56+
auto frequest = "package://robowflex_library/yaml/fetch_scenes/request" + index + ".yaml";
57+
58+
if (!request->fromYAMLFile(frequest))
59+
{
60+
ROS_ERROR("Failed to read file: %s for request", frequest.c_str());
61+
continue;
62+
}
63+
// modify the planning request here
64+
request->setNumPlanningAttempts(1);
65+
request->setAllowedPlanningTime(10);
66+
67+
// Add benchmarking request
68+
benchmark.addBenchmarkingRequest("result" + index, scene, planner, request);
69+
}
70+
71+
// How many times to solve each problem
72+
int reps = 2;
73+
// 0b001101 disables costly metrics(some crash if no plan is found)
74+
auto options = Benchmarker::Options(reps, 0b001101);
75+
std::string bpath = ros::package::getPath("robowflex_library") + "/yaml/benchmark/";
76+
benchmark.benchmark({std::make_shared<OMPLBenchmarkOutputter>(bpath)}, options);
77+
}

‎robowflex_library/scripts/fetch_scenes_visualize.cpp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
/* Author: Constantinos Chamzas */
2+
3+
// Robowflex
4+
#include <robowflex_library/util.h>
5+
#include <robowflex_library/geometry.h>
6+
#include <robowflex_library/robot.h>
7+
#include <robowflex_library/scene.h>
8+
#include <robowflex_library/planning.h>
9+
#include <robowflex_library/io/visualization.h>
10+
#include <robowflex_library/builder.h>
11+
#include <robowflex_library/detail/fetch.h>
12+
13+
using namespace robowflex;
14+
15+
static const std::string GROUP = "arm_with_torso";
16+
17+
int main(int argc, char **argv)
18+
{
19+
// Startup ROS
20+
ROS ros(argc, argv);
21+
22+
// Create the default Fetch robot.
23+
auto fetch = std::make_shared<FetchRobot>();
24+
fetch->initialize();
25+
26+
// Create an RViz visualization helper.
27+
// Publishes all topics and parameter under `/robowflex` by default.
28+
IO::RVIZHelper rviz(fetch);
29+
30+
ROS_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)...");
31+
std::cin.get();
32+
33+
int start = 1;
34+
int end = 10;
35+
for (int i = start; i <= end; i++)
36+
{
37+
std::stringstream ss;
38+
// (pre-appending 4 leading zeros)
39+
ss << std::setw(4) << std::setfill('0') << i;
40+
std::string index = ss.str();
41+
42+
auto fscene = "package://robowflex_library/yaml/fetch_scenes/scene_vicon" + index + ".yaml";
43+
44+
// Create an empty Scene.
45+
auto scene = std::make_shared<Scene>(fetch);
46+
47+
if (!scene->fromYAMLFile(fscene))
48+
{
49+
ROS_ERROR("Failed to read file: %s for scene", fscene.c_str());
50+
continue;
51+
}
52+
53+
// Create the default planner for the Fetch.
54+
auto planner = std::make_shared<OMPL::FetchOMPLPipelinePlanner>(fetch, "default");
55+
planner->initialize();
56+
57+
// Create an empty motion planning request.
58+
auto request = std::make_shared<robowflex::MotionRequestBuilder>(planner, GROUP);
59+
60+
auto frequest = "package://robowflex_library/yaml/fetch_scenes/request" + index + ".yaml";
61+
62+
if (!request->fromYAMLFile(frequest))
63+
{
64+
ROS_ERROR("Failed to read file: %s for request", frequest.c_str());
65+
continue;
66+
}
67+
68+
// Visualize the scene.
69+
rviz.updateScene(scene);
70+
rviz.updateMarkers();
71+
72+
ROS_INFO("Scene displayed! Press enter to plan...");
73+
std::cin.get();
74+
75+
// Do motion planning!
76+
planning_interface::MotionPlanResponse res = planner->plan(scene, request->getRequest());
77+
if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
78+
return 1;
79+
80+
// Publish the trajectory to a topic to display in RViz
81+
rviz.updateTrajectory(res);
82+
83+
ROS_INFO("Press enter to remove the scene.");
84+
std::cin.get();
85+
86+
rviz.removeScene();
87+
}
88+
}

‎robowflex_library/yaml/fetch_scenes/request0001.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
start_state:
2+
joint_state:
3+
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]
4+
header:
5+
frame_id: base_link
6+
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]
7+
goal_constraints:
8+
- joint_constraints:
9+
- joint_name: torso_lift_joint
10+
position: 0.3813146885304878
11+
- joint_name: shoulder_pan_joint
12+
position: -1.00151795317852
13+
- joint_name: shoulder_lift_joint
14+
position: 0.7259824827996758
15+
- joint_name: upperarm_roll_joint
16+
position: -2.247113267302562
17+
- joint_name: elbow_flex_joint
18+
position: 1.385221349117354
19+
- joint_name: forearm_roll_joint
20+
position: -2.738178902248606
21+
- joint_name: wrist_flex_joint
22+
position: 0.6328322327387069
23+
- joint_name: wrist_roll_joint
24+
position: -0.9584235758850088
25+
planner_id: RRTConnectkConfigDefault
26+
group_name: arm_with_torso
27+
num_planning_attempts: 3
28+
allowed_planning_time: 10
29+
max_velocity_scaling_factor: 0
30+
max_acceleration_scaling_factor: 0
31+
workspace_parameters:
32+
header:
33+
frame_id: ""
34+
max_corner: [1, 1, 1]
35+
min_corner: [-1, -1, -1]

‎robowflex_library/yaml/fetch_scenes/request0002.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
workspace_parameters:
2+
header:
3+
frame_id: ""
4+
max_corner: [1, 1, 1]
5+
min_corner: [-1, -1, -1]
6+
goal_constraints:
7+
- joint_constraints:
8+
- joint_name: torso_lift_joint
9+
position: 0.001383420381405535
10+
- joint_name: shoulder_pan_joint
11+
position: 0.4648722840775982
12+
- joint_name: shoulder_lift_joint
13+
position: -0.5074878033675915
14+
- joint_name: upperarm_roll_joint
15+
position: 1.778365204990892
16+
- joint_name: elbow_flex_joint
17+
position: 1.399065046316606
18+
- joint_name: forearm_roll_joint
19+
position: -2.465591618081946
20+
- joint_name: wrist_flex_joint
21+
position: 0.9340101381804361
22+
- joint_name: wrist_roll_joint
23+
position: 1.259637205870057
24+
planner_id: RRTConnectkConfigDefault
25+
group_name: arm_with_torso
26+
num_planning_attempts: 3
27+
allowed_planning_time: 10
28+
max_velocity_scaling_factor: 0
29+
max_acceleration_scaling_factor: 0
30+
start_state:
31+
joint_state:
32+
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]
33+
header:
34+
frame_id: base_link
35+
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]

‎robowflex_library/yaml/fetch_scenes/request0003.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
goal_constraints:
2+
- joint_constraints:
3+
- joint_name: torso_lift_joint
4+
position: 0
5+
- joint_name: shoulder_pan_joint
6+
position: -0.3219457710162654
7+
- joint_name: shoulder_lift_joint
8+
position: -1.031237409034907
9+
- joint_name: upperarm_roll_joint
10+
position: -0.5238098155892518
11+
- joint_name: elbow_flex_joint
12+
position: 1.248264881847507
13+
- joint_name: forearm_roll_joint
14+
position: 2.514573833120962
15+
- joint_name: wrist_flex_joint
16+
position: 0.2453362368243334
17+
- joint_name: wrist_roll_joint
18+
position: -2.251217315718256
19+
start_state:
20+
joint_state:
21+
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]
22+
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]
23+
header:
24+
frame_id: base_link
25+
workspace_parameters:
26+
header:
27+
frame_id: ""
28+
max_corner: [1, 1, 1]
29+
min_corner: [-1, -1, -1]
30+
planner_id: RRTConnectkConfigDefault
31+
group_name: arm_with_torso
32+
num_planning_attempts: 3
33+
allowed_planning_time: 10
34+
max_velocity_scaling_factor: 0
35+
max_acceleration_scaling_factor: 0

‎robowflex_library/yaml/fetch_scenes/request0004.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
planner_id: RRTConnectkConfigDefault
2+
group_name: arm_with_torso
3+
num_planning_attempts: 3
4+
allowed_planning_time: 10
5+
max_velocity_scaling_factor: 0
6+
max_acceleration_scaling_factor: 0
7+
start_state:
8+
joint_state:
9+
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]
10+
header:
11+
frame_id: base_link
12+
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]
13+
goal_constraints:
14+
- joint_constraints:
15+
- joint_name: torso_lift_joint
16+
position: 0.2970653046338838
17+
- joint_name: shoulder_pan_joint
18+
position: 0.8268250225140396
19+
- joint_name: shoulder_lift_joint
20+
position: 0.2149171972880675
21+
- joint_name: upperarm_roll_joint
22+
position: -1.442793157354775
23+
- joint_name: elbow_flex_joint
24+
position: -1.406203255911677
25+
- joint_name: forearm_roll_joint
26+
position: -0.175685296938704
27+
- joint_name: wrist_flex_joint
28+
position: 0.9390853579292325
29+
- joint_name: wrist_roll_joint
30+
position: 1.478886118706163
31+
workspace_parameters:
32+
header:
33+
frame_id: ""
34+
min_corner: [-1, -1, -1]
35+
max_corner: [1, 1, 1]

‎robowflex_library/yaml/fetch_scenes/request0005.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
goal_constraints:
2+
- joint_constraints:
3+
- joint_name: torso_lift_joint
4+
position: 8.267346270450965e-05
5+
- joint_name: shoulder_pan_joint
6+
position: -0.7390899331497579
7+
- joint_name: shoulder_lift_joint
8+
position: -1.146840886899364
9+
- joint_name: upperarm_roll_joint
10+
position: 2.173463541140414
11+
- joint_name: elbow_flex_joint
12+
position: -1.548607351735822
13+
- joint_name: forearm_roll_joint
14+
position: -0.9368171321369169
15+
- joint_name: wrist_flex_joint
16+
position: 0.6847151250403862
17+
- joint_name: wrist_roll_joint
18+
position: -1.907179170364188
19+
start_state:
20+
joint_state:
21+
header:
22+
frame_id: base_link
23+
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]
24+
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]
25+
planner_id: RRTConnectkConfigDefault
26+
group_name: arm_with_torso
27+
num_planning_attempts: 3
28+
allowed_planning_time: 10
29+
max_velocity_scaling_factor: 0
30+
max_acceleration_scaling_factor: 0
31+
workspace_parameters:
32+
min_corner: [-1, -1, -1]
33+
header:
34+
frame_id: ""
35+
max_corner: [1, 1, 1]

‎robowflex_library/yaml/fetch_scenes/request0006.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
start_state:
2+
joint_state:
3+
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]
4+
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]
5+
header:
6+
frame_id: base_link
7+
planner_id: RRTConnectkConfigDefault
8+
group_name: arm_with_torso
9+
num_planning_attempts: 3
10+
allowed_planning_time: 10
11+
max_velocity_scaling_factor: 0
12+
max_acceleration_scaling_factor: 0
13+
goal_constraints:
14+
- joint_constraints:
15+
- joint_name: torso_lift_joint
16+
position: 5.817591087453589e-08
17+
- joint_name: shoulder_pan_joint
18+
position: 0.09492901534222749
19+
- joint_name: shoulder_lift_joint
20+
position: -1.206952814771632
21+
- joint_name: upperarm_roll_joint
22+
position: -1.255509487338636
23+
- joint_name: elbow_flex_joint
24+
position: 1.726402749304374
25+
- joint_name: forearm_roll_joint
26+
position: -0.4336693715661215
27+
- joint_name: wrist_flex_joint
28+
position: -0.365113537340298
29+
- joint_name: wrist_roll_joint
30+
position: 0.7728088779874197
31+
workspace_parameters:
32+
max_corner: [1, 1, 1]
33+
header:
34+
frame_id: ""
35+
min_corner: [-1, -1, -1]

0 commit comments

Comments
 (0)