Skip to content

Commit 5003d71

Browse files
mariopratsmarioprats
committed
improve tool changing demo
1 parent d0db041 commit 5003d71

File tree

6 files changed

+137
-26
lines changed

6 files changed

+137
-26
lines changed

‎src/fanuc_sim/description/scene.xml‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<!-- UR5e inspired from https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e -->
22
<mujoco model="fanuc scene">
33
<compiler angle="radian" autolimits="true" />
4-
<option integrator="RK4" timestep="0.005" />
4+
<option integrator="implicit" />
55
<include file="lrmate200id/lrmate200id_globals.xml" />
66
<include file="tool.xml" />
77

@@ -110,16 +110,16 @@
110110
<!-- Add a scene camera -->
111111
<site
112112
name="scene_camera_optical_frame"
113-
pos="0.0 1.0 1.6"
114-
euler="2.6415 0 3.1415"
113+
pos="1.1 1.1 1.5"
114+
euler="2.6 -0.8 -2.14"
115115
/>
116116
<camera
117117
name="scene_camera"
118-
pos="0.0 1.0 1.6"
118+
pos="1.1 1.1 1.5"
119119
fovy="58"
120120
mode="fixed"
121121
resolution="640 480"
122-
euler="-0.5 0 3.1415"
122+
euler="-0.54 0.8 2.14"
123123
/>
124124
<body name="arm_pedestal" pos="0.0 0.3 0.15">
125125
<geom type="box" size="0.2 0.2 0.15" />

‎src/fanuc_sim/objectives/pick_up_block_with_tool.xml‎

Lines changed: 39 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="Pick Up Block With Tool">
23
<!--//////////-->
34
<BehaviorTree
@@ -17,22 +18,55 @@
1718
ID="InitializeMTCTask"
1819
task_id="pick_up_tool"
1920
controller_names="/joint_trajectory_controller"
21+
task="{mtc_task}"
2022
/>
21-
<Action ID="SetupMTCCurrentState" />
2223
<Action
23-
ID="SetupMTCMoveToPose"
24-
target_pose="{approach_pose}"
24+
ID="SetupMTCCurrentState"
25+
skip_collision_check="false"
26+
task="{mtc_task}"
27+
/>
28+
<Action
29+
ID="SetupMTCIgnoreCollisionsBetweenObjects"
30+
allow_collision="true"
31+
task="{mtc_task}"
32+
object_names="link_5;suction_gripper"
33+
/>
34+
<Action
35+
ID="SetupMTCPlanToPose"
36+
acceleration_scale_factor="0.5"
2537
ik_frame="tool0"
38+
keep_orientation="false"
39+
keep_orientation_tolerance="0.100000"
40+
link_padding="0.00000"
41+
max_iterations="5000"
42+
monitored_stage="current state"
43+
planning_group_name="manipulator"
44+
target_pose="{approach_pose}"
45+
trajectory_sampling_rate="100"
46+
velocity_scale_factor="0.5"
47+
task="{mtc_task}"
2648
/>
2749
<Action
2850
ID="SetupMTCMoveAlongFrameAxis"
2951
hand_frame="tool0"
3052
max_distance="0.05"
3153
min_distance="0.05"
3254
axis_frame="tool0"
55+
acceleration_scale="0.5"
56+
axis_x="0.000000"
57+
axis_y="0.000000"
58+
axis_z="1.000000"
59+
ignore_environment_collisions="false"
60+
planning_group_name="manipulator"
61+
velocity_scale="0.5"
62+
task="{mtc_task}"
63+
/>
64+
<Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
65+
<Action
66+
ID="ExecuteMTCTask"
67+
goal_duration_tolerance="-1.000000"
68+
solution="{mtc_solution}"
3369
/>
34-
<Action ID="PlanMTCTask" />
35-
<Action ID="ExecuteMTCTask" />
3670
<Action
3771
ID="MoveGripperAction"
3872
timeout="10"

‎src/fanuc_sim/objectives/pick_up_tool.xml‎

Lines changed: 33 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="Pick Up Tool">
23
<!--//////////-->
34
<BehaviorTree
@@ -16,23 +17,49 @@
1617
ID="InitializeMTCTask"
1718
task_id="pick_up_tool"
1819
controller_names="/joint_trajectory_controller"
20+
task="{mtc_task}"
1921
/>
20-
<Action ID="SetupMTCCurrentState" />
2122
<Action
22-
ID="SetupMTCMoveToPose"
23-
target_pose="{approach_pose}"
23+
ID="SetupMTCCurrentState"
24+
skip_collision_check="false"
25+
task="{mtc_task}"
26+
/>
27+
<Action
28+
ID="SetupMTCPlanToPose"
29+
acceleration_scale_factor="0.5"
2430
ik_frame="tool0"
25-
planner_interface="pro_rrt"
31+
keep_orientation="false"
32+
keep_orientation_tolerance="0.100000"
33+
link_padding="0.000000"
34+
max_iterations="5000"
35+
monitored_stage="current state"
36+
planning_group_name="manipulator"
37+
target_pose="{approach_pose}"
38+
trajectory_sampling_rate="100"
39+
velocity_scale_factor="0.5"
40+
task="{mtc_task}"
2641
/>
2742
<Action
2843
ID="SetupMTCMoveAlongFrameAxis"
2944
hand_frame="tool0"
3045
max_distance="0.095"
3146
min_distance="0.095"
3247
axis_frame="tool0"
48+
acceleration_scale="0.5"
49+
axis_x="0.000000"
50+
axis_y="0.000000"
51+
axis_z="1.000000"
52+
ignore_environment_collisions="false"
53+
planning_group_name="manipulator"
54+
velocity_scale="0.5"
55+
task="{mtc_task}"
56+
/>
57+
<Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
58+
<Action
59+
ID="ExecuteMTCTask"
60+
goal_duration_tolerance="-1.000000"
61+
solution="{mtc_solution}"
3362
/>
34-
<Action ID="PlanMTCTask" />
35-
<Action ID="ExecuteMTCTask" />
3663
<Action
3764
ID="MoveGripperAction"
3865
position="1"

‎src/fanuc_sim/objectives/place_tool_in_tool_holder.xml‎

Lines changed: 39 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="Place Tool in Tool Holder">
23
<!--//////////-->
34
<BehaviorTree
@@ -16,23 +17,55 @@
1617
ID="InitializeMTCTask"
1718
task_id="pick_up_tool"
1819
controller_names="/joint_trajectory_controller"
20+
task="{mtc_task}"
1921
/>
20-
<Action ID="SetupMTCCurrentState" />
2122
<Action
22-
ID="SetupMTCMoveToPose"
23-
target_pose="{approach_pose}"
23+
ID="SetupMTCCurrentState"
24+
skip_collision_check="false"
25+
task="{mtc_task}"
26+
/>
27+
<Action
28+
ID="SetupMTCIgnoreCollisionsBetweenObjects"
29+
allow_collision="true"
30+
task="{mtc_task}"
31+
object_names="link_5;suction_gripper"
32+
/>
33+
<Action
34+
ID="SetupMTCPlanToPose"
35+
acceleration_scale_factor="0.5"
2436
ik_frame="tool0"
25-
planner_interface="pro_rrt"
37+
keep_orientation="false"
38+
keep_orientation_tolerance="0.100000"
39+
link_padding="0.020000"
40+
max_iterations="5000"
41+
monitored_stage="current state"
42+
planning_group_name="manipulator"
43+
target_pose="{approach_pose}"
44+
trajectory_sampling_rate="100"
45+
velocity_scale_factor="0.5"
46+
task="{mtc_task}"
2647
/>
2748
<Action
2849
ID="SetupMTCMoveAlongFrameAxis"
2950
hand_frame="tool0"
3051
max_distance="0.14"
3152
axis_frame="tool0"
3253
min_distance="0.14"
54+
acceleration_scale="0.5"
55+
axis_x="0.000000"
56+
axis_y="0.000000"
57+
axis_z="1.000000"
58+
ignore_environment_collisions="false"
59+
planning_group_name="manipulator"
60+
velocity_scale="0.5"
61+
task="{mtc_task}"
62+
/>
63+
<Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
64+
<Action
65+
ID="ExecuteMTCTask"
66+
goal_duration_tolerance="-1.000000"
67+
solution="{mtc_solution}"
3368
/>
34-
<Action ID="PlanMTCTask" />
35-
<Action ID="ExecuteMTCTask" />
3669
<Action ID="WaitForDuration" delay_duration="1" />
3770
<Action
3871
ID="MoveGripperAction"

‎src/fanuc_sim/objectives/retract.xml‎

Lines changed: 20 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,24 +5,41 @@
55
ID="Retract"
66
_description="Move back a distance in the tool frame"
77
_favorite="false"
8+
distance="0.1"
89
>
910
<Control ID="Sequence" name="TopLevelSequence">
1011
<Action
1112
ID="InitializeMTCTask"
1213
controller_names="/joint_trajectory_controller"
1314
task_id="retract"
15+
task="{mtc_task}"
16+
/>
17+
<Action
18+
ID="SetupMTCCurrentState"
19+
skip_collision_check="true"
20+
task="{mtc_task}"
1421
/>
15-
<Action ID="SetupMTCCurrentState" skip_collision_check="true" />
1622
<Action
1723
ID="SetupMTCMoveAlongFrameAxis"
1824
axis_frame="tool0"
1925
axis_z="-1"
2026
max_distance="{distance}"
2127
min_distance="{distance}"
2228
hand_frame="tool0"
29+
acceleration_scale="0.5"
30+
axis_x="0.000000"
31+
axis_y="0.000000"
32+
ignore_environment_collisions="false"
33+
planning_group_name="manipulator"
34+
velocity_scale="0.5"
35+
task="{mtc_task}"
36+
/>
37+
<Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
38+
<Action
39+
ID="ExecuteMTCTask"
40+
goal_duration_tolerance="-1.000000"
41+
solution="{mtc_solution}"
2342
/>
24-
<Action ID="PlanMTCTask" />
25-
<Action ID="ExecuteMTCTask" />
2643
</Control>
2744
</BehaviorTree>
2845
<TreeNodesModel>

‎src/picknik_accessories‎

0 commit comments

Comments
 (0)