Skip to content

Commit 7ed7b64

Browse files
Breelyn Stylerbkanator
Breelyn Styler
authored and
bkanator
committed
add point cloud accessories, and demo for mpc avoiding down sample pcl with spheres
1 parent 5f3da4b commit 7ed7b64

8 files changed

+270
-125
lines changed

‎src/lab_sim/objectives/compute_posestamped_from_waypoint.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="ComputePoseStampedFromWaypoint">
3+
<!--//////////-->
4+
<BehaviorTree ID="ComputePoseStampedFromWaypoint">
5+
<Control ID="Sequence" _collapsed="false">
6+
<Action ID="RetrieveWaypoint" joint_group_name="manipulator" waypoint_joint_state="{target_joint_state}" waypoint_name="Above Pick Cube"/>
7+
<Action ID="UnpackRobotJointStateMessage" joint_state="{joint_state}" multi_dof_joint_state="{multi_dof_joint_state}" robot_joint_state="{target_joint_state}"/>
8+
<Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
9+
<Action ID="ComputeLinkPoseForwardKinematics" joint_state="{joint_state}" link_name="grasp_link" link_pose="{link_pose}" planning_scene="{planning_scene}"/>
10+
</Control>
11+
</BehaviorTree>
12+
<TreeNodesModel>
13+
<SubTree ID="ComputePoseStampedFromWaypoint">
14+
<MetadataFields>
15+
<Metadata runnable="false"/>
16+
<Metadata subcategory="Application - Basic Examples"/>
17+
</MetadataFields>
18+
<inout_port name="link_pose" default="{link_pose}" type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"/>
19+
</SubTree>
20+
</TreeNodesModel>
21+
</root>

‎src/lab_sim/objectives/create_pcl_poses_vector.xml‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Create PCL Poses Vector">
3+
<!--//////////-->
4+
<BehaviorTree ID="Create PCL Poses Vector" _favorite="false" _description="Creates a sequence of stamped poses and adds them to a vector" target_poses="{target_poses}">
5+
<Control ID="Sequence">
6+
<Action ID="CreateStampedPose" orientation_xyzw="0.000; 0.000; 0.000; 1.000" position_xyz="0.5;0.5;0.9" stamped_pose="{stamped_pose}" reference_frame="world"/>
7+
<Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
8+
<Action ID="CreateStampedPose" orientation_xyzw="0.000; 0.000; 0.000; 1.000" position_xyz="0.4;0.5;0.9" stamped_pose="{stamped_pose}" reference_frame="world"/>
9+
<Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
10+
<Action ID="CreateStampedPose" orientation_xyzw="0.000; 0.000; 0.000; 1.000" position_xyz="0.3;0.5;0.9" stamped_pose="{stamped_pose}" reference_frame="world"/>
11+
<Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
12+
<Action ID="CreateStampedPose" orientation_xyzw="0.000; 0.000; 0.000; 1.000" position_xyz="0.2;0.5;0.9" stamped_pose="{stamped_pose}" reference_frame="world"/>
13+
<Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
14+
<Action ID="CreateStampedPose" orientation_xyzw="0.000; 0.000; 0.000; 1.000" position_xyz="0.1;0.5;0.9" stamped_pose="{stamped_pose}" reference_frame="world"/>
15+
<Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
16+
<Action ID="CreateStampedPose" orientation_xyzw="0.000; 0.000; 0.000; 1.000" position_xyz="-0.1;0.5;0.9" stamped_pose="{stamped_pose}" reference_frame="world"/>
17+
<Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
18+
</Control>
19+
</BehaviorTree>
20+
<TreeNodesModel>
21+
<SubTree ID="Create PCL Poses Vector">
22+
<MetadataFields>
23+
<Metadata subcategory="Vector Handling"/>
24+
<Metadata runnable="false"/>
25+
</MetadataFields>
26+
<output_port name="target_poses" default="{target_poses}"/>
27+
</SubTree>
28+
</TreeNodesModel>
29+
</root>

‎src/lab_sim/objectives/get_pose_at_gripper_and_display.xml‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="GetPoseAtGripperandDisplay">
3+
<!--//////////-->
4+
<BehaviorTree ID="GetPoseAtGripperandDisplay" _description="Create a pose at the gripper, transform it, and display it.">
5+
<Control ID="Sequence">
6+
<Action ID="CreateStampedPose" position_xyz="0; 0; 0" stamped_pose="{stamped_pose}" reference_frame="pinch"/>
7+
<Action ID="TransformPoseFrame" input_pose="{stamped_pose}"/>
8+
<Action ID="VisualizePose" pose="{output_pose}"/>
9+
</Control>
10+
</BehaviorTree>
11+
<TreeNodesModel>
12+
<SubTree ID="GetPoseAtGripperandDisplay">
13+
<MetadataFields>
14+
<Metadata runnable="false"/>
15+
<Metadata subcategory="Pose Handling"/>
16+
</MetadataFields>
17+
<inout_port name="output_pose" default="{output_pose}" type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"/>
18+
</SubTree>
19+
</TreeNodesModel>
20+
</root>

‎src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample">
3+
<BehaviorTree ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample" _description="Example of end effector avoiding a moving point cloud that is down-sampled with spheres." _favorite="true">
4+
<Control ID="Sequence">
5+
<SubTree ID="Clear Snapshot" _collapsed="true"/>
6+
<SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Home" joint_group_name="manipulator" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"/>
7+
<SubTree ID="Create PCL Poses Vector" _collapsed="false"/>
8+
<SubTree ID="ComputePoseStampedFromWaypoint" _collapsed="false" link_pose="{link_pose}"/>
9+
<Decorator ID="ForEach" vector_in="{target_poses}" out="{stamped_pose}">
10+
<Control ID="Sequence">
11+
<Action ID="VisualizePose"/>
12+
<Action ID="BreakpointSubscriber" _skipIf="true"/>
13+
<SubTree ID="PlacePointCloudFromFileInUI" _collapsed="false" input_pose="{stamped_pose}" transform_cloud="{output_cloud}"/>
14+
<SubTree ID="GetPoseAtGripperandDisplay" _collapsed="false" output_pose="{gripper_pose}"/>
15+
<Action ID="CreateCollisionSpheresAtClosestPoints" closest_point_count="300" collision_object="{collision_object}" filter_cylinder_height="3.0" filter_cylinder_radius="3.0" point_cloud="{output_cloud}" reference_pose="{gripper_pose}" sphere_radius="0.1" debug_output_markers="true" marker_scale_xyz="{marker_scale_xyz2}" marker_poses="{marker_poses2}"/>
16+
<Action ID="CreateStampedTwist" reference_frame="world" stamped_twist="{stamped_twist}"/>
17+
<Action ID="PublishMarkers" marker_type="sphere" marker_namespace="obstacles2" marker_scale_xyz="{marker_scale_xyz2}" marker_poses="{marker_poses2}"/>
18+
<Control ID="Fallback">
19+
<Decorator ID="Timeout" msec="3000">
20+
<Control ID="Sequence">
21+
<Action ID="MPCSphereClearance" differentiable="0" follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory" gradient_num_trajectory="32" gradient_spline_points="1.000000" horizon="0.1" planning_group="manipulator" sampling_control_width="0.015000" sampling_exploration="0.050000" sampling_sample_width="0.010000" sampling_spline_points="10.000000" set_ctrl_command_to_current_state="true" timestep="0.010000" total_return="{total_return}" cartesian_acceleration="100" cartesian_acceleration_body="tool_changer" cartesian_velocity="100" cartesian_velocity_body="tool_changer" collision_sphere_object="{collision_object}" gripper_site_name="pinch" max_cartesian_acceleration_goal="500" max_cartesian_velocity_goal="0.5" mujoco_model="description/simple_scene.xml" mujoco_model_package="lab_sim" safe_distance="0.25" sphere_clearance="1.0" target_pose="{link_pose}" target_twist="{stamped_twist}" warmup_iterations="256" timeout="300" site_tracking="0.2" link_sites="attachment_site"/>
22+
</Control>
23+
</Decorator>
24+
<Action ID="AlwaysSuccess"/>
25+
</Control>
26+
</Control>
27+
</Decorator>
28+
</Control>
29+
</BehaviorTree>
30+
<TreeNodesModel>
31+
<SubTree ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample">
32+
<MetadataFields>
33+
<Metadata runnable="true"/>
34+
<Metadata subcategory="Application - MPC Examples"/>
35+
</MetadataFields>
36+
</SubTree>
37+
</TreeNodesModel>
38+
</root>

‎src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample">
3+
<BehaviorTree ID="MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample" _description="Example of end effector avoiding a point cloud that is down-sampled with spheres and statically placed in the environment." _favorite="true">
4+
<Control ID="Sequence" name="TopLevelSequence">
5+
<SubTree ID="Clear Snapshot" _collapsed="true"/>
6+
<SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Home"/>
7+
<Action ID="CreateStampedPose" position_xyz="0.0;0.5;0.8"/>
8+
<Action ID="VisualizePose"/>
9+
<SubTree ID="PlacePointCloudFromFileInUI" _collapsed="false" input_pose="{stamped_pose}" transform_cloud="{output_cloud}"/>
10+
<SubTree ID="ComputePoseStampedFromWaypoint" _collapsed="false" link_pose="{link_pose}"/>
11+
<Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="target_pose" marker_size="0.100000" pose="{link_pose}"/>
12+
<SubTree ID="GetPoseAtGripperandDisplay" _collapsed="false" output_pose="{gripper_pose}"/>
13+
<Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="sphere_pose" marker_size="0.100000" pose="{gripper_pose}"/>
14+
<Action ID="CreateCollisionSpheresAtClosestPoints" closest_point_count="300" collision_object="{collision_object}" filter_cylinder_height="3.0" filter_cylinder_radius="3.0" point_cloud="{output_cloud}" reference_pose="{gripper_pose}" sphere_radius="0.1" debug_output_markers="true" marker_scale_xyz="{marker_scale_xyz2}" marker_poses="{marker_poses2}"/>
15+
<Action ID="PublishMarkers" marker_type="sphere" marker_namespace="obstacles2" marker_scale_xyz="{marker_scale_xyz2}" marker_poses="{marker_poses2}"/>
16+
<Action ID="CreateStampedTwist" reference_frame="world" stamped_twist="{stamped_twist}"/>
17+
<Control ID="Fallback">
18+
<Decorator ID="Timeout" msec="40000">
19+
<Control ID="Sequence">
20+
<Action ID="MPCSphereClearance" differentiable="0" follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory" gradient_num_trajectory="32" gradient_spline_points="1.000000" horizon="0.1" planning_group="manipulator" sampling_control_width="0.015000" sampling_exploration="0.050000" sampling_sample_width="0.010000" sampling_spline_points="10.000000" set_ctrl_command_to_current_state="true" timestep="0.010000" total_return="{total_return}" cartesian_acceleration="100" cartesian_acceleration_body="tool_changer" cartesian_velocity="100" cartesian_velocity_body="tool_changer" collision_sphere_object="{collision_object}" gripper_site_name="pinch" max_cartesian_acceleration_goal="500" max_cartesian_velocity_goal="0.5" mujoco_model="description/simple_scene.xml" mujoco_model_package="lab_sim" safe_distance="0.25" sphere_clearance="1.0" target_pose="{link_pose}" target_twist="{stamped_twist}" warmup_iterations="256" timeout="300" site_tracking="0.2" link_sites="attachment_site"/>
21+
</Control>
22+
</Decorator>
23+
<Action ID="AlwaysSuccess"/>
24+
</Control>
25+
</Control>
26+
</BehaviorTree>
27+
<TreeNodesModel>
28+
<SubTree ID="MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample">
29+
<MetadataFields>
30+
<Metadata runnable="true"/>
31+
<Metadata subcategory="Application - MPC Examples"/>
32+
</MetadataFields>
33+
</SubTree>
34+
</TreeNodesModel>
35+
</root>

‎src/lab_sim/objectives/mpc_pose_tracking_with_point_cloud_avoidance.xml‎

Lines changed: 19 additions & 125 deletions
Original file line numberDiff line numberDiff line change
@@ -1,140 +1,34 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
2-
<root
3-
BTCPP_format="4"
4-
main_tree_to_execute="MPC Pose Tracking With Point Cloud Avoidance"
5-
>
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="MPC Pose Tracking With Point Cloud Avoidance">
63
<!--//////////-->
7-
<BehaviorTree
8-
ID="MPC Pose Tracking With Point Cloud Avoidance"
9-
_description="Uses Model Predictive Control (MPC) to track a target pose while avoiding obstacles represented by a point cloud."
10-
_favorite="false"
11-
>
4+
<BehaviorTree ID="MPC Pose Tracking With Point Cloud Avoidance" _description="Uses Model Predictive Control (MPC) to track a target pose while avoiding obstacles represented by a point cloud." _favorite="true">
125
<Control ID="Sequence" name="TopLevelSequence">
13-
<SubTree ID="Clear Snapshot" _collapsed="true" />
14-
<SubTree
15-
ID="Move to Waypoint"
16-
_collapsed="true"
17-
acceleration_scale_factor="1.0"
18-
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
19-
controller_names="joint_trajectory_controller"
20-
joint_group_name="manipulator"
21-
keep_orientation="false"
22-
keep_orientation_tolerance="0.05"
23-
link_padding="0.01"
24-
seed="0"
25-
velocity_scale_factor="1.0"
26-
waypoint_name="Look at Base"
27-
/>
28-
<Action
29-
ID="GetPointCloud"
30-
message_out="{point_cloud}"
31-
timeout_sec="5.000000"
32-
topic_name="/wrist_camera/points"
33-
/>
34-
<Action
35-
ID="CreateStampedPose"
36-
reference_frame="base_link"
37-
stamped_pose="{stamped_pose}"
38-
position_xyz="0.0;0.2;0.0"
39-
/>
40-
<Action
41-
ID="CropPointsInBox"
42-
crop_box_centroid_pose="{stamped_pose}"
43-
point_cloud="{point_cloud}"
44-
point_cloud_cropped="{point_cloud}"
45-
crop_box_size="0.2;0.2;0.2"
46-
/>
47-
<Action
48-
ID="SendPointCloudToUI"
49-
pcd_topic="/pcd_pointcloud_captures"
50-
point_cloud="{point_cloud}"
51-
/>
52-
<SubTree
53-
ID="Move to Waypoint"
54-
_collapsed="true"
55-
acceleration_scale_factor="1.0"
56-
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
57-
controller_names="joint_trajectory_controller"
58-
joint_group_name="manipulator"
59-
keep_orientation="false"
60-
keep_orientation_tolerance="0.05"
61-
link_padding="0.01"
62-
seed="0"
63-
velocity_scale_factor="1.0"
64-
waypoint_name="Home"
65-
/>
66-
<Action
67-
ID="CreateStampedPose"
68-
reference_frame="pinch"
69-
stamped_pose="{stamped_pose}"
70-
position_xyz="0;0;0.1"
71-
/>
72-
<Action
73-
ID="TransformPoseFrame"
74-
input_pose="{stamped_pose}"
75-
output_pose="{stamped_pose}"
76-
target_frame_id="world"
77-
/>
78-
<Action
79-
ID="VisualizePose"
80-
marker_lifetime="0.000000"
81-
marker_name="pose"
82-
marker_size="0.100000"
83-
pose="{stamped_pose}"
84-
/>
85-
<Action
86-
ID="CreateStampedTwist"
87-
reference_frame="world"
88-
stamped_twist="{stamped_twist}"
89-
/>
6+
<SubTree ID="Clear Snapshot" _collapsed="true"/>
7+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Look at Base"/>
8+
<Action ID="GetPointCloud" message_out="{point_cloud}" timeout_sec="5.000000" topic_name="/wrist_camera/points"/>
9+
<Action ID="CreateStampedPose" reference_frame="base_link" stamped_pose="{stamped_pose}" position_xyz="0.0;0.2;0.0"/>
10+
<Action ID="CropPointsInBox" crop_box_centroid_pose="{stamped_pose}" point_cloud="{point_cloud}" point_cloud_cropped="{point_cloud}" crop_box_size="0.2;0.2;0.2"/>
11+
<Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{point_cloud}"/>
12+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Home"/>
13+
<Action ID="CreateStampedPose" reference_frame="pinch" stamped_pose="{stamped_pose}" position_xyz="0;0.3;0.1"/>
14+
<Action ID="TransformPoseFrame" input_pose="{stamped_pose}" output_pose="{stamped_pose}" target_frame_id="world"/>
15+
<Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
16+
<Action ID="CreateStampedTwist" reference_frame="world" stamped_twist="{stamped_twist}"/>
9017
<Control ID="Fallback">
91-
<Decorator ID="Timeout" msec="5000">
18+
<Decorator ID="Timeout" msec="7000">
9219
<Control ID="Sequence">
93-
<Action
94-
ID="MPCPointCloudClearance"
95-
differentiable="0"
96-
follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory"
97-
gradient_num_trajectory="32"
98-
gradient_spline_points="1.000000"
99-
horizon="0.1"
100-
planning_group="manipulator"
101-
sampling_control_width="0.015000"
102-
sampling_exploration="0.050000"
103-
sampling_sample_width="0.010000"
104-
sampling_spline_points="10.000000"
105-
set_ctrl_command_to_current_state="true"
106-
timeout="300"
107-
timestep="0.010000"
108-
total_return="{total_return}"
109-
warmup_iterations="256"
110-
cartesian_acceleration="100"
111-
cartesian_acceleration_body="tool_changer"
112-
cartesian_velocity="100"
113-
cartesian_velocity_body="tool_changer"
114-
gripper_site_name="pinch"
115-
link_sites="base_site"
116-
max_cartesian_acceleration_goal="500"
117-
max_cartesian_velocity_goal="0.5"
118-
mujoco_model="description/simple_scene.xml"
119-
mujoco_model_package="lab_sim"
120-
point_cloud="{point_cloud}"
121-
point_cloud_clearance="1.0"
122-
safe_distance="0.25"
123-
site_tracking="1.0"
124-
target_pose="{stamped_pose}"
125-
target_twist="{stamped_twist}"
126-
/>
20+
<Action ID="MPCPointCloudClearance" differentiable="0" follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory" gradient_num_trajectory="32" gradient_spline_points="1.000000" horizon="0.1" planning_group="manipulator" sampling_control_width="0.015000" sampling_exploration="0.050000" sampling_sample_width="0.010000" sampling_spline_points="10.000000" set_ctrl_command_to_current_state="true" timeout="300" timestep="0.010000" total_return="{total_return}" warmup_iterations="256" cartesian_acceleration="100" cartesian_acceleration_body="tool_changer" cartesian_velocity="100" cartesian_velocity_body="tool_changer" gripper_site_name="pinch" link_sites="base_site" max_cartesian_acceleration_goal="500" max_cartesian_velocity_goal="0.5" mujoco_model="description/simple_scene.xml" mujoco_model_package="lab_sim" point_cloud="{point_cloud}" point_cloud_clearance="1.1" safe_distance="0.25" target_pose="{stamped_pose}" target_twist="{stamped_twist}" site_tracking="0.45"/>
12721
</Control>
12822
</Decorator>
129-
<Action ID="AlwaysSuccess" />
23+
<Action ID="AlwaysSuccess"/>
13024
</Control>
13125
</Control>
13226
</BehaviorTree>
13327
<TreeNodesModel>
13428
<SubTree ID="MPC Pose Tracking With Point Cloud Avoidance">
13529
<MetadataFields>
136-
<Metadata runnable="true" />
137-
<Metadata subcategory="Application - MPC Examples" />
30+
<Metadata runnable="true"/>
31+
<Metadata subcategory="Application - MPC Examples"/>
13832
</MetadataFields>
13933
</SubTree>
14034
</TreeNodesModel>

0 commit comments

Comments
 (0)