Skip to content

Commit 0bd7664

Browse files
mariopratsmarioprats
authored
Merge branch 'main' into update-detect-apriltags-ports
2 parents af0b623 + 26eaff1 commit 0bd7664

File tree

8 files changed

+255
-45
lines changed

8 files changed

+255
-45
lines changed

‎src/factory_sim/objectives/request_teleoperation.xml

Lines changed: 17 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -142,17 +142,23 @@
142142
</Decorator>
143143
<!--Cartesian and joint jogging-->
144144
<Control ID="Sequence" _while="teleop_mode == 2">
145-
<Action
146-
ID="ActivateControllers"
147-
controller_names="{controllers}"
148-
/>
149-
<Action
150-
ID="PoseJog"
151-
controller_names="{controllers}"
152-
link_padding="0.010000"
153-
planning_group_names="{planning_groups}"
154-
stop_safety_factor="1.500000"
155-
/>
145+
<Decorator ID="Repeat" num_cycles="-1">
146+
<Control ID="Sequence">
147+
<Action
148+
ID="ActivateControllers"
149+
controller_names="{controllers}"
150+
/>
151+
<Decorator ID="ForceSuccess">
152+
<Action
153+
ID="PoseJog"
154+
controller_names="{controllers}"
155+
link_padding="0.0"
156+
planning_group_names="{planning_groups}"
157+
stop_safety_factor="1.500000"
158+
/>
159+
</Decorator>
160+
</Control>
161+
</Decorator>
156162
</Control>
157163
<Control ID="Sequence" _while="teleop_mode == 1">
158164
<Action

‎src/grinding_sim/objectives/move_to_pose.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
</BehaviorTree>
3030
<TreeNodesModel>
3131
<SubTree ID="Move to Pose">
32+
<Metadata runnable="false" />
3233
<input_port name="target_pose" default="{target_pose}" />
3334
<MetadataFields>
3435
<Metadata subcategory="Motion - Execute" />

‎src/grinding_sim/objectives/move_to_pose_imarker.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@
4747
<SubTree ID="Move to Pose iMarker">
4848
<input_port name="target_pose" default="{target_pose}" />
4949
<MetadataFields>
50+
<Metadata runnable="false" />
5051
<Metadata subcategory="Motion - Execute" />
5152
</MetadataFields>
5253
</SubTree>

‎src/grinding_sim/objectives/request_teleoperation.xml

Lines changed: 17 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -133,17 +133,23 @@
133133
</Decorator>
134134
<!--Cartesian and joint jogging-->
135135
<Control ID="Sequence" _while="teleop_mode == 2">
136-
<Action
137-
ID="ActivateControllers"
138-
controller_names="{controllers}"
139-
/>
140-
<Action
141-
ID="PoseJog"
142-
controller_names="{controllers}"
143-
link_padding="0.010000"
144-
planning_group_names="{planning_groups}"
145-
stop_safety_factor="1.500000"
146-
/>
136+
<Decorator ID="Repeat" num_cycles="-1">
137+
<Control ID="Sequence">
138+
<Action
139+
ID="ActivateControllers"
140+
controller_names="{controllers}"
141+
/>
142+
<Decorator ID="ForceSuccess">
143+
<Action
144+
ID="PoseJog"
145+
controller_names="{controllers}"
146+
link_padding="0.0"
147+
planning_group_names="{planning_groups}"
148+
stop_safety_factor="1.500000"
149+
/>
150+
</Decorator>
151+
</Control>
152+
</Decorator>
147153
</Control>
148154
<Control ID="Sequence" _while="teleop_mode == 1">
149155
<Action

‎src/hangar_sim/objectives/move_to_pose.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@
5050
<input_port name="target_pose" default="{target_pose}" />
5151
<MetadataFields>
5252
<Metadata subcategory="Motion - Execute" />
53+
<Metadata runnable="false" />
5354
</MetadataFields>
5455
</SubTree>
5556
</TreeNodesModel>

‎src/hangar_sim/objectives/request_teleoperation.xml

Lines changed: 17 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -145,17 +145,23 @@
145145
</Decorator>
146146
<!--Cartesian and joint jogging-->
147147
<Control ID="Sequence" _while="teleop_mode == 2">
148-
<Action
149-
ID="ActivateControllers"
150-
controller_names="velocity_force_controller"
151-
/>
152-
<Action
153-
ID="PoseJog"
154-
controller_names="{controllers}"
155-
link_padding="0.00000"
156-
planning_group_names="{planning_groups}"
157-
stop_safety_factor="1.500000"
158-
/>
148+
<Decorator ID="Repeat" num_cycles="-1">
149+
<Control ID="Sequence">
150+
<Action
151+
ID="ActivateControllers"
152+
controller_names="{controllers}"
153+
/>
154+
<Decorator ID="ForceSuccess">
155+
<Action
156+
ID="PoseJog"
157+
controller_names="{controllers}"
158+
link_padding="0.0"
159+
planning_group_names="{planning_groups}"
160+
stop_safety_factor="1.500000"
161+
/>
162+
</Decorator>
163+
</Control>
164+
</Decorator>
159165
</Control>
160166
<Control ID="Sequence" _while="teleop_mode == 1">
161167
<Action

‎src/lab_sim/objectives/request_teleoperation.xml

Lines changed: 19 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
ID="Interpolate to Joint State"
4242
_collapsed="false"
4343
target_joint_state="{target_joint_state}"
44-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
44+
controller_names="/joint_trajectory_controller"
4545
/>
4646
<Action
4747
ID="PublishEmpty"
@@ -76,6 +76,7 @@
7676
ID="Move to Pose"
7777
_collapsed="false"
7878
target_pose="{target_pose}"
79+
controller_names="/joint_trajectory_controller"
7980
/>
8081
<Action
8182
ID="PublishEmpty"
@@ -141,17 +142,23 @@
141142
</Decorator>
142143
<!--Cartesian and joint jogging-->
143144
<Control ID="Sequence" _while="teleop_mode == 2">
144-
<Action
145-
ID="ActivateControllers"
146-
controller_names="velocity_force_controller"
147-
/>
148-
<Action
149-
ID="PoseJog"
150-
controller_names="{controllers}"
151-
link_padding="0.010000"
152-
planning_group_names="{planning_groups}"
153-
stop_safety_factor="1.500000"
154-
/>
145+
<Decorator ID="Repeat" num_cycles="-1">
146+
<Control ID="Sequence">
147+
<Action
148+
ID="ActivateControllers"
149+
controller_names="{controllers}"
150+
/>
151+
<Decorator ID="ForceSuccess">
152+
<Action
153+
ID="PoseJog"
154+
controller_names="{controllers}"
155+
link_padding="0.0"
156+
planning_group_names="{planning_groups}"
157+
stop_safety_factor="1.500000"
158+
/>
159+
</Decorator>
160+
</Control>
161+
</Decorator>
155162
</Control>
156163
<Control ID="Sequence" _while="teleop_mode == 1">
157164
<Action

‎src/moveit_pro_ur_configs/multi_arm_sim/objectives/request_teleoperation.xml

Lines changed: 182 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,182 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
3+
<!--The
4+
integer value used here for teleoperation mode comes from the
5+
moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
6+
<BehaviorTree
7+
ID="Request Teleoperation"
8+
_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."
9+
_favorite="false"
10+
_hardcoded="false"
11+
>
12+
<Control ID="Sequence">
13+
<Action ID="Script" code="teleop_mode := 0" />
14+
<Control ID="Parallel" success_count="1" failure_count="1">
15+
<Action
16+
ID="DoTeleoperateAction"
17+
enable_user_interaction="{enable_user_interaction}"
18+
user_interaction_prompt="{user_interaction_prompt}"
19+
initial_teleop_mode="{initial_teleop_mode}"
20+
current_teleop_mode="{teleop_mode}"
21+
planning_groups="{planning_groups}"
22+
controllers="{controllers}"
23+
/>
24+
<Decorator ID="KeepRunningUntilFailure">
25+
<Control ID="Sequence">
26+
<!--Joint
27+
sliders interpolate to joint state-->
28+
<Decorator ID="ForceSuccess" _while="teleop_mode == 5">
29+
<Control ID="Sequence">
30+
<Control ID="Fallback" name="root">
31+
<Control ID="Sequence">
32+
<Action
33+
ID="RetrieveJointStateParameter"
34+
timeout_sec="-1"
35+
joint_state="{target_joint_state}"
36+
/>
37+
<SubTree
38+
ID="Interpolate to Joint State"
39+
_collapsed="false"
40+
target_joint_state="{target_joint_state}"
41+
/>
42+
<Action
43+
ID="PublishEmpty"
44+
topic="/studio_ui/motion_ended"
45+
queue_size="1"
46+
use_best_effort="false"
47+
/>
48+
</Control>
49+
<Control ID="Sequence">
50+
<Action
51+
ID="PublishEmpty"
52+
topic="/studio_ui/motion_ended"
53+
queue_size="1"
54+
use_best_effort="false"
55+
/>
56+
<Action ID="AlwaysFailure" />
57+
</Control>
58+
</Control>
59+
</Control>
60+
</Decorator>
61+
<!--Interactive
62+
markers move to pose-->
63+
<Decorator ID="ForceSuccess" _while="teleop_mode == 4">
64+
<Control ID="Sequence">
65+
<Control ID="Fallback" name="root">
66+
<Control ID="Sequence">
67+
<Action
68+
ID="RetrievePoseParameter"
69+
timeout_sec="-1"
70+
pose="{target_pose}"
71+
/>
72+
<SubTree
73+
ID="Move to Pose"
74+
_collapsed="false"
75+
target_pose="{target_pose}"
76+
/>
77+
<Action
78+
ID="PublishEmpty"
79+
topic="/studio_ui/motion_ended"
80+
queue_size="1"
81+
use_best_effort="false"
82+
/>
83+
</Control>
84+
<Control ID="Sequence">
85+
<Action
86+
ID="PublishEmpty"
87+
topic="/studio_ui/motion_ended"
88+
queue_size="1"
89+
use_best_effort="false"
90+
/>
91+
<Action ID="AlwaysFailure" />
92+
</Control>
93+
</Control>
94+
</Control>
95+
</Decorator>
96+
<!--Waypoint
97+
buttons move to joint state-->
98+
<Decorator ID="ForceSuccess" _while="teleop_mode == 3">
99+
<Control ID="Sequence">
100+
<Control ID="Fallback" name="root">
101+
<Control ID="Sequence">
102+
<Action
103+
ID="RetrieveJointStateParameter"
104+
timeout_sec="-1"
105+
joint_state="{target_joint_state}"
106+
/>
107+
<SubTree
108+
ID="Move to Joint State"
109+
_collapsed="false"
110+
target_joint_state="{target_joint_state}"
111+
acceleration_scale_factor="1.0"
112+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
113+
controller_names="joint_trajectory_controller"
114+
joint_group_name="manipulator"
115+
keep_orientation="false"
116+
keep_orientation_tolerance="0.05"
117+
link_padding="0.01"
118+
seed="0"
119+
velocity_scale_factor="1.0"
120+
/>
121+
<Action
122+
ID="PublishEmpty"
123+
topic="/studio_ui/motion_ended"
124+
queue_size="1"
125+
use_best_effort="false"
126+
/>
127+
</Control>
128+
<Control ID="Sequence">
129+
<Action
130+
ID="PublishEmpty"
131+
topic="/studio_ui/motion_ended"
132+
queue_size="1"
133+
use_best_effort="false"
134+
/>
135+
<Action ID="AlwaysFailure" />
136+
</Control>
137+
</Control>
138+
</Control>
139+
</Decorator>
140+
<!--Cartesian and joint jogging-->
141+
<Control ID="Sequence" _while="teleop_mode == 2">
142+
<Decorator ID="Repeat" num_cycles="-1">
143+
<Control ID="Sequence">
144+
<Action
145+
ID="ActivateControllers"
146+
controller_names="{controllers}"
147+
/>
148+
<Decorator ID="ForceSuccess">
149+
<Action
150+
ID="PoseJog"
151+
controller_names="{controllers}"
152+
link_padding="0.0"
153+
planning_group_names="{planning_groups}"
154+
stop_safety_factor="1.500000"
155+
/>
156+
</Decorator>
157+
</Control>
158+
</Decorator>
159+
</Control>
160+
<Control ID="Sequence" _while="teleop_mode == 1">
161+
<Action
162+
ID="TeleoperateJointJog"
163+
controller_name="servo_controller"
164+
/>
165+
</Control>
166+
</Control>
167+
</Decorator>
168+
</Control>
169+
</Control>
170+
</BehaviorTree>
171+
<TreeNodesModel>
172+
<SubTree ID="Request Teleoperation">
173+
<MetadataFields>
174+
<Metadata subcategory="User Input" />
175+
<Metadata runnable="false" />
176+
</MetadataFields>
177+
<input_port name="enable_user_interaction" default="false" />
178+
<input_port name="initial_teleop_mode" default="3" />
179+
<input_port name="user_interaction_prompt" default="" />
180+
</SubTree>
181+
</TreeNodesModel>
182+
</root>

0 commit comments

Comments
 (0)