Skip to content
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

Fix JointDiagnostic Objective #80

Open
wants to merge 1 commit into
base: v7.0
Choose a base branch
from
Open

Fix JointDiagnostic Objective #80

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
12 changes: 6 additions & 6 deletions src/lab_sim/objectives/joint_diagnostic.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@
ID="Move to Waypoint"
waypoint_name="Look at Table"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
controller_names="/joint_trajectory_controller"
link_padding="0.0"
/>
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
Expand All @@ -21,15 +21,15 @@
ID="Move to Waypoint"
waypoint_name="Wrist 2 Max"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
controller_names="/joint_trajectory_controller"
link_padding="0.0"
/>
<SubTree
ID="Move to Waypoint"
waypoint_name="Wrist 2 Min"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
controller_names="/joint_trajectory_controller"
link_padding="0.0"
/>
</Control>
</Decorator>
Expand Down
161 changes: 161 additions & 0 deletions src/lab_sim/objectives/request_teleoperation.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,161 @@
<?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.-->
<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"
_subtreeOnly="true"
>
<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-->
<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-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 5">
<Control ID="Sequence">
<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-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 4">
<Control ID="Sequence">
<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-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 3">
<Control ID="Sequence">
<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}"
link_padding="0.0"
/>
<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-->
<Control ID="Sequence" _while="teleop_mode == 2">
<Action
ID="TeleoperateTwist"
controller_name="servo_controller"
/>
</Control>
<Control ID="Sequence" _while="teleop_mode == 1">
<Action
ID="TeleoperateJointJog"
controller_name="servo_controller"
/>
</Control>
</Control>
</Decorator>
</Control>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Request Teleoperation">
<input_port name="enable_user_interaction" default="false" />
<input_port name="user_interaction_prompt" default="" />
<input_port name="initial_teleop_mode" default="3" />
<MetadataFields>
<Metadata subcategory="User Input" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
Loading