Skip to content

Commit 8d93878

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 8d93878

8 files changed

+557
-5
lines changed

‎src/lab_sim/objectives/compute_posestamped_from_waypoint.xml‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
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
7+
ID="RetrieveWaypoint"
8+
joint_group_name="manipulator"
9+
waypoint_joint_state="{target_joint_state}"
10+
waypoint_name="Above Pick Cube"
11+
/>
12+
<Action
13+
ID="UnpackRobotJointStateMessage"
14+
joint_state="{joint_state}"
15+
multi_dof_joint_state="{multi_dof_joint_state}"
16+
robot_joint_state="{target_joint_state}"
17+
/>
18+
<Action
19+
ID="GetCurrentPlanningScene"
20+
planning_scene_msg="{planning_scene}"
21+
/>
22+
<Action
23+
ID="ComputeLinkPoseForwardKinematics"
24+
joint_state="{joint_state}"
25+
link_name="grasp_link"
26+
link_pose="{link_pose}"
27+
planning_scene="{planning_scene}"
28+
/>
29+
</Control>
30+
</BehaviorTree>
31+
<TreeNodesModel>
32+
<SubTree ID="ComputePoseStampedFromWaypoint">
33+
<MetadataFields>
34+
<Metadata runnable="false" />
35+
<Metadata subcategory="Application - Basic Examples" />
36+
</MetadataFields>
37+
<inout_port
38+
name="link_pose"
39+
default="{link_pose}"
40+
type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
41+
/>
42+
</SubTree>
43+
</TreeNodesModel>
44+
</root>

‎src/lab_sim/objectives/create_pcl_poses_vector.xml‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Create PCL Poses Vector">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Create PCL Poses Vector"
6+
_favorite="false"
7+
_description="Creates a sequence of stamped poses and adds them to a vector"
8+
target_poses="{target_poses}"
9+
>
10+
<Control ID="Sequence">
11+
<Action
12+
ID="CreateStampedPose"
13+
orientation_xyzw="0.000; 0.000; 0.000; 1.000"
14+
position_xyz="0.5;0.5;0.9"
15+
stamped_pose="{stamped_pose}"
16+
reference_frame="world"
17+
/>
18+
<Action
19+
ID="AddPoseStampedToVector"
20+
vector="{target_poses}"
21+
input="{stamped_pose}"
22+
/>
23+
<Action
24+
ID="CreateStampedPose"
25+
orientation_xyzw="0.000; 0.000; 0.000; 1.000"
26+
position_xyz="0.4;0.5;0.9"
27+
stamped_pose="{stamped_pose}"
28+
reference_frame="world"
29+
/>
30+
<Action
31+
ID="AddPoseStampedToVector"
32+
vector="{target_poses}"
33+
input="{stamped_pose}"
34+
/>
35+
<Action
36+
ID="CreateStampedPose"
37+
orientation_xyzw="0.000; 0.000; 0.000; 1.000"
38+
position_xyz="0.3;0.5;0.9"
39+
stamped_pose="{stamped_pose}"
40+
reference_frame="world"
41+
/>
42+
<Action
43+
ID="AddPoseStampedToVector"
44+
vector="{target_poses}"
45+
input="{stamped_pose}"
46+
/>
47+
<Action
48+
ID="CreateStampedPose"
49+
orientation_xyzw="0.000; 0.000; 0.000; 1.000"
50+
position_xyz="0.2;0.5;0.9"
51+
stamped_pose="{stamped_pose}"
52+
reference_frame="world"
53+
/>
54+
<Action
55+
ID="AddPoseStampedToVector"
56+
vector="{target_poses}"
57+
input="{stamped_pose}"
58+
/>
59+
<Action
60+
ID="CreateStampedPose"
61+
orientation_xyzw="0.000; 0.000; 0.000; 1.000"
62+
position_xyz="0.1;0.5;0.9"
63+
stamped_pose="{stamped_pose}"
64+
reference_frame="world"
65+
/>
66+
<Action
67+
ID="AddPoseStampedToVector"
68+
vector="{target_poses}"
69+
input="{stamped_pose}"
70+
/>
71+
<Action
72+
ID="CreateStampedPose"
73+
orientation_xyzw="0.000; 0.000; 0.000; 1.000"
74+
position_xyz="-0.1;0.5;0.9"
75+
stamped_pose="{stamped_pose}"
76+
reference_frame="world"
77+
/>
78+
<Action
79+
ID="AddPoseStampedToVector"
80+
vector="{target_poses}"
81+
input="{stamped_pose}"
82+
/>
83+
</Control>
84+
</BehaviorTree>
85+
<TreeNodesModel>
86+
<SubTree ID="Create PCL Poses Vector">
87+
<MetadataFields>
88+
<Metadata subcategory="Vector Handling" />
89+
<Metadata runnable="false" />
90+
</MetadataFields>
91+
<output_port name="target_poses" default="{target_poses}" />
92+
</SubTree>
93+
</TreeNodesModel>
94+
</root>

‎src/lab_sim/objectives/get_pose_at_gripper_and_display.xml‎

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

‎src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root
3+
BTCPP_format="4"
4+
main_tree_to_execute="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
5+
>
6+
<BehaviorTree
7+
ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
8+
_description="Example of end effector avoiding a moving point cloud that is down-sampled with spheres."
9+
_favorite="true"
10+
>
11+
<Control ID="Sequence">
12+
<SubTree ID="Clear Snapshot" _collapsed="true" />
13+
<SubTree
14+
ID="Move to Waypoint"
15+
_collapsed="true"
16+
waypoint_name="Home"
17+
joint_group_name="manipulator"
18+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
19+
/>
20+
<SubTree ID="Create PCL Poses Vector" _collapsed="false" />
21+
<SubTree
22+
ID="ComputePoseStampedFromWaypoint"
23+
_collapsed="false"
24+
link_pose="{link_pose}"
25+
/>
26+
<Decorator ID="ForEach" vector_in="{target_poses}" out="{stamped_pose}">
27+
<Control ID="Sequence">
28+
<Action ID="VisualizePose" />
29+
<Action ID="BreakpointSubscriber" _skipIf="true" />
30+
<SubTree
31+
ID="PlacePointCloudFromFileInUI"
32+
_collapsed="false"
33+
input_pose="{stamped_pose}"
34+
transform_cloud="{output_cloud}"
35+
/>
36+
<SubTree
37+
ID="GetPoseAtGripperandDisplay"
38+
_collapsed="false"
39+
output_pose="{gripper_pose}"
40+
/>
41+
<Action
42+
ID="CreateCollisionSpheresAtClosestPoints"
43+
closest_point_count="300"
44+
collision_object="{collision_object}"
45+
filter_cylinder_height="3.0"
46+
filter_cylinder_radius="3.0"
47+
point_cloud="{output_cloud}"
48+
reference_pose="{gripper_pose}"
49+
sphere_radius="0.1"
50+
debug_output_markers="true"
51+
marker_scale_xyz="{marker_scale_xyz2}"
52+
marker_poses="{marker_poses2}"
53+
/>
54+
<Action
55+
ID="CreateStampedTwist"
56+
reference_frame="world"
57+
stamped_twist="{stamped_twist}"
58+
/>
59+
<Action
60+
ID="PublishMarkers"
61+
marker_type="sphere"
62+
marker_namespace="obstacles2"
63+
marker_scale_xyz="{marker_scale_xyz2}"
64+
marker_poses="{marker_poses2}"
65+
/>
66+
<Control ID="Fallback">
67+
<Decorator ID="Timeout" msec="3000">
68+
<Control ID="Sequence">
69+
<Action
70+
ID="MPCSphereClearance"
71+
differentiable="0"
72+
follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory"
73+
gradient_num_trajectory="32"
74+
gradient_spline_points="1.000000"
75+
horizon="0.1"
76+
planning_group="manipulator"
77+
sampling_control_width="0.015000"
78+
sampling_exploration="0.050000"
79+
sampling_sample_width="0.010000"
80+
sampling_spline_points="10.000000"
81+
set_ctrl_command_to_current_state="true"
82+
timestep="0.010000"
83+
total_return="{total_return}"
84+
cartesian_acceleration="100"
85+
cartesian_acceleration_body="tool_changer"
86+
cartesian_velocity="100"
87+
cartesian_velocity_body="tool_changer"
88+
collision_sphere_object="{collision_object}"
89+
gripper_site_name="pinch"
90+
max_cartesian_acceleration_goal="500"
91+
max_cartesian_velocity_goal="0.5"
92+
mujoco_model="description/simple_scene.xml"
93+
mujoco_model_package="lab_sim"
94+
safe_distance="0.25"
95+
sphere_clearance="1.0"
96+
target_pose="{link_pose}"
97+
target_twist="{stamped_twist}"
98+
warmup_iterations="256"
99+
timeout="300"
100+
site_tracking="0.2"
101+
link_sites="attachment_site"
102+
/>
103+
</Control>
104+
</Decorator>
105+
<Action ID="AlwaysSuccess" />
106+
</Control>
107+
</Control>
108+
</Decorator>
109+
</Control>
110+
</BehaviorTree>
111+
<TreeNodesModel>
112+
<SubTree
113+
ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
114+
>
115+
<MetadataFields>
116+
<Metadata runnable="true" />
117+
<Metadata subcategory="Application - MPC Examples" />
118+
</MetadataFields>
119+
</SubTree>
120+
</TreeNodesModel>
121+
</root>

0 commit comments

Comments
 (0)