Skip to content

Add Objectives for showing mpc pushing end-effector with spheres down sampled from a pcl #465

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

Open
bkanator wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Add Objectives for showing mpc pushing end-effector with spheres down sampled from a pcl #465

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
47 changes: 47 additions & 0 deletions src/lab_sim/objectives/compute_posestamped_from_waypoint.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="ComputePoseStampedFromWaypoint">
<!--//////////-->
<BehaviorTree ID="ComputePoseStampedFromWaypoint">
<Control ID="Sequence" _collapsed="false">
<Action
ID="RetrieveWaypoint"
joint_group_name="manipulator"
waypoint_joint_state="{target_joint_state}"
waypoint_name="Above Pick Cube"
/>
<Action
ID="UnpackRobotJointStateMessage"
joint_state="{joint_state}"
multi_dof_joint_state="{multi_dof_joint_state}"
robot_joint_state="{target_joint_state}"
/>
<Action
ID="GetCurrentPlanningScene"
planning_scene_msg="{planning_scene}"
/>
<Action
ID="ComputeLinkPoseForwardKinematics"
joint_state="{joint_state}"
link_name="grasp_link"
link_pose="{link_pose}"
planning_scene="{planning_scene}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="ComputePoseStampedFromWaypoint">
<MetadataFields>
<Metadata runnable="false" />
<Metadata subcategory="Application - Basic Examples" />
<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."
/>
</MetadataFields>
<inout_port
name="link_pose"
default="{link_pose}"
type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
/>
</SubTree>
</TreeNodesModel>
</root>
94 changes: 94 additions & 0 deletions src/lab_sim/objectives/create_pcl_poses_vector.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Create PCL Poses Vector">
<!--//////////-->
<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}"
>
<Control ID="Sequence">
<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"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<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"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<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"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<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"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<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"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<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"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Create PCL Poses Vector">
<MetadataFields>
<Metadata subcategory="Vector Handling" />
<Metadata runnable="false" />
</MetadataFields>
<output_port name="target_poses" default="{target_poses}" />
</SubTree>
</TreeNodesModel>
</root>
32 changes: 32 additions & 0 deletions src/lab_sim/objectives/get_pose_at_gripper_and_display.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="GetPoseAtGripperandDisplay">
<!--//////////-->
<BehaviorTree
ID="GetPoseAtGripperandDisplay"
_description="Create a pose at the gripper, transform it, and display it."
>
<Control ID="Sequence">
<Action
ID="CreateStampedPose"
position_xyz="0; 0; 0"
stamped_pose="{stamped_pose}"
reference_frame="pinch"
/>
<Action ID="TransformPoseFrame" input_pose="{stamped_pose}" />
<Action ID="VisualizePose" pose="{output_pose}" />
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="GetPoseAtGripperandDisplay">
<MetadataFields>
<Metadata runnable="false" />
<Metadata subcategory="Pose Handling" />
</MetadataFields>
<inout_port
name="output_pose"
default="{output_pose}"
type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
/>
</SubTree>
</TreeNodesModel>
</root>
121 changes: 121 additions & 0 deletions ...im/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.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="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
>
<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"
>
<Control ID="Sequence">
<SubTree ID="Clear Snapshot" _collapsed="true" />
<SubTree
ID="Move to Waypoint"
_collapsed="true"
waypoint_name="Home"
joint_group_name="manipulator"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
/>
<SubTree ID="Create PCL Poses Vector" _collapsed="false" />
<SubTree
ID="ComputePoseStampedFromWaypoint"
_collapsed="false"
link_pose="{link_pose}"
/>
<Decorator ID="ForEach" vector_in="{target_poses}" out="{stamped_pose}">
<Control ID="Sequence">
<Action ID="VisualizePose" />
<Action ID="BreakpointSubscriber" _skipIf="true" />
<SubTree
ID="PlacePointCloudFromFileInUI"
_collapsed="false"
input_pose="{stamped_pose}"
transform_cloud="{output_cloud}"
/>
<SubTree
ID="GetPoseAtGripperandDisplay"
_collapsed="false"
output_pose="{gripper_pose}"
/>
<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}"
/>
<Action
ID="CreateStampedTwist"
reference_frame="world"
stamped_twist="{stamped_twist}"
/>
<Action
ID="PublishMarkers"
marker_type="sphere"
marker_namespace="obstacles2"
marker_scale_xyz="{marker_scale_xyz2}"
marker_poses="{marker_poses2}"
/>
<Control ID="Fallback">
<Decorator ID="Timeout" msec="3000">
<Control ID="Sequence">
<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"
/>
</Control>
</Decorator>
<Action ID="AlwaysSuccess" />
</Control>
</Control>
</Decorator>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree
ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
>
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="Application - MPC Examples" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
Loading
Loading