Skip to content

Commit bcaf2c7

Browse files
MikeWrockMikeWrock
committed
Squashed 'src/lab_sim/' changes from 2ee0461a..e620d305
e620d305 Merge pull request #2 from PickNikRobotics/subtree-sync 62008ced Sam naming (#38) 5690add5 Sync 6.5 to main (#37) ee7dd7dd Add example SAM 2 behavior and objective (#23) 4c700980 Sync V6.5 to main (#29) e8f315ed added descriptions 86a6350b Updating objectives ca65f4df formatting e01d58aa more fixes c0f151e9 Merge branch 'main' into pr-update-to-make-ports-explicit 3c5a9af8 Fix pick ApriTag object aafc6e5c Update ports explicitly 6987b5bc version bump 9375a38b Updating xacro and favourites 51e72413 formatting 8eea418b formatting 9dcf6b86 Moving registration behaviors cfeeac7d Add pick and place example using custom MTC example behavior d9bc48d4 Added mtc to jtc converter git-subtree-dir: src/lab_sim git-subtree-split: e620d305b8a75b8880a8734c3b40cc88779c0d55
1 parent 36da3ca commit bcaf2c7

37 files changed

+41427
-177
lines changed

‎config/config.yaml‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,6 @@ objectives:
4242
- "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
4343
- "moveit_studio::behaviors::ServoBehaviorsLoader"
4444
- "moveit_studio::behaviors::VisionBehaviorsLoader"
45-
- "picknik_registration::PicknikRegistrationBehaviorsLoader"
4645
# Specify source folder for objectives
4746
# [Required]
4847
objective_library_paths:
@@ -71,9 +70,9 @@ ros2_control:
7170
- "robotiq_gripper_controller"
7271
- "joint_state_broadcaster"
7372
- "servo_controller"
73+
- "joint_trajectory_controller"
7474
# Load but do not start these controllers so they can be activated later if needed.
7575
controllers_inactive_at_startup:
76-
- "joint_trajectory_controller"
7776
- "joint_trajectory_admittance_controller"
7877
- "velocity_force_controller"
7978
# Any controllers here will not be spawned by MoveIt Pro.

‎description/picknik_ur.xacro‎

Lines changed: 7 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,9 @@
77
<material name="plastic">
88
<color rgba="0.1 0.1 0.1 1" />
99
</material>
10+
<material name="offwhite">
11+
<color rgba="0.01 0.01 0.01 1" />
12+
</material>
1013
<link name="world" />
1114
<joint name="tool_changer_joint" type="fixed">
1215
<!-- The parent link must be read from the robot model it is attached to. -->
@@ -1174,57 +1177,15 @@
11741177
<!-- Table -->
11751178
<link name="table">
11761179
<visual>
1177-
<origin rpy="0 0 0" xyz="0 0 0.025" />
1178-
<geometry>
1179-
<mesh
1180-
filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae"
1181-
scale="0.125 0.125 0.00625"
1182-
/>
1183-
</geometry>
1184-
</visual>
1185-
<visual>
1186-
<origin rpy="0 0 0" xyz="0.4 0.4 -0.2" />
1187-
<geometry>
1188-
<box size="0.04 0.04 0.45" />
1189-
</geometry>
1190-
<material name="grey">
1191-
<color rgba="0.1 0.1 0.1 1.0" />
1192-
</material>
1193-
</visual>
1194-
<visual>
1195-
<origin rpy="0 0 0" xyz="0.4 -0.4 -0.2" />
1196-
<geometry>
1197-
<box size="0.04 0.04 0.45" />
1198-
</geometry>
1199-
<material name="grey">
1200-
<color rgba="0.1 0.1 0.1 1.0" />
1201-
</material>
1202-
</visual>
1203-
<visual>
1204-
<origin rpy="0 0 0" xyz="-0.4 -0.4 -0.2" />
1180+
<origin rpy="0 0 0" xyz="0 0 -0.45" />
12051181
<geometry>
1206-
<box size="0.04 0.04 0.45" />
1182+
<box size="3.5 1 0.9" />
12071183
</geometry>
1208-
<material name="grey">
1209-
<color rgba="0.1 0.1 0.1 1.0" />
1210-
</material>
1211-
</visual>
1212-
<visual>
1213-
<origin rpy="0 0 0" xyz="-0.4 0.4 -0.2" />
1214-
<geometry>
1215-
<box size="0.04 0.04 0.45" />
1216-
</geometry>
1217-
<material name="grey">
1218-
<color rgba="0.1 0.1 0.1 1.0" />
1219-
</material>
12201184
</visual>
12211185
<collision>
1222-
<origin rpy="0 0 0" xyz="0 0 0" />
1186+
<origin rpy="0 0 0" xyz="0 0 -0.45" />
12231187
<geometry>
1224-
<mesh
1225-
filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae"
1226-
scale="0.125 0.125 0.00625"
1227-
/>
1188+
<box size="3.5 1 0.9" />
12281189
</geometry>
12291190
</collision>
12301191
</link>

‎objectives/3_waypoint_pick_and_place.xml‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<BehaviorTree
44
ID="3 Waypoints Pick and Place"
55
_description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position"
6-
_favorite="true"
6+
_favorite="false"
77
>
88
<Control ID="Sequence">
99
<!--Reset the planning scene to ensure no old collision objects are hanging around-->
@@ -119,4 +119,11 @@
119119
</Decorator>
120120
</Control>
121121
</BehaviorTree>
122+
<TreeNodesModel>
123+
<SubTree ID="3 Waypoints Pick and Place">
124+
<MetadataFields>
125+
<Metadata subcategory="Application - Basic Examples" />
126+
</MetadataFields>
127+
</SubTree>
128+
</TreeNodesModel>
122129
</root>

‎objectives/add_waypoints_to_mtc_task.xml‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Add Waypoints to MTC Task">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Add Waypoints to MTC Task"
6+
_subtreeOnly="true"
7+
_description=""
8+
>
9+
<Control ID="Sequence">
10+
<Action
11+
ID="RetrieveWaypoint"
12+
waypoint_joint_state="{target_joint_state}"
13+
waypoint_name="Above Pick Cube"
14+
joint_group_name="{joint_group_name}"
15+
/>
16+
<Action
17+
ID="SetupMTCMoveToJointState"
18+
joint_state="{target_joint_state}"
19+
name="SetupMTCMoveToJointState"
20+
planning_group_name="{joint_group_name}"
21+
planner_interface="{planner_interface}"
22+
task="{mtc_task}"
23+
/>
24+
<Action
25+
ID="RetrieveWaypoint"
26+
waypoint_joint_state="{target_joint_state}"
27+
waypoint_name="Above Place Cube"
28+
joint_group_name="{joint_group_name}"
29+
/>
30+
<Action
31+
ID="SetupMTCMoveToJointState"
32+
joint_state="{target_joint_state}"
33+
name="SetupMTCMoveToJointState"
34+
planning_group_name="{joint_group_name}"
35+
planner_interface="{planner_interface}"
36+
task="{mtc_task}"
37+
/>
38+
</Control>
39+
</BehaviorTree>
40+
<TreeNodesModel>
41+
<SubTree ID="Add Waypoints to MTC Task">
42+
<input_port name="joint_group_name" default="manipulator" />
43+
<inout_port name="mtc_task" default="{mtc_task}" />
44+
<input_port name="planner_interface" default="moveit_default" />
45+
<MetadataFields>
46+
<Metadata subcategory="Application - Basic Examples" />
47+
</MetadataFields>
48+
</SubTree>
49+
</TreeNodesModel>
50+
</root>

‎objectives/apriltag_detection_config.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ DetectAprilTags:
33
apriltag_size: 0.04
44
max_hamming: 0
55
n_threads: 1
6-
quad_decimate: 1
7-
quad_sigma: 0.1
8-
refine_edges: false
6+
quad_decimate: 2
7+
quad_sigma: 0.0
8+
refine_edges: true
99
z_up: false

‎objectives/apriltag_pick_object.xml‎

Lines changed: 33 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,3 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
21
<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
32
<!--//////////-->
43
<BehaviorTree
@@ -9,7 +8,7 @@
98
<Control ID="Sequence" name="TopLevelSequence">
109
<SubTree ID="Look at table" _collapsed="true" />
1110
<Action ID="ClearSnapshot" />
12-
<SubTree ID="Wrist camera snapshot" _collapsed="true" />
11+
<SubTree ID="Take wrist camera snapshot" _collapsed="true" />
1312
<Control ID="Sequence" name="Setup">
1413
<SubTree ID="Open Gripper" />
1514
</Control>
@@ -28,19 +27,39 @@
2827
topic_name="/wrist_camera/color"
2928
message_out="{image}"
3029
/>
30+
<SubTree
31+
ID="Get AprilTag Pose from Image"
32+
image="{image}"
33+
camera_info="{camera_info}"
34+
parameters="{parameters}"
35+
_collapsed="true"
36+
detection_pose="{detection_pose}"
37+
/>
3138
<Action
32-
ID="DetectAprilTags"
39+
ID="TransformPose"
40+
output_pose="{view_pose}"
41+
input_pose="{detection_pose}"
42+
quaternion_xyzw="0;0;0;1"
43+
translation_xyz="0;-0.1;-0.15"
44+
/>
45+
<SubTree
46+
ID="Plan Move To Pose"
47+
target_pose="{view_pose}"
48+
move_to_pose_solution="{move_to_pose_solution}"
49+
/>
50+
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
51+
<SubTree
52+
ID="Get AprilTag Pose from Image"
3353
image="{image}"
3454
camera_info="{camera_info}"
3555
parameters="{parameters}"
36-
detections="{detections}"
56+
_collapsed="true"
57+
detection_pose="{detection_pose}"
3758
/>
38-
<Action ID="GetDetectionPose" />
39-
<Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
4059
<Action
4160
ID="TransformPose"
4261
output_pose="{offset_pose}"
43-
input_pose="{output_pose}"
62+
input_pose="{detection_pose}"
4463
quaternion_xyzw="0;0;0;1"
4564
translation_xyz="0;0;-0.01"
4665
/>
@@ -69,4 +88,11 @@
6988
<Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
7089
</Control>
7190
</BehaviorTree>
91+
<TreeNodesModel>
92+
<SubTree ID="Pick April Tag Labeled Object">
93+
<MetadataFields>
94+
<Metadata subcategory="Application - Advanced Examples" />
95+
</MetadataFields>
96+
</SubTree>
97+
</TreeNodesModel>
7298
</root>

‎objectives/constrained_pick_place.xml‎

Lines changed: 15 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<BehaviorTree
44
ID="Constrained Pick and Place"
55
_description="More advanced example of picking and placing an (invisible) object with orientation constraints enforced on the gripper's motion"
6-
_favorite="true"
6+
_favorite="false"
77
>
88
<Control ID="Sequence">
99
<SubTree
@@ -51,8 +51,10 @@
5151
constraints="{constraints}"
5252
/>
5353
</Decorator>
54-
<!--Pick-->
55-
<SubTree ID="Close Gripper" />
54+
<!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
55+
<Decorator ID="ForceSuccess">
56+
<SubTree ID="Close Gripper" />
57+
</Decorator>
5658
<!--Move to place (drop) location-->
5759
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
5860
<SubTree
@@ -110,7 +112,9 @@
110112
waypoint_name="Place Cube"
111113
/>
112114
</Decorator>
113-
<SubTree ID="Close Gripper" />
115+
<Decorator ID="ForceSuccess">
116+
<SubTree ID="Close Gripper" />
117+
</Decorator>
114118
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
115119
<SubTree
116120
ID="Move to Waypoint"
@@ -148,4 +152,11 @@
148152
</Decorator>
149153
</Control>
150154
</BehaviorTree>
155+
<TreeNodesModel>
156+
<SubTree ID="Constrained Pick and Place">
157+
<MetadataFields>
158+
<Metadata subcategory="Application - Advanced Examples" />
159+
</MetadataFields>
160+
</SubTree>
161+
</TreeNodesModel>
151162
</root>

0 commit comments

Comments
 (0)