Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Commit

Permalink
Teleop objective refactor (#314)
Browse files Browse the repository at this point in the history 
* refactored teleoperate so that it does not use subtrees, and make the subtrees that were used now useable elsewhere

---------

Co-authored-by: Sebastian Jahr <[email protected]>
  • Loading branch information
@MikeWrock @sjahr
MikeWrock and sjahr authored Aug 12, 2024
1 parent 69ea0c7 commit 61cc6fa
Show file tree
Hide file tree
Showing 10 changed files with 135 additions and 97 deletions.
33 changes: 15 additions & 18 deletions src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,17 @@
<?xml version="1.0"?>
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
<BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
<BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation">
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Interpolate to Joint State">
<input_port name="target_joint_state" default="{target_joint_state}"/>
</SubTree>
</TreeNodesModel>
</root>
33 changes: 15 additions & 18 deletions src/picknik_ur_base_config/objectives/move_to_joint_state.xml
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,17 @@
<?xml version="1.0"?>
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
<BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" planner_interface="moveit_default" />
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
<BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state">
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Move to Joint State">
<input_port name="target_joint_state" default="{target_joint_state}"/>
</SubTree>
</TreeNodesModel>
</root>
35 changes: 16 additions & 19 deletions src/picknik_ur_base_config/objectives/move_to_pose.xml
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,18 @@
<?xml version="1.0"?>
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Move to Pose">
<BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
<Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="{controller_names}" task="{move_to_pose_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
<BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_pose_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Move to Pose">
<input_port name="target_pose" default="{target_pose}"/>
</SubTree>
</TreeNodesModel>
</root>
48 changes: 39 additions & 9 deletions src/picknik_ur_base_config/objectives/request_teleoperation.xml
View file
Original file line number Diff line number Diff line change
@@ -1,42 +1,72 @@
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
<!-- The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message. -->
<!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
<BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false">
<Control ID="Sequence">
<Action ID="Script" code="teleop_mode := 0"/>
<Control ID="Parallel" success_count="1" failure_count="1">
<Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<!-- Closing and opening the gripper -->
<!--Closing and opening the gripper-->
<Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
<SubTree ID="Close Gripper"/>
</Decorator>
<Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
<SubTree ID="Open Gripper"/>
</Decorator>
<!-- Joint sliders interpolate to joint state -->
<!--Joint sliders interpolate to joint state-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 5">
<Control ID="Sequence">
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
<SubTree ID="Interpolate to Joint State"/>
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</Control>
</Decorator>
<!-- Interactive markers move to pose -->
<!--Interactive markers move to pose-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 4">
<Control ID="Sequence">
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
<SubTree ID="Move to Pose" controller_names="/joint_trajectory_controller"/>
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
<SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</Control>
</Decorator>
<!-- Waypoint buttons move to joint state -->
<!--Waypoint buttons move to joint state-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 3">
<Control ID="Sequence">
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
<SubTree ID="Move to Joint State"/>
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</Control>
</Decorator>
<!-- Cartesian and joint jogging -->
<!--Cartesian and joint jogging-->
<Control ID="Sequence" _while="teleop_mode == 2">
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
<Action ID="TeleoperateTwist" controller_name="servo_controller"/>
Expand Down
4 changes: 0 additions & 4 deletions src/picknik_ur_multi_arm_config/config/config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,6 @@ hardware:
package: "picknik_ur_base_config"
path: "config/moveit/joint_limits.yaml"
# The following files are loaded based on the ur_description package layout.
# To use parameters from a different package, place them in a config/ROBOT_NAME/ directory,
# replace ROBOT_NAME with the value used for hardware.type in this file.
- kinematics_parameters_file:
# Load default_kinematics.yaml from ur_description/config/<ur_type>
package: "ur_description"
Expand All @@ -69,7 +67,6 @@ hardware:
package: "ur_description"
path: "config/ur5e/visual_parameters.yaml"


# Sets ROS global params for launch.
# [Optional]
ros_global_params:
Expand Down Expand Up @@ -124,7 +121,6 @@ moveit_params:
allowed_goal_duration_margin: 5.0
allowed_start_tolerance: 0.01


# Configuration for launching ros2_control processes.
# [Required, if using ros2_control]
ros2_control:
Expand Down
13 changes: 5 additions & 8 deletions src/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,17 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
<BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/multi_arm_joint_trajectory_controller" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="multi_arm_manipulator" task="{interpolate_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Interpolate to Joint State">
<input_port name="target_joint_state" default="{target_joint_state}"/>
</SubTree>
</TreeNodesModel>
</root>
13 changes: 5 additions & 8 deletions src/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,17 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
<BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/multi_arm_joint_trajectory_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="multi_arm_manipulator" task="{move_to_waypoint_task}" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Move to Joint State">
<input_port name="target_joint_state" default="{target_joint_state}"/>
</SubTree>
</TreeNodesModel>
</root>
13 changes: 5 additions & 8 deletions src/picknik_ur_multi_arm_config/objectives/move_to_pose.xml
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,18 @@
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Move to Pose">
<BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
<Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/multi_arm_joint_trajectory_controller" task="{move_to_pose_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="first_grasp_link" planning_group_name="first_manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Move to Pose">
<input_port name="target_pose" default="{target_pose}"/>
</SubTree>
</TreeNodesModel>
</root>
Loading

0 comments on commit 61cc6fa

Please sign in to comment.