Skip to content

Commit 7c7b1b6

Browse files
mkhansenbotmkhansenbot
authored
Bring it add objectives (#59)
Adds objectives for moving close to the bar and picking up beer. Picking currently fails to plan a solution
1 parent ffa191d commit 7c7b1b6

File tree

8 files changed

+176
-21
lines changed

8 files changed

+176
-21
lines changed

‎src/moveit_pro_ur_configs/bring_it_sim/description/bar.xml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@
6666
friction="0.8 0.2"
6767
/>
6868
</body>
69-
<body name="beer_bottle_3" pos="0 2 0.9">
69+
<body name="beer_bottle_3" pos="0 1.5 0.9">
7070
<freejoint />
7171
<site name="beer_bottle_3" />
7272
<geom
@@ -77,7 +77,7 @@
7777
friction="0.8 0.2"
7878
/>
7979
</body>
80-
<body name="beer_bottle_4" pos="0.5 2 0.9">
80+
<body name="beer_bottle_4" pos="0.5 1.5 0.9">
8181
<freejoint />
8282
<site name="beer_bottle_4" />
8383
<geom

‎src/moveit_pro_ur_configs/bring_it_sim/description/picknik_ur_mujoco_ros2_control.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@
8888
<param name="render_publish_rate">60</param>
8989
<param name="tf_publish_rate">60</param>
9090
<param name="lidar_publish_rate">10</param>
91-
<param name="ignore_joint_validation">linear_x_joint linear_y_joint rotational_yaw_joint world_bar world_beer_bottle_1 world_beer_bottle_2 world_beer_bottle_3 world_beer_bottle_4 world_beer_bottle_5 world_table_1 world_table_2 world_table_3 world_table_4</param>
91+
<param name="ignore_joint_validation">linear_x_joint linear_y_joint rotational_yaw_joint</param>
9292
<param name="publish_odom">true</param>
9393
<param name="odom_child_frame">ridgeback_base_link</param>
9494
<param name="odom_zero_z">true</param>

‎src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xacro‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,6 @@
2424
<xacro:include filename="$(find bring_it_sim)/description/picknik_ur_attachments_macro.xacro"/>
2525
<xacro:include filename="$(find bring_it_sim)/description/picknik_ur_mujoco_ros2_control.xacro"/>
2626
<xacro:include filename="$(find ridgeback_description)/urdf/ridgeback.urdf.xacro"/>
27-
<xacro:include filename="$(find bring_it_sim)/description/bar.xacro"/>
2827

2928
<link name="world"/>
3029

‎src/moveit_pro_ur_configs/bring_it_sim/objectives/move_close_to_bar.xml‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Move close to bar">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Move close to bar"
6+
_description="Move close enough to the bar to pick up a beer"
7+
_favorite="true"
8+
>
9+
<Control ID="Sequence">
10+
<Action
11+
ID="CreateStampedPose"
12+
orientation_xyzw="0;0;0;1"
13+
position_xyz="0;1;0"
14+
stamped_pose="{home_location}"
15+
reference_frame="world"
16+
/>
17+
<Action
18+
ID="VisualizeMesh"
19+
mesh_path="package://bring_it_sim/description/assets/locationmarker.dae"
20+
mesh_pose="{home_location}"
21+
/>
22+
<Action
23+
ID="NavigateToPoseAction"
24+
pose_stamped="{home_location}"
25+
behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
26+
action_name="/navigate_to_pose"
27+
/>
28+
</Control>
29+
</BehaviorTree>
30+
<TreeNodesModel>
31+
<SubTree ID="Move close to bar" />
32+
</TreeNodesModel>
33+
</root>

‎src/moveit_pro_ur_configs/bring_it_sim/objectives/pick_beer.xml‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Pick beer">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Pick beer"
6+
_description=""
7+
_favorite="false"
8+
_subtreeOnly="true"
9+
name=""
10+
>
11+
<Control ID="Sequence" name="root">
12+
<SubTree ID="Open Gripper" />
13+
<Control ID="Sequence">
14+
<Action
15+
ID="CreateStampedPose"
16+
orientation_xyzw="1;0;0;0"
17+
position_xyz="0;0;0.22"
18+
reference_frame="{beer_frame}"
19+
/>
20+
<Action ID="VisualizePose" />
21+
<Action
22+
ID="AddPoseStampedToVector"
23+
input="{stamped_pose}"
24+
vector="{pose_vector}"
25+
/>
26+
</Control>
27+
<SubTree
28+
ID="Pick first object in vector"
29+
pose_vector="{pose_vector}"
30+
_collapsed="false"
31+
/>
32+
</Control>
33+
</BehaviorTree>
34+
<TreeNodesModel>
35+
<SubTree ID="Pick beer">
36+
<input_port name="beer_frame" default="{frame_id}" />
37+
</SubTree>
38+
</TreeNodesModel>
39+
</root>

‎src/moveit_pro_ur_configs/bring_it_sim/objectives/pick_up_beer.xml‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Pick up Beer">
3+
<!--//////////-->
4+
<BehaviorTree ID="Pick up Beer" _description="" _favorite="true">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Clear Snapshot" _collapsed="true" />
7+
<SubTree ID="Look at table" _collapsed="true" />
8+
<!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
9+
<SubTree ID="Take scene camera snapshot" _collapsed="true" />
10+
<SubTree ID="Pick beer" _collapsed="true" beer_frame="{frame_id}" />
11+
</Control>
12+
</BehaviorTree>
13+
<TreeNodesModel>
14+
<SubTree ID="Pick up Beer" />
15+
</TreeNodesModel>
16+
</root>

‎src/moveit_pro_ur_configs/bring_it_sim/params/nav2_params.yaml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -199,7 +199,7 @@ local_costmap:
199199
inflation_layer:
200200
plugin: "nav2_costmap_2d::InflationLayer"
201201
cost_scaling_factor: 3.0
202-
inflation_radius: 0.55
202+
inflation_radius: 0.20
203203
voxel_layer:
204204
plugin: "nav2_costmap_2d::VoxelLayer"
205205
enabled: True
@@ -264,7 +264,7 @@ global_costmap:
264264
inflation_layer:
265265
plugin: "nav2_costmap_2d::InflationLayer"
266266
cost_scaling_factor: 3.0
267-
inflation_radius: 0.55
267+
inflation_radius: 0.20
268268
always_send_full_costmap: True
269269

270270
map_server:

‎src/moveit_pro_ur_configs/bring_it_sim/waypoints/ur_waypoints.yaml‎

Lines changed: 83 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,48 +1,69 @@
1-
- description: ''
1+
- description: ""
22
favorite: true
33
joint_group_names:
4+
- gripper
5+
- linear_actuator
46
- manipulator
7+
- wheel_actuator
58
joint_state:
69
effort: []
710
header:
8-
frame_id: ''
11+
frame_id: ""
912
stamp:
1013
nanosec: 0
1114
sec: 0
1215
name:
16+
- robotiq_85_left_knuckle_joint
17+
- linear_x_joint
18+
- linear_y_joint
19+
- rotational_yaw_joint
1320
- shoulder_pan_joint
21+
- shoulder_lift_joint
1422
- elbow_joint
23+
- wrist_1_joint
1524
- wrist_2_joint
1625
- wrist_3_joint
17-
- wrist_1_joint
18-
- shoulder_lift_joint
26+
- front_rocker
27+
- front_left_wheel
28+
- front_right_wheel
29+
- rear_left_wheel
30+
- rear_right_wheel
1931
position:
20-
- -8.704708810562631e-06
21-
- 1.4068654234675446
22-
- 1.56799762591031
23-
- 8.888612690481538e-10
24-
- 0.782482319413393
25-
- -1.2156111214535257
32+
- 0.1025781249307494
33+
- 0
34+
- 0
35+
- 0
36+
- 0.8371631576820436
37+
- -0.3352390746492166
38+
- 0.19765088507041076
39+
- -2.3182411494283217
40+
- -1.4856134479671956
41+
- -0.059141607883296456
42+
- 6.680871677833013e-05
43+
- -5.2400322737957
44+
- 5.2356089836249575
45+
- -5.238369713479739
46+
- 5.233722234090497
2647
velocity: []
2748
multi_dof_joint_state:
2849
header:
29-
frame_id: ''
50+
frame_id: ""
3051
stamp:
3152
nanosec: 0
3253
sec: 0
3354
joint_names: []
3455
transforms: []
3556
twist: []
3657
wrench: []
37-
name: Look at Table
38-
- description: ''
58+
name: Near bottle 3
59+
- description: ""
3960
favorite: true
4061
joint_group_names:
4162
- manipulator
4263
joint_state:
4364
effort: []
4465
header:
45-
frame_id: ''
66+
frame_id: ""
4667
stamp:
4768
nanosec: 0
4869
sec: 0
@@ -63,7 +84,7 @@
6384
velocity: []
6485
multi_dof_joint_state:
6586
header:
66-
frame_id: ''
87+
frame_id: ""
6788
stamp:
6889
nanosec: 0
6990
sec: 0
@@ -72,3 +93,50 @@
7293
twist: []
7394
wrench: []
7495
name: Stowed
96+
- description: ""
97+
favorite: true
98+
joint_group_names:
99+
- gripper
100+
- linear_actuator
101+
- manipulator
102+
joint_state:
103+
effort: []
104+
header:
105+
frame_id: ""
106+
stamp:
107+
nanosec: 0
108+
sec: 0
109+
name:
110+
- robotiq_85_left_knuckle_joint
111+
- linear_x_joint
112+
- linear_y_joint
113+
- rotational_yaw_joint
114+
- shoulder_pan_joint
115+
- shoulder_lift_joint
116+
- elbow_joint
117+
- wrist_1_joint
118+
- wrist_2_joint
119+
- wrist_3_joint
120+
position:
121+
- 0
122+
- 0.4
123+
- 0.4
124+
- 1
125+
- 0
126+
- -1.5708
127+
- 0.785398
128+
- -1.2948195437205516
129+
- -1.5708
130+
- 0
131+
velocity: []
132+
multi_dof_joint_state:
133+
header:
134+
frame_id: ""
135+
stamp:
136+
nanosec: 0
137+
sec: 0
138+
joint_names: []
139+
transforms: []
140+
twist: []
141+
wrench: []
142+
name: Look at Table

0 commit comments

Comments
 (0)