2 Finger Gripper(85) on a Universal Robot 5

antbiv 2018-03-12

I need to have in ROS Gazebo and MoveIt an UR5 robot who has the Robotiq 2 Finger Gripper(85) in the simulation models. At the moment all I have is the UR5 model only for simulations. How do I add the gripper to this model

carlosjoserg 2017-05-16

Hi @antbiv

So far, I haven't seen any custom implementation of the 2 Finger Gripper in Gazebo, and moreover, in the ROS-i repos, I couldn't find and URDF of the new gripper model 2f-85. So I created one that you can find it in our fork&branch: https://github.com/beta-robots/robotiq/tree/new_2_finger_models/robotiq_2f_model (The idea is to contribute it back to the ROS-i repos after I test it more).

If you don't need the adaptivity-mode of the gripper in simulation (which is not easy, btw), then you can use the gazebo_ros_control plugin. The plugin works better with one single instance of an URDF. So my suggestion is for you to assembly one single URDF with UR5+Robotiq 2F-85, and then add the plugin to that. Something like:

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="assembled_robot">

	<!-- THE ROBOT -->
	<link name="world" />

	<!-- UR from ROS-i repos -->
	<xacro:include filename="$(find ur_description)/urdf/ur10.urdf.xacro" />
	<joint name="world_joint" type="fixed">
		<parent link="world" />
		<child link = "simple_arm_base_link" />
		<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
	</joint>
	<xacro:ur10_robot prefix="simple_arm_" joint_limited="false" precise_collision="false"/>

	<!-- Robotiq from Beta Robots fork-->
	<xacro:include filename="$(find robotiq_2f_model)/model/robotiq_2f_85.urdf.xacro" />
	<xacro:robotiq_2f_85 name="simple_gripper" parent="simple_arm_tool0" precise_collision="false" adaptive_transmission="false">
		<origin xyz="0 0 0" rpy="0 0 1.5708" />
	</xacro:robotiq_2f_85>

       <!-- Plugin -->
	<plugin name="ros_control" filename="libgazebo_ros_control.so" />

</robot>

Then you need to configure, load and start any compatible ros-controller, as you'd normally do with a real robot or UR5 you say you have. If you need further details on how to do that, let me know.

Best,
Carlos

Wonchul 2017-08-10

Thanks for your advice!
However, since I am a beginner.... Could you explain more please?

I downloaded robotiq_2f_85 model and tried to assembly it to the wrist_3_link of the UR3....
But, failed....

When i attached precise_collision="false" like the below:
<xacro:ur3_robot prefix="" joint_limited="false" precise_collision="false" />

Invalid parameter "precise_collision"
XacroException('Invalid parameter "precise_collision"',)

THis error came up, and i got this error when i added only simple_arm_ to the name of ur3_robot:
<xacro:ur3_robot prefix="simple_arm_" joint_limited="false" />
Error: Failed to build tree: child link [base_link] of joint [world_joint] not found
at line 226 in /build/urdfdom-UJ3kd6/urdfdom-0.4.1/urdf_parser/src/model.cpp
Error [parser_urdf.cc:3408] Unable to call parseURDF on robot model
Error [parser.cc:340] parse as old deprecated model file failed.

When i added this

	<xacro:robotiq_2f_85 name="simple_gripper" parent="simple_arm_tool0" precise_collision="false" adaptive_transmission="false">

I got error about 'can't find simple_arm_tool0'

Wonchul 2017-08-11

I followed your advice and,,, got this error:
[ERROR] [1502413570.173243633, 0.306000000]: This robot has a joint named "simple_gripper_right_driver_joint" which is not in the gazebo model.
[FATAL] [1502413570.174087383, 0.306000000]: Could not initialize robot simulation interface

carlosjoserg 2017-08-11

Hi @Wonchul

Let me try to answer point by point.

When i attached precise_collision="false" like the below:
<xacro:ur3_robot prefix="" joint_limited="false" precise_collision="false" />
Invalid parameter "precise_collision"
XacroException('Invalid parameter "precise_collision"',)

The universal robot package in ros-industrial does not have this parameter. I added it because I think that the geometry of these robots can be fairly well approximated using cylinders, which increases considerably the speed for collision checking within motion planning, instead of the convex hull of the real mesh. I did this for the UR10 model only though, because it is the one I was using, you can check what this parameter does within the URDF model here.

Anyway, you can omit this parameter, if time for motion planning is not an issue for you.

<xacro:robotiq_2f_85 name="simple_gripper" parent="simple_arm_tool0" precise_collision="false" adaptive_transmission="false">

I got error about 'can't find simple_arm_tool0'

The parent depends on what name you used in the ur3_robot prefix parameter. That link exist in the UR3 model, as you can see here.

[ERROR] [1502413570.173243633, 0.306000000]: This robot has a joint named "simple_gripper_right_driver_joint" which is not in the gazebo model.
[FATAL] [1502413570.174087383, 0.306000000]: Could not initialize robot simulation interface

Now that looks like an issue. Is there any place where I can see you launch file completely? Are you spawning the assembled URDF into gazebo? You need to have something like this for instance (note that in this line, the URDF loaded in the robot_description parameter is converted to SDF on-the-fly and pushed to the gazebo world)

<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg robot_name)"  respawn="false" output="screen" />

Wonchul 2017-08-14

Thank you for your reply!!!

Since i am real beginner of ROS, I just downloaded your codes(universal robots and robotiq) and tried 'catkin_make'.

However, I got this error:
-- ==> add_subdirectory(robotiq_85_gripper-master/robotiq_85_moveit_config)
CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
MODBUS_INCLUDE_DIR (ADVANCED)
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
   used as include directory in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb
MODBUS_LIBRARIES (ADVANCED)
    linked by target "robotiq_2f_hw_usb_node" in directory /home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_2f_hw_usb

-- Configuring incomplete, errors occurred!
See also "/home/icsl/catkin_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/icsl/catkin_ws/build/CMakeFiles/CMakeError.log".
Makefile:6954: recipe for target 'cmake_check_build_system' failed
make: *** [cmake_check_build_system] Error 1
Invoking "make cmake_check_build_system" failed
icsl@icsl:~/catkin_ws$

carofide 2017-08-16

Hi @Wonchul,
I am doing the same things you mention in your post, but with a different robot. I was able to install the package. I also had the same problem as you. The solution of that problem is here http://dof.robotiq.com/discussion/comment/2838/#Comment_2838

Wonchul 2017-08-17

Thanks for your advice.

However, i got this error:
/home/icsl/catkin_ws/src/robotiq-new_2_finger_models/robotiq_ethercat/src/ethercat_manager.cpp:10:26: fatal error: ethercattype.h: No such file or directory
compilation terminated.
[ 5%] Built target diagnostic_msgs_generate_messages_cpp
robotiq-new_2_finger_models/robotiq_ethercat/CMakeFiles/robotiq_ethercat.dir/build.make:62: recipe for target 'robotiq-new_2_finger_models/robotiq_ethercat/CMakeFiles/robotiq_ethercat.dir/src/ethercat_manager.cpp.o' failed
make[2]: *** [robotiq-new_2_finger_models/robotiq_ethercat/CMakeFiles/robotiq_ethercat.dir/src/ethercat_manager.cpp.o] Error 1
CMakeFiles/Makefile2:5503: recipe for target 'robotiq-new_2_finger_models/robotiq_ethercat/CMakeFiles/robotiq_ethercat.dir/all' failed
make[1]: *** [robotiq-new_2_finger_models/robotiq_ethercat/CMakeFiles/robotiq_ethercat.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 5%] Built target diagnostic_msgs_generate_messages_nodejs
[ 5%] Built target diagnostic_msgs_generate_messages_py
[ 5%] Built target camera_test
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2

Even after i installed soem by 'sudo apt-get install ros-kinetic-soem'

I googled it,,, but there was no solution...

Could you help me ???

carofide 2017-08-17

Hi @Wonchul
The solution of that problem is mentioned here
What I did was to make the following modification in this CMakeLists.txt
1. Change ${soem_INCLUDE_DIRS} by ${soem_INCLUDE_DIRS}/soem

With that, the software compiles.

an_10 2017-08-17

Hey @carlosjoserg In the first post you mentioned you can share more details regarding the controllers for the gripper plus arm in gazebo. Please can you elaborate on that. I am using a UR10 with the robotiq_s_model gripper.

an_10 2017-08-18

I currently have included the following the launch file:
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>

<rosparam ns="/robot" file="$(find ur_gazebo)/controller/arm_controller_ur10.yaml" command="load"/>
<node pkg="robot_state_publisher" type="state_publisher" name="robotiq_hands_l_hand_robot_state_publisher"><param name="publish_frequency" type="double" value="50.0"/><remap from="joint_states" to="/robotiq_hands/left_hand/joint_states"/></node>

<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/robot" args="arm_controller"/>

However from the rqt_graph I can see that gazebo does not subscribe to /robot_state_publisher or /robotiq_hands/left_hand/joint_states

Please can you help

Wonchul 2017-08-18

Thank you!@carlosjoserg

I could compile successfully.

I have one more question.

In robotiq-new-2-finger-models which carlosjoserg shared in github,
there is c, c2, and s_model.... what is difference?????

Also, could you share any example such as how to control the gripper?

carlosjoserg 2017-08-18

Hi @an_10

Is your repo public anywhere so I can take a look? Usually there is a lot of confusion about namespaces of the controller manager, controller configurations and the gazebo_ros_control plugin.

Best

an_10 2017-08-18

Hi @carlosjoserg, my repo is not public. But I can share details here.
The launch file currently looks like this:
<?xml version="1.0" encoding="utf-8"?>
<launch>

    <arg name="gazebo" default="true"/>
    <arg name="limited" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="gui" default="true"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="load_robot_description" default="true"/>

      <param if="$(arg load_robot_description)" name="robot_description"
      command="$(find xacro)/xacro --inorder $(find bin_clearing_simulation_ur)/urdf/ur_test.urdf "/>
        <include file="$(find gazebo_ros)/launch/empty_world.launch">
            <arg name="world_name" value="worlds/empty.world"/>
            <arg name="gui" value="true"/>
            <arg name="paused" value="$(arg paused)"/>
            <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        </include>

      <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param 'robot_description' -model robot -z 0.1" respawn="false" output="screen" />
      <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>

      <rosparam ns="/robot" file="$(find ur_gazebo)/controller/arm_controller_ur10.yaml" command="load"/>
      <node pkg="robot_state_publisher" type="state_publisher" name="robotiq_hands_l_hand_robot_state_publisher"><param name="publish_frequency" type="double" value="50.0"/><remap from="joint_states" to="/robotiq_hands/left_hand/joint_states"/></node>

      <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/robot" args="arm_controller"/>

</launch>

--------------------------------------------------ur_test.urdf----------------------------------------

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="assembled_robot">

    
    <link name="world" />

    
    <xacro:include filename="$(find ur_description)/urdf/ur10.urdf.xacro" />
    <joint name="world_joint" type="fixed">
        <parent link="world" />
        <child link = "base_link" />
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
    </joint>
    <xacro:ur10_robot prefix="" joint_limited="false" />

    
    <xacro:include filename="$(find robotiq_s_model_visualization)/cfg/robotiq_hand_macro.urdf.xacro" />
    <xacro:robotiq_hand prefix="simple_gripper" parent="tool0" reflect="" >
        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
    </xacro:robotiq_hand>

       
    <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
        <robotNamespace>/robot</robotNamespace>
    </plugin>
    </gazebo>

</robot>
-----------------------------------------------------
rosservice list | grep controller_manager on terminal gives me this:
/robot/controller_manager/list_controller_types
/robot/controller_manager/list_controllers
/robot/controller_manager/load_controller
/robot/controller_manager/reload_controller_libraries
/robot/controller_manager/switch_controller
/robot/controller_manager/unload_controller

-----------------------------------------------------------
So my arm controller has topic /robot/arm_controller/command, which is subscribed by gazebo. But for the robotiq gripper /robotiq_hands/left_hand/joint_states this topic is not subscibed by gazebo. Main issue is how to control the gripper?

an_10 2017-08-18

I have included the following in my ur_test.urdf file:
    <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
        <robotNamespace>/robot</robotNamespace>
    </plugin>
    <plugin name="RobotiqHandPlugin" filename="libRobotiqHandPlugin.so">
    </plugin>
    </gazebo>

an_10 2017-08-18

When I try yo visualize in rqt, it does not show the gripper controller. It only shows the arm controller till wrist_3

philwall3 2017-11-03

So I got the same error as @Wonchul in post #4.
I'm trying to spawn the UR5 with the 85 robotiq_2f_gripper in gazebo.

phil@phil-VirtualBox:~$ roslaunch myrobot_gazebo myrobot.launch
... logging to /home/phil/.ros/log/803f8d72-c09b-11e7-97bf-080027c0668a/roslaunch-phil-VirtualBox-8299.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://phil-VirtualBox:36158/

SUMMARY
========

PARAMETERS
 * /robot_description: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    spawn_urdf (gazebo_ros/spawn_model)

auto-starting new master
process[master]: started with pid [8314]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 803f8d72-c09b-11e7-97bf-080027c0668a
process[rosout-1]: started with pid [8327]
started core service [/rosout]
process[gazebo-2]: started with pid [8337]
process[gazebo_gui-3]: started with pid [8346]
process[spawn_urdf-4]: started with pid [8349]
Gazebo multi-robot simulator, version 2.2.3
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Gazebo multi-robot simulator, version 2.2.3
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Msg Waiting for master.[ INFO] [1509715977.477475783]: Finished loading Gazebo ROS API Plugin.
Msg Waiting for master
[ INFO] [1509715977.479278547]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 10.0.2.15
spawn_model script started
[ INFO] [1509715977.851876552, 0.023000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [WallTime: 1509715977.864693] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1509715977.869258] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1509715977.872528] [0.000000] Calling service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1509715978.023507] [0.128000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1509715978.064116405, 0.128000000]: Physics dynamic reconfigure ready.
[ INFO] [1509715978.200640016, 0.128000000]: Loading gazebo_ros_control plugin
[ INFO] [1509715978.200757322, 0.128000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1509715978.201972037, 0.128000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[spawn_urdf-4] process has finished cleanly
log file: /home/phil/.ros/log/803f8d72-c09b-11e7-97bf-080027c0668a/spawn_urdf-4*.log

Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 10.0.2.15
[ERROR] [1509715978.376213190, 0.128000000]: This robot has a joint named "simple_gripper_right_driver_joint" which is not in the gazebo model.
[FATAL] [1509715978.376320815, 0.128000000]: Could not initialize robot simulation interface
Error [Param.cc:181] Unable to set value [1,0471975511965976] for key[horizontal_fov]
Error [Param.cc:181] Unable to set value [0,100000001] for key[near]

And it looks like this:

So far I have not been able to identify the problem. So any help is appreciated, from @carlosjoserg or anyone that has an idea. Maybe you found a solution, @Wonchul ?

This is the myrobot.launch:

<launch>
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find myrobot_gazebo)/worlds/myrobot.world"/>
    <arg name="paused" value="false"/>    
    <!-- more default parameters can be changed here -->
  </include>

  <!-- Convert an xacro and put on parameter server -->
  <param name="robot_description" command="$(find xacro)/xacro.py $(find myrobot_description)/urdf/join_macro.xacro" />

  <!-- Spawn a robot into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model myrobot" respawn="false" output="screen" />
</launch>

And this is the join_macro.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro"
       name="ur5" >

	<!-- THE ROBOT -->
	<link name="world" />

	<!-- UR from ROS-i repos -->
	<xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
	<joint name="world_joint" type="fixed">
		<parent link="world" />
		<child link = "simple_arm_base_link" />
		<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
	</joint>
	<xacro:ur5_robot prefix="simple_arm_" joint_limited="false"/>

	<!-- Robotiq from Beta Robots fork-->
	<xacro:include filename="$(find robotiq_2f_model)/model/robotiq_2f_85.urdf.xacro" />
	<xacro:robotiq_2f_85 name="simple_gripper" parent="simple_arm_tool0" precise_collision="false" adaptive_transmission="false" with_pads="true">
		<origin xyz="0 0 0" rpy="0 0 1.5708" />
	</xacro:robotiq_2f_85>

	<gazebo>
	  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">	   
	  </plugin>
	</gazebo>

 
</robot>

philwall3 2017-11-03

Ok I have at least a partial solution.
One problem are the inertial elements in robotiq_2f_85.xacro which are commented out. Gazebo needs that information. I commented them in and additionally commented the 'cylinder_inertia' in.

This is how the beginning looks like now with the 'cylinder_inertia' commented in:





<robot xmlns:xacro="http://ros.org/wiki/xacro">

	<xacro:macro name="cylinder_inertia" params="radius length mass">
		<inertia ixx="${mass * (3 * radius * radius + length * length) / 12}" 
			 iyy="${mass * radius* radius / 2}" 
			 izz="${mass * (3 * radius * radius + length * length) / 12}" 
			 ixy="0" 
			 iyz="0" 
			 ixz="0"/>
	</xacro:macro>

	<xacro:macro name="robotiq_2f_85" params="parent name precise_collision:=false adaptive_transmission:=false with_pads:=true *origin">
[...snip...]

It now looks like this in gazebo:

So the gripper body is still missing as far as I can see.

carlosjoserg 2017-11-04

Hi @philwall3

In Gazebo you need to have a collision and inertial paremeters defined, otherwise the spawning does nothing. You need to be careful with that cylinder inertial macro, since it assumes some axes orientation, in general, the smaller value goes along the cylinder axis. You can also double check by visualizing the inertia "cube" in Gazebo, check on the upper menus for that option, it will certainly help debugging good inertia values.

I didn't actually finished the gazeboing part of the model, and in fact, I have received more than one question about it, so I'll try to dedicate some time in the next weeks to finish the model properly. In the meantime, please, let me know your advances, if you have a public github repo, that'd be nice to have at hand as well.

Best,
Carlos

philwall3 2017-11-06

Thank you very much, that would be awesome!
In the meantime: Can you point me to where I need to look to get the body/base of the gripper to show up? I haven't had much time to look at it closely, but the base seems to have the necessary inertial parameters.

Right now I don't have a public repo, but if I make more changes I'll consider it.

carlosjoserg 2017-11-08

Alright. The base link inertial properties are here commented.

philwall3 2017-11-09

Yeah that's what I already had commented in, still not showing up.

philwall3 2017-11-10

I also have that weird triangle showing up sometimes without me changing anything. Restarting the launch file a few times usually fixes it.

I've also created a repo for the gripper now. https://github.com/philwall3/robotiq

rachillesf 2018-03-10

I'm exactly at the same point that the previous post of @philwall3.

The base is not showing up and the joints seems disconected, if a start gazebo and whait a few seconds this is how the end griper is:

I suspect that is something related to the joints, given that if I set all the joints as fixed, instead of revolute, the problem does not happen.

I have no idea about how to make the base appear as well, perhaps @carlosjoserg can help please.

I'm attaching the files for my assembled robot, the modified hand (with uncommented inertial properties) and my gazebo launch.

(I also have the triangle appearing, similar to @philwall3)

Thank you all in advance.

philwall3 2018-03-15

Hi @rachillesf ,
I couldn't solve that problem so I eventually just used the StanleyInnovation URDF model and used it with the ros_control controller from @carlosjoserg for the real gripper.
https://github.com/philwall3/UR5-with-Robotiq-Gripper-and-Kinect

malwaru 2021-11-09

If anyone is still struggling with this you can find a complete working solution in my github. Look at the repos (https://github.com/malwaru/robot_bringups) and (https://github.com/malwaru/robot_descriptions) . I used the robotiq gripper description from @philwall3 . Use the "https://github.com/malwaru/moveit_configs" to run the robot on rviz