Skip to content

Setup the fanuc_sim config with a basic tool-changing example #31

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

Merged
marioprats merged 2 commits into from
Jan 20, 2025
Merged

Setup the fanuc_sim config with a basic tool-changing example #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d0db041
Setup the fanuc_sim config with a tool-changing example
marioprats Dec 3, 2024
5003d71
improve tool changing demo
marioprats Jan 16, 2025
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
3 changes: 3 additions & 0 deletions src/fanuc_sim/config/config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ hardware:
urdf_params:
# Use "mock", "mujoco", or "real"
- hardware_interface: "mujoco"
- mujoco_viewer: "false"

# Sets ROS global params for launch.
# [Optional]
Expand Down Expand Up @@ -112,6 +113,8 @@ ros2_control:
- "joint_trajectory_controller"
- "servo_controller"
- "joint_state_broadcaster"
- "tool_attach_controller"
- "suction_cup_controller"
# - "servo_controller"
# Load but do not start these controllers so they can be activated later if needed.
# [Optional, default=[]]
Expand Down
23 changes: 23 additions & 0 deletions src/fanuc_sim/config/control/picknik_fanuc.ros2_control.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,29 @@ controller_manager:
type: joint_trajectory_controller/JointTrajectoryController
servo_controller:
type: joint_trajectory_controller/JointTrajectoryController
# A controller to enable/disable the tool attachment interface at the robot flange.
# In sim, this is modeled as a weld constraint in Mujoco.
# In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange.
tool_attach_controller:
type: position_controllers/GripperActionController
# A controller to enable/disable the suction cup.
# In sim, this is modeled as a weld constraint in Mujoco.
# In a real robot, this controller would activate / deactivate a suction cup mechanism.
suction_cup_controller:
type: position_controllers/GripperActionController


tool_attach_controller:
ros__parameters:
default: true
joint: suction_cup_tool_interface
allow_stalling: true

suction_cup_controller:
ros__parameters:
default: true
joint: suction_cup_tool_tip
allow_stalling: true

joint_state_broadcaster:
ros__parameters:
Expand Down
1 change: 1 addition & 0 deletions src/fanuc_sim/config/moveit/picknik_fanuc.srdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<end_effector name="fanuc_ee" parent_link="tool0" group="gripper"/>

<!--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. -->
<disable_collisions link1="arm_pedestal" link2="base_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="link_2" reason="Never"/>
<disable_collisions link1="base_link" link2="link_3" reason="Never"/>
Expand Down
2 changes: 2 additions & 0 deletions src/fanuc_sim/description/fanuc_lrmate200id.ros2_control.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
params="name initial_positions_file hardware_interface mujoco_model"
>
<xacro:arg name="mujoco_model" default="description/scene.xml" />
<xacro:arg name="mujoco_viewer" default="false" />
<xacro:arg name="hardware_interface" default="mock" />
<xacro:property
name="initial_positions"
Expand All @@ -26,6 +27,7 @@
<plugin>picknik_mujoco_ros/MujocoSystem</plugin>
<param name="mujoco_model">$(arg mujoco_model)</param>
<param name="mujoco_model_package">fanuc_sim</param>
<param name="mujoco_viewer">$(arg mujoco_viewer)</param>
<param name="render_publish_rate">10</param>
<param name="tf_publish_rate">60</param>
<param name="lidar_publish_rate">10</param>
Expand Down
55 changes: 52 additions & 3 deletions src/fanuc_sim/description/picknik_fanuc.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,61 @@
filename="$(find fanuc_sim)/description/fanuc_lrmate200id.ros2_control.xacro"
/>

<link name="world" />
<joint name="world_joint" type="fixed">
<link name="world"/>
<link name="arm_pedestal">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.4 0.4 0.3" />
</geometry>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.23 0.03 0.1" />
<geometry>
<box size="0.12 0.02 0.02" />
</geometry>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.23 -0.03 0.1" />
<geometry>
<box size="0.12 0.02 0.02" />
</geometry>
</visual>
</link>
<joint name="scene_joint" type="fixed">
<parent link="world" />
<child link="arm_pedestal" />
<origin rpy="0 0 0" xyz="0 0.3 0.15" />
</joint>
<joint name="base_joint" type="fixed">
<parent link="arm_pedestal" />
<child link="base_link" />
<origin rpy="0 0 0" xyz="0 0 0" />
<origin rpy="0 0 0" xyz="0 0 0.15" />
</joint>

<link name="table">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="1 1 0.05" />
</geometry>
<material name="brown">
<color rgba="0.3 0.15 0.05 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="1 1 0.05" />
</geometry>
</collision>
</link>
<joint name="table_joint" type="fixed">
<parent link="world" />
<child link="table" />
<origin rpy="0 0 0" xyz="0 1 0.45" />
</joint>

<xacro:fanuc_lrmate200id prefix="" />
<xacro:fanuc_lrmate200id_ros2_control
name="fanuc_robot"
Expand Down
55 changes: 49 additions & 6 deletions src/fanuc_sim/description/scene.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
<!-- UR5e inspired from https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e -->
<mujoco model="fanuc scene">
<compiler angle="radian" autolimits="true" />
<option integrator="RK4" timestep="0.005" />
<option integrator="implicit" />
<include file="lrmate200id/lrmate200id_globals.xml" />
<include file="tool.xml" />
Copy link
Member

@nbbrooks nbbrooks Dec 30, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

lrmate200id_globals.xml needs some KP and other tuning. I'll pull some in from https://github.com/PickNikRobotics/picknik_accessories/tree/pr-fanuc-workshop-assets

Right now it is overshooting and hitting the tool during docking very aggressively.

Copy link
Author

@marioprats marioprats Jan 16, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated the gains in this PR.
Not super perfect but should behave much better. I'm able to run the same objective over and over again.

Copy link
Member

@nbbrooks nbbrooks Jan 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is way better! I can run it successfully reliably as well.


<asset>
<!-- Define textures and materials -->
Expand Down Expand Up @@ -82,6 +83,7 @@
<!-- Define the block with AprilTags on each face -->
<body name="block1" pos="0.2 0.75 0.575">
<freejoint name="block1" />
<site name="block1" pos="0 0 0" />
Copy link
Collaborator

@MikeWrock MikeWrock Dec 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

are these sites necessary for the weld constraint to activate?

Copy link
Member

@nbbrooks nbbrooks Dec 30, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes

<geom
type="box"
size="0.025 0.025 0.025"
Expand All @@ -91,11 +93,13 @@
</body>
<body name="block2" pos="0.0 0.75 0.575">
<freejoint name="block2" />
<site name="block2" pos="0 0 0" />
<geom class="visual" mesh="cube" pos="0 0 -0.03" />
<geom class="collision" type="box" size="0.025 0.025 0.025" pos="0 0 0" />
</body>
<body name="block3" pos="-0.2 0.75 0.575">
<freejoint name="block3" />
<site name="block3" pos="0 0 0" />
<geom
type="box"
size="0.025 0.025 0.025"
Expand All @@ -106,19 +110,58 @@
<!-- Add a scene camera -->
<site
name="scene_camera_optical_frame"
pos="0.0 1.6 1.6"
euler="2.2415 0 3.1415"
pos="1.1 1.1 1.5"
euler="2.6 -0.8 -2.14"
/>
<camera
name="scene_camera"
pos="0.0 1.6 1.6"
pos="1.1 1.1 1.5"
fovy="58"
mode="fixed"
resolution="640 480"
euler="-0.9 0 3.1415"
euler="-0.54 0.8 2.14"
/>
<body name="arm_mount">
<body name="arm_pedestal" pos="0.0 0.3 0.15">
<geom type="box" size="0.2 0.2 0.15" />
<geom type="box" size="0.06 0.01 0.01" pos="0.23 0.03 0.1" />
<geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.03 0.1" />
<site name="tool_holder_site" pos="0.25 0 0.15" euler="3.1415 0 0" />
</body>
<body name="arm_mount" pos="0.0 0.3 0.3">
<include file="lrmate200id/lrmate200id.xml" />
</body>
</worldbody>

<!-- Define weld constraints between the robot flange (tool0) and the gripper, and between the suction cup (tool_tip)
and the blocks in the scene -->
<equality>
<weld
name="tool_attachment"
body1="tool0"
body2="gripper_base"
active="false"
torquescale="1"
Copy link
Member

@nbbrooks nbbrooks Dec 30, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this needs to be increased. The attached tool is sort of "floppy" right now.

Copy link
Member

@nbbrooks nbbrooks Jan 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No longer an issue!

/>
<weld
name="suction_cup_block1"
body1="tool_tip"
body2="block1"
active="false"
torquescale="1"
/>
<weld
name="suction_cup_block2"
body1="tool_tip"
body2="block2"
active="false"
torquescale="1"
/>
<weld
name="suction_cup_block3"
body1="tool_tip"
body2="block3"
active="false"
torquescale="1"
/>
</equality>
</mujoco>
42 changes: 42 additions & 0 deletions src/fanuc_sim/description/tool.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
<?xml version="1.0" ?>
<robot name="fanuc_tool" xmlns:xacro="http://wiki.ros.org/xacro">
<link name="gripper_base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.02" />
<geometry>
<box size="0.06 0.06 0.04"/>
</geometry>
<material name="darkgrey">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.02" />
<geometry>
<box size="0.06 0.06 0.04"/>
</geometry>
<material name="darkgrey"/>
</collision>
</link>
<link name="suction_cylinder">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.03" />
<geometry>
<box size="0.01 0.01 0.06"/>
</geometry>
<material name="darkgrey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.03" />
<geometry>
<box size="0.01 0.01 0.06"/>
</geometry>
<material name="darkgrey"/>
</collision>
</link>
<joint name="base_joint" type="fixed">
<parent link="gripper_base" />
<child link="suction_cylinder" />
<origin rpy="0 0 0" xyz="0 0 0.04" />
</joint>
</robot>
44 changes: 44 additions & 0 deletions src/fanuc_sim/description/tool.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
<mujoco model="fanuc_tool">
<compiler angle="radian" autolimits="true" />
<option integrator="RK4" timestep="0.005" />

<worldbody>
<body
name="gripper_base"
gravcomp="1"
pos="0.25 0.3 0.3"
euler="3.1415 0 0"
>
<joint
type="free"
stiffness="0"
damping="0"
frictionloss=".001"
armature="0"
/>
<site name="tool_attach_site" pos="0 0 0" euler="0 0 0" />

<geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1" />
<body name="suction_cylinder" gravcomp="1" pos="0 0 0.04">
<geom
type="cylinder"
size="0.01 0.03"
pos="0 0 0.03"
margin="0.01"
gap="0.01"
rgba=".1 .1 .1 1"
/>
<body name="tool_tip" gravcomp="1" pos="0 0 0.055">
<geom
class="collision"
type="cylinder"
size="0.01 0.005"
margin="0.01"
gap="0.01"
/>
<site name="suction_cup_tool_tip" />
</body>
</body>
</body>
</worldbody>
</mujoco>
19 changes: 19 additions & 0 deletions src/fanuc_sim/objectives/activate_vacuum.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Activate Vacuum">
<BehaviorTree
ID="Activate Vacuum"
_description="Activate the vacuum gripper"
_subtreeOnly="false"
>
<Control ID="Sequence" name="root">
<Action
ID="MoveGripperAction"
position="1"
gripper_command_action_name="/suction_cup_controller/gripper_cmd"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Activate Vacuum" />
</TreeNodesModel>
</root>
7 changes: 0 additions & 7 deletions src/fanuc_sim/objectives/close_gripper.xml
View file
Open in desktop

This file was deleted.

20 changes: 20 additions & 0 deletions src/fanuc_sim/objectives/deactivate_vacuum.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Deactivate Vacuum">
<!--//////////-->
<BehaviorTree
ID="Deactivate Vacuum"
_description="Deactivate the vacuum gripper"
_subtreeOnly="false"
>
<Control ID="Sequence" name="root">
<Action
ID="MoveGripperAction"
position="0"
gripper_command_action_name="/suction_cup_controller/gripper_cmd"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Deactivate Vacuum" />
</TreeNodesModel>
</root>
8 changes: 0 additions & 8 deletions src/fanuc_sim/objectives/open_gripper.xml
View file
Open in desktop

This file was deleted.

Loading
Loading