Skip to content

Commit 04d2aae

Browse files
MikeWrockMikeWrock
authored
Merge pull request #77 from PickNikRobotics/update-kinova-objectives-7.0
Update kinova objectives
2 parents 9186bb0 + 6b9f7ed commit 04d2aae

File tree

7 files changed

+293
-53
lines changed

7 files changed

+293
-53
lines changed

‎src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml‎

Lines changed: 18 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<?xml version="1.0" encoding="UTF-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Move Along Path">
3-
<!-- ////////// -->
3+
<!--//////////-->
44
<BehaviorTree
55
ID="Move Along Path"
66
_description="Moves the robot tip on a rectangular path."
@@ -9,6 +9,10 @@
99
>
1010
<Control ID="Sequence" name="TopLevelSequence">
1111
<Control ID="Sequence">
12+
<Action
13+
ID="ActivateControllers"
14+
controller_names="/joint_trajectory_controller"
15+
/>
1216
<Action
1317
ID="CreateStampedPose"
1418
reference_frame="base_link"
@@ -18,8 +22,8 @@
1822
/>
1923
<Action
2024
ID="AddPoseStampedToVector"
21-
input_pose="{stamped_pose}"
22-
pose_stamped_vector="{pose_stamped_vector}"
25+
input="{stamped_pose}"
26+
vector="{pose_stamped_vector}"
2327
/>
2428
<Action
2529
ID="CreateStampedPose"
@@ -30,8 +34,8 @@
3034
/>
3135
<Action
3236
ID="AddPoseStampedToVector"
33-
input_pose="{stamped_pose}"
34-
pose_stamped_vector="{pose_stamped_vector}"
37+
input="{stamped_pose}"
38+
vector="{pose_stamped_vector}"
3539
/>
3640
<Action
3741
ID="CreateStampedPose"
@@ -42,8 +46,8 @@
4246
/>
4347
<Action
4448
ID="AddPoseStampedToVector"
45-
input_pose="{stamped_pose}"
46-
pose_stamped_vector="{pose_stamped_vector}"
49+
input="{stamped_pose}"
50+
vector="{pose_stamped_vector}"
4751
/>
4852
<Action
4953
ID="CreateStampedPose"
@@ -54,8 +58,8 @@
5458
/>
5559
<Action
5660
ID="AddPoseStampedToVector"
57-
input_pose="{stamped_pose}"
58-
pose_stamped_vector="{pose_stamped_vector}"
61+
input="{stamped_pose}"
62+
vector="{pose_stamped_vector}"
5963
/>
6064
</Control>
6165
<Decorator ID="KeepRunningUntilFailure">
@@ -65,13 +69,14 @@
6569
path="{pose_stamped_vector}"
6670
planning_group_name="manipulator"
6771
tip_link="end_effector_link"
68-
tip_offset="0.0;0.0;0.0"
72+
tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
6973
position_only="false"
7074
blending_radius="0.02"
7175
velocity_scale_factor="0.5"
7276
acceleration_scale_factor="0.5"
7377
trajectory_sampling_rate="100"
7478
joint_trajectory_msg="{joint_trajectory_msg}"
79+
debug_solution="{debug_solution}"
7580
/>
7681
<Action
7782
ID="GetCurrentPlanningScene"
@@ -84,11 +89,14 @@
8489
joint_trajectory_msg="{joint_trajectory_msg}"
8590
joint_space_step="0.05"
8691
cartesian_space_step="0.02"
92+
debug_solution="{debug_solution}"
8793
/>
8894
<Action
8995
ID="ExecuteFollowJointTrajectory"
9096
execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
9197
joint_trajectory_msg="{joint_trajectory_msg}"
98+
goal_position_tolerance="0.000000"
99+
goal_time_tolerance="0.000000"
92100
/>
93101
</Control>
94102
</Decorator>

‎src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/request_teleoperation.xml‎

Lines changed: 122 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,12 @@
11
<?xml version="1.0" encoding="UTF-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
3-
<!-- The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message. -->
3+
<!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
44
<BehaviorTree
55
ID="Request Teleoperation"
66
_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."
77
_favorite="false"
88
_hardcoded="false"
9+
_subtreeOnly="true"
910
>
1011
<Control ID="Sequence">
1112
<Action ID="Script" code="teleop_mode := 0" />
@@ -19,39 +20,141 @@
1920
/>
2021
<Decorator ID="KeepRunningUntilFailure">
2122
<Control ID="Sequence">
22-
<!-- Closing and opening the gripper -->
23+
<!--Closing and opening the gripper-->
2324
<Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
2425
<SubTree ID="Close Gripper" />
2526
</Decorator>
2627
<Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
2728
<SubTree ID="Open Gripper" />
2829
</Decorator>
29-
<!-- Joint sliders interpolate to joint state -->
30+
<!--Joint sliders interpolate to joint state-->
3031
<Decorator ID="ForceSuccess" _while="teleop_mode == 5">
31-
<SubTree ID="Interpolate to Joint State" />
32+
<Control ID="Sequence">
33+
<Control ID="Fallback" name="root">
34+
<Control ID="Sequence">
35+
<Action
36+
ID="RetrieveJointStateParameter"
37+
timeout_sec="-1"
38+
joint_state="{target_joint_state}"
39+
/>
40+
<SubTree
41+
ID="Interpolate to Joint State"
42+
_collapsed="false"
43+
target_joint_state="{target_joint_state}"
44+
/>
45+
<Action
46+
ID="PublishEmpty"
47+
topic="/studio_ui/motion_ended"
48+
queue_size="1"
49+
use_best_effort="false"
50+
/>
51+
</Control>
52+
<Control ID="Sequence">
53+
<Action
54+
ID="PublishEmpty"
55+
topic="/studio_ui/motion_ended"
56+
queue_size="1"
57+
use_best_effort="false"
58+
/>
59+
<Action ID="AlwaysFailure" />
60+
</Control>
61+
</Control>
62+
</Control>
3263
</Decorator>
33-
<!-- Interactive markers move to pose -->
64+
<!--Interactive markers move to pose-->
3465
<Decorator ID="ForceSuccess" _while="teleop_mode == 4">
35-
<SubTree ID="Move to Pose" />
66+
<Control ID="Sequence">
67+
<Control ID="Fallback" name="root">
68+
<Control ID="Sequence">
69+
<Action
70+
ID="RetrievePoseParameter"
71+
timeout_sec="-1"
72+
pose="{target_pose}"
73+
/>
74+
<SubTree
75+
ID="Move to Pose"
76+
_collapsed="false"
77+
target_pose="{target_pose}"
78+
/>
79+
<Action
80+
ID="PublishEmpty"
81+
topic="/studio_ui/motion_ended"
82+
queue_size="1"
83+
use_best_effort="false"
84+
/>
85+
</Control>
86+
<Control ID="Sequence">
87+
<Action
88+
ID="PublishEmpty"
89+
topic="/studio_ui/motion_ended"
90+
queue_size="1"
91+
use_best_effort="false"
92+
/>
93+
<Action ID="AlwaysFailure" />
94+
</Control>
95+
</Control>
96+
</Control>
3697
</Decorator>
37-
<!-- Waypoint buttons move to joint state -->
98+
<!--Waypoint buttons move to joint state-->
3899
<Decorator ID="ForceSuccess" _while="teleop_mode == 3">
39-
<SubTree ID="Move to Joint State" />
100+
<Control ID="Sequence">
101+
<Control ID="Fallback" name="root">
102+
<Control ID="Sequence">
103+
<Action
104+
ID="RetrieveJointStateParameter"
105+
timeout_sec="-1"
106+
joint_state="{target_joint_state}"
107+
/>
108+
<SubTree
109+
ID="Move to Joint State"
110+
_collapsed="false"
111+
target_joint_state="{target_joint_state}"
112+
/>
113+
<Action
114+
ID="PublishEmpty"
115+
topic="/studio_ui/motion_ended"
116+
queue_size="1"
117+
use_best_effort="false"
118+
/>
119+
</Control>
120+
<Control ID="Sequence">
121+
<Action
122+
ID="PublishEmpty"
123+
topic="/studio_ui/motion_ended"
124+
queue_size="1"
125+
use_best_effort="false"
126+
/>
127+
<Action ID="AlwaysFailure" />
128+
</Control>
129+
</Control>
130+
</Control>
40131
</Decorator>
41-
<!-- Cartesian and joint jogging -->
42-
<Action
43-
ID="TeleoperateTwist"
44-
controller_name="servo_controller"
45-
_while="teleop_mode == 2"
46-
/>
47-
<Action
48-
ID="TeleoperateJointJog"
49-
controller_name="servo_controller"
50-
_while="teleop_mode == 1"
51-
/>
132+
<!--Cartesian and joint jogging-->
133+
<Control ID="Sequence" _while="teleop_mode == 2">
134+
<Action
135+
ID="TeleoperateTwist"
136+
controller_name="servo_controller"
137+
/>
138+
</Control>
139+
<Control ID="Sequence" _while="teleop_mode == 1">
140+
<Action
141+
ID="TeleoperateJointJog"
142+
controller_name="servo_controller"
143+
/>
144+
</Control>
52145
</Control>
53146
</Decorator>
54147
</Control>
55148
</Control>
56149
</BehaviorTree>
150+
<TreeNodesModel>
151+
<SubTree ID="Request Teleoperation">
152+
<input_port name="enable_user_interaction" default="false" />
153+
<input_port name="user_interaction_prompt" default="" />
154+
<input_port name="initial_teleop_mode" default="3" />
155+
<MetadataFields>
156+
<Metadata subcategory="User Input" />
157+
</MetadataFields>
158+
</SubTree>
159+
</TreeNodesModel>
57160
</root>

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,8 @@
6262
<Control ID="Sequence">
6363
<Action
6464
ID="LoadPointCloudFromFile"
65-
file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
65+
file_path="descriptions/miscellaneous/mock_satellite.stl"
66+
package_name="picknik_accessories"
6667
frame_id="object_frame"
6768
num_sampled_points="3000"
6869
random_seed="1234"

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,8 @@
6262
<Control ID="Sequence">
6363
<Action
6464
ID="LoadPointCloudFromFile"
65-
file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
65+
file_path="descriptions/miscellaneous/mock_satellite.stl"
66+
package_name="picknik_accessories"
6667
frame_id="object_frame"
6768
num_sampled_points="3000"
6869
random_seed="1234"

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,8 @@
6262
<Control ID="Sequence">
6363
<Action
6464
ID="LoadPointCloudFromFile"
65-
file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
65+
file_path="descriptions/miscellaneous/mock_satellite.stl"
66+
package_name="picknik_accessories"
6667
frame_id="object_frame"
6768
num_sampled_points="3000"
6869
random_seed="1234"

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path.xml‎

Lines changed: 25 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
1+
<?xml version="1.0" encoding="utf-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Move Along Path">
33
<!--//////////-->
44
<BehaviorTree
@@ -8,6 +8,19 @@
88
_hardcoded="false"
99
>
1010
<Control ID="Sequence" name="TopLevelSequence">
11+
<SubTree
12+
ID="Move to Waypoint"
13+
_collapsed="false"
14+
acceleration_scale_factor="1.0"
15+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
16+
controller_names="/joint_trajectory_controller"
17+
joint_group_name="manipulator"
18+
keep_orientation="false"
19+
keep_orientation_tolerance="0.05"
20+
link_padding="0.01"
21+
velocity_scale_factor="1.0"
22+
waypoint_name="View Satellite"
23+
/>
1124
<Control ID="Sequence">
1225
<Action
1326
ID="ActivateControllers"
@@ -69,14 +82,16 @@
6982
path="{pose_stamped_vector}"
7083
planning_group_name="manipulator"
7184
tip_link="end_effector_link"
72-
tip_offset="0.0;0.0;0.0"
85+
tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
7386
position_only="false"
7487
blending_radius="0.02"
7588
velocity_scale_factor="0.5"
7689
acceleration_scale_factor="0.5"
7790
trajectory_sampling_rate="100"
7891
joint_trajectory_msg="{joint_trajectory_msg}"
7992
debug_solution="{debug_solution}"
93+
ik_cartesian_space_density="0.010000"
94+
ik_joint_space_density="0.100000"
8095
/>
8196
<Action
8297
ID="GetCurrentPlanningScene"
@@ -97,9 +112,17 @@
97112
joint_trajectory_msg="{joint_trajectory_msg}"
98113
goal_position_tolerance="0.000000"
99114
goal_time_tolerance="0.000000"
115+
goal_duration_tolerance="-1.000000"
100116
/>
101117
</Control>
102118
</Decorator>
103119
</Control>
104120
</BehaviorTree>
121+
<TreeNodesModel>
122+
<SubTree ID="Move Along Path">
123+
<MetadataFields>
124+
<Metadata runnable="true" />
125+
</MetadataFields>
126+
</SubTree>
127+
</TreeNodesModel>
105128
</root>

0 commit comments

Comments
 (0)