Skip to content

Commit 6fa480c

Browse files
mariopratsmarioprats
authored
Merge pull request #31 from PickNikRobotics/tool-change-example
Setup the fanuc_sim config with a basic tool-changing example
2 parents bcd5add + 5003d71 commit 6fa480c

20 files changed

+615
-27
lines changed

‎src/fanuc_sim/config/config.yaml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ hardware:
4343
urdf_params:
4444
# Use "mock", "mujoco", or "real"
4545
- hardware_interface: "mujoco"
46+
- mujoco_viewer: "false"
4647

4748
# Sets ROS global params for launch.
4849
# [Optional]
@@ -112,6 +113,8 @@ ros2_control:
112113
- "joint_trajectory_controller"
113114
- "servo_controller"
114115
- "joint_state_broadcaster"
116+
- "tool_attach_controller"
117+
- "suction_cup_controller"
115118
# - "servo_controller"
116119
# Load but do not start these controllers so they can be activated later if needed.
117120
# [Optional, default=[]]

‎src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,29 @@ controller_manager:
77
type: joint_trajectory_controller/JointTrajectoryController
88
servo_controller:
99
type: joint_trajectory_controller/JointTrajectoryController
10+
# A controller to enable/disable the tool attachment interface at the robot flange.
11+
# In sim, this is modeled as a weld constraint in Mujoco.
12+
# In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange.
13+
tool_attach_controller:
14+
type: position_controllers/GripperActionController
15+
# A controller to enable/disable the suction cup.
16+
# In sim, this is modeled as a weld constraint in Mujoco.
17+
# In a real robot, this controller would activate / deactivate a suction cup mechanism.
18+
suction_cup_controller:
19+
type: position_controllers/GripperActionController
20+
21+
22+
tool_attach_controller:
23+
ros__parameters:
24+
default: true
25+
joint: suction_cup_tool_interface
26+
allow_stalling: true
27+
28+
suction_cup_controller:
29+
ros__parameters:
30+
default: true
31+
joint: suction_cup_tool_tip
32+
allow_stalling: true
1033

1134
joint_state_broadcaster:
1235
ros__parameters:

‎src/fanuc_sim/config/moveit/picknik_fanuc.srdf‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
<end_effector name="fanuc_ee" parent_link="tool0" group="gripper"/>
2020

2121
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
22+
<disable_collisions link1="arm_pedestal" link2="base_link" reason="Adjacent"/>
2223
<disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
2324
<disable_collisions link1="base_link" link2="link_2" reason="Never"/>
2425
<disable_collisions link1="base_link" link2="link_3" reason="Never"/>

‎src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
params="name initial_positions_file hardware_interface mujoco_model"
66
>
77
<xacro:arg name="mujoco_model" default="description/scene.xml" />
8+
<xacro:arg name="mujoco_viewer" default="false" />
89
<xacro:arg name="hardware_interface" default="mock" />
910
<xacro:property
1011
name="initial_positions"
@@ -26,6 +27,7 @@
2627
<plugin>picknik_mujoco_ros/MujocoSystem</plugin>
2728
<param name="mujoco_model">$(arg mujoco_model)</param>
2829
<param name="mujoco_model_package">fanuc_sim</param>
30+
<param name="mujoco_viewer">$(arg mujoco_viewer)</param>
2931
<param name="render_publish_rate">10</param>
3032
<param name="tf_publish_rate">60</param>
3133
<param name="lidar_publish_rate">10</param>

‎src/fanuc_sim/description/picknik_fanuc.xacro‎

Lines changed: 52 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,61 @@
1313
filename="$(find fanuc_sim)/description/fanuc_lrmate200id.ros2_control.xacro"
1414
/>
1515

16-
<link name="world" />
17-
<joint name="world_joint" type="fixed">
16+
<link name="world"/>
17+
<link name="arm_pedestal">
18+
<visual>
19+
<origin rpy="0 0 0" xyz="0 0 0" />
20+
<geometry>
21+
<box size="0.4 0.4 0.3" />
22+
</geometry>
23+
</visual>
24+
<visual>
25+
<origin rpy="0 0 0" xyz="0.23 0.03 0.1" />
26+
<geometry>
27+
<box size="0.12 0.02 0.02" />
28+
</geometry>
29+
</visual>
30+
<visual>
31+
<origin rpy="0 0 0" xyz="0.23 -0.03 0.1" />
32+
<geometry>
33+
<box size="0.12 0.02 0.02" />
34+
</geometry>
35+
</visual>
36+
</link>
37+
<joint name="scene_joint" type="fixed">
1838
<parent link="world" />
39+
<child link="arm_pedestal" />
40+
<origin rpy="0 0 0" xyz="0 0.3 0.15" />
41+
</joint>
42+
<joint name="base_joint" type="fixed">
43+
<parent link="arm_pedestal" />
1944
<child link="base_link" />
20-
<origin rpy="0 0 0" xyz="0 0 0" />
45+
<origin rpy="0 0 0" xyz="0 0 0.15" />
2146
</joint>
47+
48+
<link name="table">
49+
<visual>
50+
<origin rpy="0 0 0" xyz="0 0 0" />
51+
<geometry>
52+
<box size="1 1 0.05" />
53+
</geometry>
54+
<material name="brown">
55+
<color rgba="0.3 0.15 0.05 1" />
56+
</material>
57+
</visual>
58+
<collision>
59+
<origin rpy="0 0 0" xyz="0 0 0" />
60+
<geometry>
61+
<box size="1 1 0.05" />
62+
</geometry>
63+
</collision>
64+
</link>
65+
<joint name="table_joint" type="fixed">
66+
<parent link="world" />
67+
<child link="table" />
68+
<origin rpy="0 0 0" xyz="0 1 0.45" />
69+
</joint>
70+
2271
<xacro:fanuc_lrmate200id prefix="" />
2372
<xacro:fanuc_lrmate200id_ros2_control
2473
name="fanuc_robot"

‎src/fanuc_sim/description/scene.xml‎

Lines changed: 49 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,9 @@
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" />
6+
<include file="tool.xml" />
67

78
<asset>
89
<!-- Define textures and materials -->
@@ -82,6 +83,7 @@
8283
<!-- Define the block with AprilTags on each face -->
8384
<body name="block1" pos="0.2 0.75 0.575">
8485
<freejoint name="block1" />
86+
<site name="block1" pos="0 0 0" />
8587
<geom
8688
type="box"
8789
size="0.025 0.025 0.025"
@@ -91,11 +93,13 @@
9193
</body>
9294
<body name="block2" pos="0.0 0.75 0.575">
9395
<freejoint name="block2" />
96+
<site name="block2" pos="0 0 0" />
9497
<geom class="visual" mesh="cube" pos="0 0 -0.03" />
9598
<geom class="collision" type="box" size="0.025 0.025 0.025" pos="0 0 0" />
9699
</body>
97100
<body name="block3" pos="-0.2 0.75 0.575">
98101
<freejoint name="block3" />
102+
<site name="block3" pos="0 0 0" />
99103
<geom
100104
type="box"
101105
size="0.025 0.025 0.025"
@@ -106,19 +110,58 @@
106110
<!-- Add a scene camera -->
107111
<site
108112
name="scene_camera_optical_frame"
109-
pos="0.0 1.6 1.6"
110-
euler="2.2415 0 3.1415"
113+
pos="1.1 1.1 1.5"
114+
euler="2.6 -0.8 -2.14"
111115
/>
112116
<camera
113117
name="scene_camera"
114-
pos="0.0 1.6 1.6"
118+
pos="1.1 1.1 1.5"
115119
fovy="58"
116120
mode="fixed"
117121
resolution="640 480"
118-
euler="-0.9 0 3.1415"
122+
euler="-0.54 0.8 2.14"
119123
/>
120-
<body name="arm_mount">
124+
<body name="arm_pedestal" pos="0.0 0.3 0.15">
125+
<geom type="box" size="0.2 0.2 0.15" />
126+
<geom type="box" size="0.06 0.01 0.01" pos="0.23 0.03 0.1" />
127+
<geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.03 0.1" />
128+
<site name="tool_holder_site" pos="0.25 0 0.15" euler="3.1415 0 0" />
129+
</body>
130+
<body name="arm_mount" pos="0.0 0.3 0.3">
121131
<include file="lrmate200id/lrmate200id.xml" />
122132
</body>
123133
</worldbody>
134+
135+
<!-- Define weld constraints between the robot flange (tool0) and the gripper, and between the suction cup (tool_tip)
136+
and the blocks in the scene -->
137+
<equality>
138+
<weld
139+
name="tool_attachment"
140+
body1="tool0"
141+
body2="gripper_base"
142+
active="false"
143+
torquescale="1"
144+
/>
145+
<weld
146+
name="suction_cup_block1"
147+
body1="tool_tip"
148+
body2="block1"
149+
active="false"
150+
torquescale="1"
151+
/>
152+
<weld
153+
name="suction_cup_block2"
154+
body1="tool_tip"
155+
body2="block2"
156+
active="false"
157+
torquescale="1"
158+
/>
159+
<weld
160+
name="suction_cup_block3"
161+
body1="tool_tip"
162+
body2="block3"
163+
active="false"
164+
torquescale="1"
165+
/>
166+
</equality>
124167
</mujoco>

‎src/fanuc_sim/description/tool.urdf‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
<?xml version="1.0" ?>
2+
<robot name="fanuc_tool" xmlns:xacro="http://wiki.ros.org/xacro">
3+
<link name="gripper_base">
4+
<visual>
5+
<origin rpy="0 0 0" xyz="0 0 0.02" />
6+
<geometry>
7+
<box size="0.06 0.06 0.04"/>
8+
</geometry>
9+
<material name="darkgrey">
10+
<color rgba="0.1 0.1 0.1 1"/>
11+
</material>
12+
</visual>
13+
<collision>
14+
<origin rpy="0 0 0" xyz="0 0 0.02" />
15+
<geometry>
16+
<box size="0.06 0.06 0.04"/>
17+
</geometry>
18+
<material name="darkgrey"/>
19+
</collision>
20+
</link>
21+
<link name="suction_cylinder">
22+
<visual>
23+
<origin rpy="0 0 0" xyz="0 0 0.03" />
24+
<geometry>
25+
<box size="0.01 0.01 0.06"/>
26+
</geometry>
27+
<material name="darkgrey"/>
28+
</visual>
29+
<collision>
30+
<origin rpy="0 0 0" xyz="0 0 0.03" />
31+
<geometry>
32+
<box size="0.01 0.01 0.06"/>
33+
</geometry>
34+
<material name="darkgrey"/>
35+
</collision>
36+
</link>
37+
<joint name="base_joint" type="fixed">
38+
<parent link="gripper_base" />
39+
<child link="suction_cylinder" />
40+
<origin rpy="0 0 0" xyz="0 0 0.04" />
41+
</joint>
42+
</robot>

‎src/fanuc_sim/description/tool.xml‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
<mujoco model="fanuc_tool">
2+
<compiler angle="radian" autolimits="true" />
3+
<option integrator="RK4" timestep="0.005" />
4+
5+
<worldbody>
6+
<body
7+
name="gripper_base"
8+
gravcomp="1"
9+
pos="0.25 0.3 0.3"
10+
euler="3.1415 0 0"
11+
>
12+
<joint
13+
type="free"
14+
stiffness="0"
15+
damping="0"
16+
frictionloss=".001"
17+
armature="0"
18+
/>
19+
<site name="tool_attach_site" pos="0 0 0" euler="0 0 0" />
20+
21+
<geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1" />
22+
<body name="suction_cylinder" gravcomp="1" pos="0 0 0.04">
23+
<geom
24+
type="cylinder"
25+
size="0.01 0.03"
26+
pos="0 0 0.03"
27+
margin="0.01"
28+
gap="0.01"
29+
rgba=".1 .1 .1 1"
30+
/>
31+
<body name="tool_tip" gravcomp="1" pos="0 0 0.055">
32+
<geom
33+
class="collision"
34+
type="cylinder"
35+
size="0.01 0.005"
36+
margin="0.01"
37+
gap="0.01"
38+
/>
39+
<site name="suction_cup_tool_tip" />
40+
</body>
41+
</body>
42+
</body>
43+
</worldbody>
44+
</mujoco>

‎src/fanuc_sim/objectives/activate_vacuum.xml‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Activate Vacuum">
3+
<BehaviorTree
4+
ID="Activate Vacuum"
5+
_description="Activate the vacuum gripper"
6+
_subtreeOnly="false"
7+
>
8+
<Control ID="Sequence" name="root">
9+
<Action
10+
ID="MoveGripperAction"
11+
position="1"
12+
gripper_command_action_name="/suction_cup_controller/gripper_cmd"
13+
/>
14+
</Control>
15+
</BehaviorTree>
16+
<TreeNodesModel>
17+
<SubTree ID="Activate Vacuum" />
18+
</TreeNodesModel>
19+
</root>

‎src/fanuc_sim/objectives/close_gripper.xml‎

Lines changed: 0 additions & 7 deletions
This file was deleted.

0 commit comments

Comments
 (0)