Object not getting attached to Robotiq 2 fingered gripper

[aaky]

Hello @JenniferBuehler I tried using your plugin for my setup of UR5 arm with Robotiq 2 fingered gripper for grasping cube of 50 mm length and breadth with no success on Gazebo 2 since I have ROS Indigo installed. Here is my Plugin config settings.Kindly guide me through the same

ur5 robotiq_85_left_inner_knuckle_link robotiq_85_right_knuckle_link robotiq_85_right_finger_tip_link robotiq_85_left_finger_tip_link 100 10 10 20 0.005 true __default_topic__

[JenniferBuehler]

Hi, can you send the output of the plugin please? So you're grasping the object correctly, and there is a collision with both gripper gingers and the object, but the object still slips out of the hand?

[JenniferBuehler]

I mean fingers not gingers, of course

[aaky]

Hi, above attached is screenshot of output of plugin when loaded.Yes,there was collision between object to be picked and both fingers of Robotiq gripper but still object was just slipping out when I tried to move arm in upward direction from base.

[JenniferBuehler]

Is that the output at the time when the grasp is closed? I'm asking because it should be printing something when it detects the object to be between the grippers. Do both finger tip links (*finger_tip_link_collision) links touch the object from opposing sides? Can you post a screen shot from the moment when the grasp should be attaching?

[aaky]

That's the screenshot when plugin has just initialized and even when object is colliding between 2 finger tip links of gripper, plugin dosen't print anything and object just slides away.My guess is am I going wrong somewhere in initializing plugin?means its config parameters.

[JenniferBuehler]

Yes you could play around with the parameters, for example what did you set <forces_angle_tolerance>100</forces_angle_tolerance> to? See also header documentation in GazeboGraspFix.h for documentation of the parameters.

The grasp attachment only happens if there are contact forces applied from opposing sides of the object, where opposing means the average forces have to be at an angle of more than forces_angle_tolerance. Also, the collision shape of your robot hand is what counts, not the visual shape you see in Gazebo... often the visual shape is the same as the collision shape though, but it depends on your model. Did you make sure the gripper is closed as far so that a collision with the collision shape happens, not just the visual geometry? To check whether contacts really happen, you can also visualize contacts in Gazebo (view -> contacts). If you can send me a screenshot of the robot in Gazebo, that could help too.

You could also try to get some debug prints, e.g. by uncommenting this line in GazeboGraspFix.cpp to get a hint on whether there are contacts with opposing forces at all, and then send me the output.

I wanted to make it easier to debug by setting debug flags for a while now, maybe I'll do it in the next few days, and then it will be easier to find the cause of your problem.

You can also send me a link to your repo with the model and I'll see if I can find out what's going wrong, though I can't promise I'll get around to do this before next week...

[aaky]

Hi @JenniferBuehler Sorry for late reply.I am attaching screenshot of my robot in gazebo and also output of logs after enabling debug logs as per your suggestion. I am setting force angle tolerance to 90 since it is recommended to keep above 90 degree as per your debug logs which I got after keeping it below PI/2. I am still not clear how to measure angle between opposing forces applied by my gripper on object to be picked which I think is creating problems.I enabled contacts in Gazebo view and also was able to view blue color lines appearing after contact between gripper links and object but still nothing appears on output log.

[JenniferBuehler]

Thanks for sharing this. The setup looks OK in the screenshot, and if there are contacts in Gazebo, it should be doing something - you could try increasing the angle tolerance, maybe try 120 or higher and see if it changes things, then you can gradually decrease it again.

I think the debug printing would have to be enabled to see what's going on. You would have to go into the code (as previously pointed out) to enable it - I should probably make this a parameter as well so it's easier to debug. I may do this later this week. If you would like me to have a look and fix it up, I'd need a link to the repository where you have your stuff, along with a short description where's what, if that's possible.

[aaky]

Hi @JenniferBuehler It's not possible for me to share git repo access currently maybe after certain permission I could be able to do that.I tried increasing angle tolerance from 120 and higher with no success.I have also enabled debug logs as per your instruction in previous comment but still after contact it dosent display anything.Does your plugin require some contact or collision topic to be published from Gazebo? I ensured as shown in previous screenshot that object is in contact with gripper tip links and some blue lines appear in gazebo as I try to close gripper.But before getting any debug logs object slips away after those contact are displayed in Gazebo.So basically not getting any contact logs.Here are list of parameters which I have set: Palm links : robotiq_85_left_inner_knuckle_link and robotiq_85_right_knuckle_link Gripper links : robotiq_85_right_finger_tip_link and robotiq_85_left_finger_tip_link forces_angle_tolerance : 120 update_rate : 4 grip_count_threshold : 4 max_grip_count : 8 release_tolerance : 0.005 disable_collisions_on_attach : false contact_topic : __default_topic__

robotiq_85_urdf.txt Kindly also find Robotiq URDF file for reference.

[JenniferBuehler]

Ok, that's no problem, if you can't share the code we can try to fix it up by enabling debug messages step by step. It will take a bit of work on your side to try things out, but we should get there eventually ;)

First, while Gazebo is running and your robot is touching something (or standing on the ground, so while there are contacts), do

gz topic -l

this should include the topic ~/robot/contacts in the list. This should be the gazebo topic for your contacts. If it does have it in the list, verify it gets the contacts with:

gz topic -e ~/robot/contacts

If this prints nothing, then the plugin won't get anything from Gazebo either.

If that's all fine, then, I'd like to know if the grasp plugin gets the contacts as well. Line 678 in GazeboGraspFix::OnContact() should be printing something (you have to uncomment the debug obviously), as well as line 678.

Lets see if we get an object contact, and then continue there.

[aaky]

Above attached is logs of gztopic echo /gazebo/default/robot/contacts I tried listing topics published by Gazebo and in that list I am getting topic name /gazebo/default/robot/contacts I also uncommented Line 678 of GazeboGraspFix::OnContact() still not getting anything printed on contact with object

[JenniferBuehler]

Ok, I see... if you don't get an output in OnContact (and I presume you didn't forget to recompile ;)), then that's the issue. The subscription to the topic is definitely happening, the message in line 212 is printed in your output. But the plugin is not getting any contacts at all. Obviously they apparently exist, or your gz echo command would not print anything. Can you try to rename the topic in the configuration from ~/robot/contacts to /gazebo/default/robot/contacts, thought it shouldn't make a difference... but you could try that.

Another try would be to insert a printout message in line 358 and see if OnUpdate is being called, to verify that the plugin is still running and hasn't crashed.

It's hard to tell without being able to try it myself, but I can at least tell you that the lack of correct subscription is the problem. The plugin is not getting any contact messages. The callback OnContact() should be called if the subscription to the topic was successful. Hopefully that helps you to continue debugging what's going wrong.

[JenniferBuehler]

I presume this has been resolved as there has been no further response. Please re-open if you still have questions about this!

[yt100323]

i have the same problem, the output of GazeboGraspFix::OnContact() is "the numbers of .... : 2"

[wafaesebbata]

Hello @JenniferBuehler, @akashpatil92, I have roughly the same case, i use ur5 and robotiq gripper(2-fingers), I couldn't attach the gripper to the cube for grasping in gazebo. could you explain me how to plug it.

[JenniferBuehler]

Hi @wafaezouhair , can you send the output of the plugin for the moment the robot grips the object please? Then I may be able to help you better with debugging this.

[wafaesebbata]

hi @JenniferBuehler, below attached the screenshot the output when running gazebo ant the second screenshot the output of the plugin when the robot grips the object (there was collision between object to be picked and both fingers of Robotiq gripper),

[JenniferBuehler]

In the second screenshot it shows that the object was attached, but then got detached soon after. The arm was moving according to the printed warnings about dropped trajectory points? Could it be that the motion planner avoids collisions with objects, is aware of the gripped object, and therefore doesn't grip it tight enough so that there are brief moments of non-contact where the gripper opens slightly?

The plugin releases the object as soon as there are no two opposing forces by grippers applied to the object any more for some time. The attachment itself is working for you, according to the output in the screenshot, so your robot links are configured properly for the plugin. But it looks like the criterion to drop the object is achieved too soon or too easily. You could try to adjust max_grip_count and increase release_tolerance (both described on the wiki) so the releasing doesn't happen too soon. Let me know if that helps.

[wafaesebbata]

thank you a lot Jennifer for your help, I solved the problem.

[JenniferBuehler]

Awesome, very glad to hear!

[LuserBen]

Hello @wafaezouhair , Can you tell me what you do to solve this problem, I tried several days, but it is still not take effect. Thanks.

[wafaesebbata]

hello @LuserBen , My problem was exactly that the gripper continue to close even though it attached the cube (see above attached my screenshot) until the cube slip out, then i had detaching just after an attaching . In the first i changed static cube by a dynamic and I settled the closure of the gripper. I changed the condition of the closure. but unfortunately to will grasp well the cube should be in the middle .. keep me informed of your progress on this point

[LuserBen]

Hi @wafaezouhair I don't make any progress. I grasp a 6cm cube, so I just set the gripper joint value to 0.266 when robotiq grasp the cube. The joint value will not adjust to the size of cube. Maybe I will improve it later, now I just want the gripper to work.

[JenniferBuehler]

If you have your stuff on a github repository I could also have a quick look and see if I can help

[geweihgg]

Hi, I'm confused about how to use the gazebo_grasp_plugin to fix the problem. The steps that I follows are: 1) cd src/ && git clone https://github.com/JenniferBuehler/gazebo-pkgs.git 2) cd .. && catkin make 3) add the plugin codes into the ur5_gripper_robot.urdf.xacro, so the file looks like this:

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

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="true"/>

  <!-- robotiq g85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
      <origin xyz="0 0 0" rpy="1.5708 0 0"/>
  </xacro:robotiq_85_gripper>

  <joint name="grasp_joint" type="fixed">
    <origin xyz="0.13 0 0" rpy="0 0 0"/>
    <parent link="robotiq_85_base_link"/>
    <child link="grasp_link"/>
  </joint>
  <link name="grasp_link"/>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <!--<origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />-->
    <origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />
  </joint>

  <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
     <arm>
        <arm_name>ur5_robot</arm_name>
        <palm_link> robotiq_85_left_inner_knuckle_link robotiq_85_right_knuckle_link </palm_link>
        <gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
        <gripper_link> robotiq_85_right_finger_tip_link </gripper_link>
     </arm>
     <forces_angle_tolerance>120</forces_angle_tolerance>
     <update_rate>10</update_rate>
     <grip_count_threshold>10</grip_count_threshold>
     <max_grip_count>20</max_grip_count>
     <release_tolerance>0.005</release_tolerance>
     <disable_collisions_on_attach>false</disable_collisions_on_attach>
     <contact_topic>__default_topic__</contact_topic>
  </plugin>

</robot>

4) launch the gazebo, but there seems no info about "Loading gazebo-fix plugin" in the output. Could you help me? Thank you very much~ GeWei

[JenniferBuehler]

Your set-up looks OK. So there is absolutely no "Loading grasp-fix plugin" output? Can you find libgazebo_grasp_fix.so (should be in the devel/lib directory of your catkin workspace)?

How are you "launching" the robot?

If you could send me the whole output that would be helpful.

[geweihgg]

Hi @JenniferBuehler My computer are not at hand and I will show more information here about two hours later. Thank you very much. GeWei

[geweihgg]

Hi. @JenniferBuehler , I checked the devel/lib directory just now, libgazebo_grasp_fix.so existed there. The output lists here:

moran@JustDo1t:~/ros_ws/ur5_ws/src/robotiq_85_gripper/robotiq_85_simulation/robotiq_85_gazebo/launch$ roslaunch ur_gazebo ur5_sample_joint_limited.launch 
... logging to /home/moran/.ros/log/8e98b9d2-5030-11e8-99ed-7c67a28b0b23/roslaunch-JustDo1t-17331.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://JustDo1t:44971/

SUMMARY
========

PARAMETERS
 * /arm_controller/action_monitor_rate: 10
 * /arm_controller/constraints/elbow_joint/goal: 0.1
 * /arm_controller/constraints/elbow_joint/trajectory: 0.1
 * /arm_controller/constraints/goal_time: 0.6
 * /arm_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /arm_controller/constraints/shoulder_lift_joint/trajectory: 0.1
 * /arm_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /arm_controller/constraints/shoulder_pan_joint/trajectory: 0.1
 * /arm_controller/constraints/stopped_velocity_tolerance: 0.05
 * /arm_controller/constraints/wrist_1_joint/goal: 0.1
 * /arm_controller/constraints/wrist_1_joint/trajectory: 0.1
 * /arm_controller/constraints/wrist_2_joint/goal: 0.1
 * /arm_controller/constraints/wrist_2_joint/trajectory: 0.1
 * /arm_controller/constraints/wrist_3_joint/goal: 0.1
 * /arm_controller/constraints/wrist_3_joint/trajectory: 0.1
 * /arm_controller/joints: ['shoulder_pan_jo...
 * /arm_controller/state_publish_rate: 25
 * /arm_controller/stop_trajectory_duration: 0.5
 * /arm_controller/type: position_controll...
 * /gripper/action_monitor_rate: 10
 * /gripper/constraints/goal_time: 0.6
 * /gripper/constraints/robotiq_85_left_knuckle_joint/goal: 0.1
 * /gripper/constraints/robotiq_85_left_knuckle_joint/trajectory: 0.1
 * /gripper/constraints/stopped_velocity_tolerance: 0.05
 * /gripper/joints: ['robotiq_85_left...
 * /gripper/state_publish_rate: 25
 * /gripper/stop_trajectory_duration: 0.5
 * /gripper/type: position_controll...
 * /joint_state_controller/publish_rate: 50
 * /joint_state_controller/type: joint_state_contr...
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 50.0
 * /robot_state_publisher/tf_prefix: 
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /use_sim_time: True

NODES
  /
    arm_controller_spawner (controller_manager/controller_manager)
    fake_joint_calibration (rostopic/rostopic)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    gripper_controller_spawner (controller_manager/spawner)
    joint_state_controller_spawner (controller_manager/controller_manager)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_gazebo_model (gazebo_ros/spawn_model)

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

setting /run_id to 8e98b9d2-5030-11e8-99ed-7c67a28b0b23
process[rosout-1]: started with pid [17369]
started core service [/rosout]
process[gazebo-2]: started with pid [17377]
process[gazebo_gui-3]: started with pid [17385]
process[spawn_gazebo_model-4]: started with pid [17391]
process[robot_state_publisher-5]: started with pid [17400]
process[fake_joint_calibration-6]: started with pid [17403]
process[joint_state_controller_spawner-7]: started with pid [17404]
process[arm_controller_spawner-8]: started with pid [17405]
process[gripper_controller_spawner-9]: started with pid [17407]
Gazebo multi-robot simulator, version 2.2.6
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

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

Msg Waiting for master.[ INFO] [1525503012.708585564]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1525503012.711829681]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
Msg Waiting for master
Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 192.168.19.107
spawn_model script started
[INFO] [WallTime: 1525503013.167324] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1525503013.170298] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1525503013.472633] [0.000000] Calling service /gazebo/spawn_urdf_model

Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 192.168.19.107
[ INFO] [1525503013.731677285]: Camera Plugin (robotNamespace = ), Info: the 'robotNamespace' param did not exit
[ INFO] [1525503013.734677026]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
[INFO] [WallTime: 1525503013.748304] [9977.832000] Spawn status: SpawnModel: Model pushed to spawn queue, but spawn service timed out waiting for model to appear in simulation under the name robot
[spawn_gazebo_model-4] process has finished cleanly
log file: /home/moran/.ros/log/8e98b9d2-5030-11e8-99ed-7c67a28b0b23/spawn_gazebo_model-4*.log
[ INFO] [1525503015.186383833, 9977.832000000]: Loading gazebo_ros_control plugin
[ INFO] [1525503015.186476094, 9977.832000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1525503015.187488912, 9977.832000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ INFO] [1525503015.308998034, 9977.832000000]: Loaded gazebo_ros_control.
[ INFO] [1525503015.333295849, 9977.853000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1525503015.362930168, 9977.882000000]: Physics dynamic reconfigure ready.
Loaded joint_state_controller
Started ['joint_state_controller'] successfully
Loaded arm_controller
Started ['arm_controller'] successfully
[joint_state_controller_spawner-7] process has finished cleanly
log file: /home/moran/.ros/log/8e98b9d2-5030-11e8-99ed-7c67a28b0b23/joint_state_controller_spawner-7*.log
[arm_controller_spawner-8] process has finished cleanly
log file: /home/moran/.ros/log/8e98b9d2-5030-11e8-99ed-7c67a28b0b23/arm_controller_spawner-8*.log
Warning [Publisher.cc:134] Queue limit reached for topic /gazebo/default/pose/local/info, deleting message. This warning is printed only once.

By the way, I only catkin_make the gazebo_grasp_plugin directory and excluded the gazebo_state_plugin、gazebo_test_tools、gazebo_world_plugin_loader because when I catkin_make the whole gazebo-pkgs, some error occured. I don't know whether this is ok or not. May be the gazebo_world_plugin_loader should be included? I'll try this later.

Thanks a lot. GeWei

[JenniferBuehler]

Just a thought, but are you sure the .xacro file is used, and not another .urdf that was generated out of an old version of the .xacro file?

It seems to not be loaded (otherwise it would also print an error about the library not being found I think). What if you introduce a typo into the xacro file which should definitely cause an error, like e.g. when you destroy the xml format?

[geweihgg]

Oh， I think I’ll check it this afternoon. Thank you for reminding me of this problem. GeWei

[geweihgg]

Um... When I add "hello" tag into the "ur5_gripper_robot.urdf.xacro" file, Error occurs. My code is like this:

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

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="true"/>

  <!-- robotiq g85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
      <origin xyz="0 0 0" rpy="1.5708 0 0"/>
  </xacro:robotiq_85_gripper>

  <joint name="grasp_joint" type="fixed">
    <origin xyz="0.13 0 0" rpy="0 0 0"/>
    <parent link="robotiq_85_base_link"/>
    <child link="grasp_link"/>
  </joint>
  <link name="grasp_link"/>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <!--<origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />-->
    <origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />
  </joint>

  <hello>

<!--
  <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
     <arm>
        <arm_name>ur5_robot</arm_name>
        <palm_link> robotiq_85_left_inner_knuckle_link </palm_link>
        <gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
        <palm_link> robotiq_85_right_knuckle_link </palm_link>
        <gripper_link> robotiq_85_right_finger_tip_link </gripper_link>
     </arm>
     <forces_angle_tolerance>120</forces_angle_tolerance>
     <update_rate>10</update_rate>
     <grip_count_threshold>1</grip_count_threshold>
     <max_grip_count>20</max_grip_count>
     <release_tolerance>0.005</release_tolerance>
     <disable_collisions_on_attach>true</disable_collisions_on_attach>
     <contact_topic>__default_topic__</contact_topic>
  </plugin>
-->
</robot>

So, I think the file is exactly what I launch. So the problem with my code may be that I use the plugin in a wrong way. Maybe this block I wrote is wrong?

  <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
     <arm>
        <arm_name>ur5_robot</arm_name>
        <palm_link> robotiq_85_left_inner_knuckle_link </palm_link>
        <gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
        <palm_link> robotiq_85_right_knuckle_link </palm_link>
        <gripper_link> robotiq_85_right_finger_tip_link </gripper_link>
     </arm>
     <forces_angle_tolerance>120</forces_angle_tolerance>
     <update_rate>10</update_rate>
     <grip_count_threshold>1</grip_count_threshold>
     <max_grip_count>20</max_grip_count>
     <release_tolerance>0.005</release_tolerance>
     <disable_collisions_on_attach>true</disable_collisions_on_attach>
     <contact_topic>__default_topic__</contact_topic>
  </plugin>

I'm not sure about how to write the palm_link and gripper_link. Is my usage right? Thank you. GeWei

[LuserBen]

Hi, @geweihgg I have done the same work and I am no problem. What is your problem?

[geweihgg]

Hi, @LuserBen My problem is that ,when I launch the robot file, there seems no output info about "gazebo-fix plugin", as I mentioned above. So I wonder that whether my usage is right or not. The xacro file is like this:

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

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="true"/>

  <!-- robotiq g85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
      <origin xyz="0 0 0" rpy="1.5708 0 0"/>
  </xacro:robotiq_85_gripper>

  <joint name="grasp_joint" type="fixed">
    <origin xyz="0.13 0 0" rpy="0 0 0"/>
    <parent link="robotiq_85_base_link"/>
    <child link="grasp_link"/>
  </joint>
  <link name="grasp_link"/>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <!--<origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />-->
    <origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />
  </joint>

  <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
     <arm>
        <arm_name>ur5_robot</arm_name>
        <palm_link> robotiq_85_left_inner_knuckle_link </palm_link>
        <gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
        <palm_link> robotiq_85_right_knuckle_link </palm_link>
        <gripper_link> robotiq_85_right_finger_tip_link </gripper_link>
     </arm>
     <forces_angle_tolerance>120</forces_angle_tolerance>
     <update_rate>10</update_rate>
     <grip_count_threshold>1</grip_count_threshold>
     <max_grip_count>20</max_grip_count>
     <release_tolerance>0.005</release_tolerance>
     <disable_collisions_on_attach>true</disable_collisions_on_attach>
     <contact_topic>__default_topic__</contact_topic>
  </plugin>

</robot>

Thank you for help. GeWei

[LuserBen]

I have two gauss.

1. Can't find the file libgazebo_grasp_fix.so. I just copy this file to /opt/ros/indigo/lib/, or maybe add /catkin_ws/devel/lib to enviroment path.
2. For me, I add "" into the "ur5_gripper_robot.urdf.xacro" file. ` ` and add "" into gzplugin_grasp_fix.urdf.xacro ` ur_env gripper_finger1_inner_knuckle_link gripper_finger2_inner_knuckle_link gripper_finger1_finger_tip_link gripper_finger2_finger_tip_link 100 20 1 3 0.005 false __default_topic__ ` I don't know whether it make influence.

[geweihgg]

Hi, @LuserBen I copied the libgazebo_grasp_fix.so file in to /opt/ros/indigo/lib, and changed my ur5_gripper_robot.urdf.xacro file as yours:

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="true"/>

  <!-- robotiq g85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
      <origin xyz="0 0 0" rpy="1.5708 0 0"/>
  </xacro:robotiq_85_gripper>

  <joint name="grasp_joint" type="fixed">
    <origin xyz="0.13 0 0" rpy="0 0 0"/>
    <parent link="robotiq_85_base_link"/>
    <child link="grasp_link"/>
  </joint>
  <link name="grasp_link"/>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <!--<origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />-->
    <origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />
  </joint>

  <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
     <arm>
        <arm_name>ur5_robot</arm_name>
        <palm_link> robotiq_85_left_inner_knuckle_link </palm_link>
        <palm_link> robotiq_85_right_inner_knuckle_link </palm_link>
        <gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
        <gripper_link> robotiq_85_right_finger_tip_link </gripper_link>
     </arm>
     <forces_angle_tolerance>120</forces_angle_tolerance>
     <update_rate>20</update_rate>
     <grip_count_threshold>1</grip_count_threshold>
     <max_grip_count>3</max_grip_count>
     <release_tolerance>0.005</release_tolerance>
     <disable_collisions_on_attach>false</disable_collisions_on_attach>
     <contact_topic>__default_topic__</contact_topic>
  </plugin>

</robot>

But no message about "Load gazebo-fix plugin. Do you build the whole gazebo-pkgs? I only catkin_make the "gazebo_grasp_plugin" file and "gazebo_world_plugin_loader" because when I build the whole file, some error occurs:

CMake Warning at /opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake:76 (find_package):
  Could not find a package configuration file provided by "object_msgs" with
  any of the following names:

    object_msgsConfig.cmake
    object_msgs-config.cmake

  Add the installation prefix of "object_msgs" to CMAKE_PREFIX_PATH or set
  "object_msgs_DIR" to a directory containing one of the above files.  If
  "object_msgs" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  gazebo-pkgs/gazebo_test_tools/CMakeLists.txt:7 (find_package)

-- Could not find the required component 'object_msgs'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "object_msgs" with
  any of the following names:

    object_msgsConfig.cmake
    object_msgs-config.cmake

  Add the installation prefix of "object_msgs" to CMAKE_PREFIX_PATH or set
  "object_msgs_DIR" to a directory containing one of the above files.  If
  "object_msgs" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  gazebo-pkgs/gazebo_test_tools/CMakeLists.txt:7 (find_package)

-- Configuring incomplete, errors occurred!
See also "/home/moran/ros_ws/ur5_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/moran/ros_ws/ur5_ws/build/CMakeFiles/CMakeError.log".
Invoking "cmake" failed

I don't know how to fix this. Have you encountered this problem? Thank you. GeWei

[LuserBen]

hi, i only catkin make package gazebo_grasp_plugin without others.

[geweihgg]

Hi, @LuserBen Yes, I've tried it too, but also no info about "load gazebo-fix plugin". Now, I'm sure that I launch the right gazebo file, so I guess that the plugin is not included right. If this plugin is included correctly, I think, there should be some info about the plugin as @JenniferBuehler metions.

[LuserBen]

Hi, @geweihgg I refered to this file for adding gazebo_grasp_plugin into my xacro file. `

......

` So maybe you can try again.

[geweihgg]

Hi, @LuserBen I changed my file like this:

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

<xacro:arg name="load_grasp_fix" default="true"/>

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="true"/>

  <!-- robotiq g85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
      <origin xyz="0 0 0" rpy="1.5708 0 0"/>
  </xacro:robotiq_85_gripper>

  <joint name="grasp_joint" type="fixed">
    <origin xyz="0.13 0 0" rpy="0 0 0"/>
    <parent link="robotiq_85_base_link"/>
    <child link="grasp_link"/>
  </joint>
  <link name="grasp_link"/>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <!--<origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />-->
    <origin xyz="0.0 0.0 0.68" rpy="0.0 0.0 0.0" />
  </joint>

  <xacro:if value="$(arg load_grasp_fix)">
    <xacro:include filename="$(find ur_pkg_udef)/gzplugin_grasp_fix.urdf.xacro"/>
    <xacro:gzplugin_grasp_fix/>
  </xacro:if>

</robot>

and the file gzplugin_grasp_fix.urdf.xacro:

<?xml version="1.0"?>
<gzplugin_grasp_fix xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:macro name="gzplugin_grasp_fix">
  <arm>
      <arm_name>ur5_robot</arm_name>
      <palm_link> robotiq_85_left_inner_knuckle_link </palm_link>
      <palm_link> robotiq_85_right_inner_knuckle_link </palm_link>
      <gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
      <gripper_link> robotiq_85_right_finger_tip_link </gripper_link>
  </arm>
  <forces_angle_tolerance>120</forces_angle_tolerance>
  <update_rate>20</update_rate>
  <grip_count_threshold>1</grip_count_threshold>
  <max_grip_count>3</max_grip_count>
  <release_tolerance>0.005</release_tolerance>
  <disable_collisions_on_attach>false</disable_collisions_on_attach>
  <contact_topic>__default_topic__</contact_topic>
</xacro:macro>
</gzplugin_grasp_fix>

But there is still no info about the plugin. Shouldn't it be used as "plugin" tag? Why your file have no "plugin" tag? Could you tell me the procedure you follows? Maybe I missed sth important. Thank you very much. GeWei

[LuserBen]

Hi, @geweihgg gzplugin_grasp_fix.urdf.xacro refer to this file. The file have "plugin" tag.

[geweihgg]

Hi, @LuserBen Wow, it works, the "Load gazebo-fix plugin" info appears. It's very kind of you to help me. Em...but the cube slips away from the gripper. I'll try to change the joint trajectory command of the gripper.

[geweihgg]

Hi, @JenniferBuehler Now, with @LuserBen 's help, I've loaded the plugin successfully. But the cube always slips away from the gripper. I wonder that maybe we can have two "update_rate" params for "attach" and "detach" the object seperately. Then, I can have a bigger rate for "attach", making it easier for the gripper to grasp the cube. And, a smaller update_rate for "detach", cause I can open the gripper and wait for it to detach. I'll try to do this tomorrow. Thank you. GeWei

[JenniferBuehler]

Hi GeWei, sorry for not getting back to you earlier, it's been pretty busy here. It's possible to have two update rates, however I won't have the time to implement it very soon, I'm flat out with work. Feel free to submit a PR though if you have the time to do it :)

Does it help if you increase the update rate in the first place? Does it attach better with that?

Have you tried increasing <grip_count_threshold> (it should be lower than <max_grip_count> which you then need to increase too)? Or increase the forces_angle_tolerance?

[geweihgg]

Hi, @JenniferBuehler I've tuned the parameters, and it works well. But there is a problem that, when the gripper is not exactly above the cube, having some deviations to left or right, closing the gripper will lead to the collision problem, and then the cube moves away. I don't know how to solve this, it's pretty hard for me. Sorry to interrupt you again. Thank you very much. GeWei

[JenniferBuehler]

Hi @geweihgg , glad it works now. So what you mean is that if the conditions for the gripper (i.e. opposing forces) don't apply, and the gripper collides with the cube as it otherwise would, the old problem persists? There will always be the problem that when the robot actually doesn't grasp the object (and it wouldn't grasp it in reality either because it's offf), then a normal collision happens and the old problem will be there again. What you can try to increase the likelyhood that the grasp plugin condition is met is to increase forces_angle_tolerance. Have you tried that?

[geweihgg]

Hi, @JenniferBuehler , My problem is that, when the left finger of the gripper touches the cube, and the right finger don't, the cube will move away. So we need both fingers touch the cube almost at the same time.
But in learning problems, we cannot provide the accurate position of the cube, so, this is a problem for me. It seems a little difficult for me to solve this. Thank you. GeWei

[JenniferBuehler]

Yes this problem is not solvable with the grasp plugin, it's not a situation in which the robot is meant to grasp the cube. The plugin is meant to simulate grasping which would in reality work.

Not having the accurate pose of the cube is an entirely different problem, which you should solve separately. It's more important for your real-robot application than problems in the Gazebo simulator... it's not easy, I know. Good luck with that, you'll get there! As everything in robotics, things just take time and sweat ;)

[geweihgg]

Hi, @JenniferBuehler , Yes, I'll try my best~ Thank you again. ^-^ GeWei

[williamjames88]

Hi @JenniferBuehler , may I know if I were to use this https://github.com/utecrobotics/ur5 will I be able to use your grasping plugin? Im using this github repo and everything is working fine except that the gripper is not gripping the cube. I'm suspecting the following:

• The gripper is not in contact with the object .
• Missing files such as table and cube . ( dae and stl files )
• Robotiq gripper not working ( but it's able to move , open and close gripper )

Could you please help me look into it ? Thanks !

[JenniferBuehler]

Hi @williamjames88, I haven't tried this particular hand, but in theory it should work with any. So I take it that you have tried the plugin, but the object doesn't attach? Do I understand this correctly?

Can you post a close-up screenshot of the gripper grasping the object at the time where it should attach? And also a screenshot with the collision visuals enabled (View -> Collisions)?