Skip to content

Clean up and improve lab_sim objectives #526

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
davetcoleman merged 1 commit into from
Mar 12, 2026
Merged

Clean up and improve lab_sim objectives #526

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/lab_sim/config/control/picknik_ur.ros2_control.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ robotiq_gripper_controller:
joint: robotiq_85_left_knuckle_joint
allow_stalling: true
stall_timeout: 0.05
stall_velocity_threshold: 0.004
goal_tolerance: 0.02

force_torque_sensor_broadcaster:
Expand Down
34 changes: 0 additions & 34 deletions src/lab_sim/objectives/_scan_scene.xml
View file
Open in desktop

This file was deleted.

10 changes: 7 additions & 3 deletions ...ctives/compute_linkpose_from_waypoint.xml → ...bjectives/computelinkposefromwaypoint.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="ComputeLinkPoseFromWaypoint">
<!--//////////-->
<BehaviorTree ID="ComputeLinkPoseFromWaypoint" _favorite="false">
<BehaviorTree
ID="ComputeLinkPoseFromWaypoint"
_favorite="false"
_description=""
>
<Control ID="Sequence" _collapsed="false">
<Action
ID="RetrieveWaypoint"
joint_group_name="manipulator"
waypoint_joint_state="{target_joint_state}"
waypoint_name="Above Pick Cube"
waypoint_name="Above Pick Object"
/>
<Action
ID="UnpackRobotJointStateMessage"
Expand All @@ -32,7 +36,7 @@
<SubTree ID="ComputeLinkPoseFromWaypoint">
<MetadataFields>
<Metadata runnable="false" />
<Metadata subcategory="Application - Basic Examples" />
<Metadata subcategory="Manipulation" />
<Metadata
description="This objective retrieves a named waypoint, unpacks the joint state, gets the current planning scene, and computes the pose of a specified link using forward kinematics. The resulting pose is output as a geometry_msgs::msg::PoseStamped."
/>
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/grasp_planning.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Clear Snapshot" _collapsed="true" />
<SubTree ID="Look at Table" _collapsed="true" />
<!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
<!--Take a snapshot so we have a visualization of the objects to use in grasp pose correction-->
<SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
<SubTree ID="Open Gripper" _collapsed="true" />
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/mpc_pose_tracking.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
ID="Move to Waypoint"
_collapsed="true"
joint_group_name="manipulator"
waypoint_name="Above Pick Cube"
waypoint_name="Above Pick Object"
/>
<Action
ID="CreateStampedPose"
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Clear Snapshot" _collapsed="true" />
<SubTree ID="_Scan Scene" _collapsed="true" />
<SubTree ID="Scan Scene - Multiple Point Clouds" _collapsed="true" />
<Action
ID="GetPointCloud"
message_out="{point_cloud}"
Expand Down
6 changes: 3 additions & 3 deletions src/lab_sim/objectives/open_gripper.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,19 @@
<?xml version="1.0" ?>
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Open Gripper">
<!--//////////-->
<BehaviorTree
ID="Open Gripper"
_description="Open the gripper"
_favorite="false"
>
<Control ID="Sequence" name="root">
<Decorator ID="ForceSuccess">
<Action
ID="MoveGripperAction"
gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
position="0.05"
timeout="15.000000"
/>
</Control>
</Decorator>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Open Gripper">
Expand Down
44 changes: 0 additions & 44 deletions src/lab_sim/objectives/pick_and_place_example.xml
View file
Open in desktop

This file was deleted.

2 changes: 1 addition & 1 deletion src/lab_sim/objectives/pick_from_pose.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@
<TreeNodesModel>
<SubTree ID="Pick from Pose">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata subcategory="Manipulation" />
<Metadata runnable="false" />
</MetadataFields>
<inout_port name="approach_distance" default="0.1" />
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/pick_from_pose_with_approval.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@
<TreeNodesModel>
<SubTree ID="Pick from Pose With Approval">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata subcategory="Manipulation" />
<Metadata runnable="false" />
</MetadataFields>
<inout_port name="approach_distance" default="0.1" />
Expand Down
10 changes: 5 additions & 5 deletions src/lab_sim/objectives/pick_up_cube.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,21 +1,21 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
<root BTCPP_format="4" main_tree_to_execute="Pick up Object">
<!--//////////-->
<BehaviorTree
ID="Pick up Cube"
_description="Pick up a cube from a manually defined location"
ID="Pick up Object"
_description="Pick up an object from a manually defined location"
_favorite="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Clear Snapshot" _collapsed="true" />
<SubTree ID="Look at Table" _collapsed="false" />
<!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
<!--Take a snapshot so we have a visualization of the objects to use in grasp pose correction-->
<SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
<SubTree ID="Pick object" _collapsed="false" />
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Pick up Cube">
<SubTree ID="Pick up Object">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata runnable="true" />
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/place_at_pose.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@
<TreeNodesModel>
<SubTree ID="Place at Pose">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata subcategory="Manipulation" />
<Metadata runnable="false" />
</MetadataFields>
<inout_port name="approach_distance" default="0.1" />
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/place_at_pose_with_approval.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@
<TreeNodesModel>
<SubTree ID="Place at Pose with Approval">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata subcategory="Manipulation" />
<Metadata runnable="false" />
</MetadataFields>
<inout_port name="place_poses" default="{place_poses}" />
Expand Down
26 changes: 0 additions & 26 deletions src/lab_sim/objectives/scan_multiple_views.xml
View file
Open in desktop

This file was deleted.

121 changes: 121 additions & 0 deletions src/lab_sim/objectives/scan_scene_-_multiple_point_clouds.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,121 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root
BTCPP_format="4"
main_tree_to_execute="Scan Scene - Multiple Point Clouds"
>
<!--//////////-->
<BehaviorTree
ID="Scan Scene - Multiple Point Clouds"
_description="Take multiple snapshots from the wrist camera at 3 positions (center, left, right) using Cartesian offsets and merge them into a single point cloud."
_favorite="true"
>
<Control ID="Sequence">
<!--Clear previous data-->
<SubTree ID="Clear Snapshot" _collapsed="true" />
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
<!--Move to center scan position and capture-->
<SubTree
ID="Move to Waypoint"
_collapsed="true"
joint_group_name="manipulator"
waypoint_name="Look at Table"
/>
<SubTree
ID="Add Point Cloud to Vector"
_collapsed="true"
point_cloud_vector="{point_cloud_vector}"
topic_name="/wrist_camera/points"
/>
<!--Cartesian move right from center and capture-->
<Action ID="ResetPoseStampedVector" vector="{cartesian_path}" />
<Action
ID="CreateStampedPose"
reference_frame="grasp_link"
stamped_pose="{offset_pose}"
position_xyz="0.80;0;0"
/>
<Action
ID="AddPoseStampedToVector"
input="{offset_pose}"
vector="{cartesian_path}"
/>
<Action
ID="PlanCartesianPath"
acceleration_scale_factor="1.000000"
blending_radius="0.020000"
debug_solution="{debug_solution}"
ik_cartesian_space_density="0.010000"
ik_joint_space_density="0.100000"
joint_trajectory_msg="{joint_trajectory_msg}"
path="{cartesian_path}"
planning_group_name="manipulator"
position_only="false"
tip_links="grasp_link"
trajectory_sampling_rate="100"
velocity_scale_factor="1.000000"
/>
<Action ID="ExecuteTrajectory" />
<SubTree
ID="Add Point Cloud to Vector"
_collapsed="true"
point_cloud_vector="{point_cloud_vector}"
topic_name="/wrist_camera/points"
/>
<!--Cartesian move from right directly to left and capture-->
<Action ID="ResetPoseStampedVector" vector="{cartesian_path}" />
<Action
ID="CreateStampedPose"
reference_frame="grasp_link"
stamped_pose="{offset_pose}"
position_xyz="-1.60;0;0"
/>
<Action
ID="AddPoseStampedToVector"
input="{offset_pose}"
vector="{cartesian_path}"
/>
<Action
ID="PlanCartesianPath"
acceleration_scale_factor="1.000000"
blending_radius="0.020000"
debug_solution="{debug_solution}"
ik_cartesian_space_density="0.010000"
ik_joint_space_density="0.100000"
joint_trajectory_msg="{joint_trajectory_msg}"
path="{cartesian_path}"
planning_group_name="manipulator"
position_only="false"
tip_links="grasp_link"
trajectory_sampling_rate="100"
velocity_scale_factor="1.000000"
/>
<Action ID="ExecuteTrajectory" />
<SubTree
ID="Add Point Cloud to Vector"
_collapsed="true"
point_cloud_vector="{point_cloud_vector}"
topic_name="/wrist_camera/points"
/>
<!--Merge all point clouds and display-->
<Action
ID="MergePointClouds"
grid_resolution_meters="0.010000"
merged_cloud="{merged_cloud}"
point_clouds="{point_cloud_vector}"
/>
<Action
ID="SendPointCloudToUI"
point_cloud="{merged_cloud}"
pcd_topic="/pcd_pointcloud_captures"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Scan Scene - Multiple Point Clouds">
<MetadataFields>
<Metadata subcategory="Perception - 3D Point Cloud" />
<Metadata runnable="true" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
6 changes: 3 additions & 3 deletions src/lab_sim/objectives/stack_blocks_with_icp.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Stack Blocks with ICP">
<root BTCPP_format="4" main_tree_to_execute="Stack Objects with ICP">
<!--//////////-->
<BehaviorTree
ID="Stack Blocks with ICP"
ID="Stack Objects with ICP"
_description="Solution to the advanced exercise in the ICP section of the Perception &amp; ML tutorial"
_favorite="false"
>
Expand All @@ -29,7 +29,7 @@
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Stack Blocks with ICP">
<SubTree ID="Stack Objects with ICP">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="Training Examples" />
Expand Down
Loading
Loading