Object not getting attached to Robotiq 2 fingered gripper about gazebo-pkgs

[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.

[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"

[wafaezouhair]

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.

[wafaezouhair]

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.

[wafaezouhair]

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.

[wafaezouhair]

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

[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.
   <!-- gazebo grasp plugin --> <xacro:if value="$(arg load_grasp_fix)"> <xacro:include filename="$(find my_robot)/urdf/gzplugin_grasp_fix.urdf.xacro"/> <xacro:gzplugin_grasp_fix/> </xacro:if>
   and add "" into gzplugin_grasp_fix.urdf.xacro
   `
   

<xacro:macro name="gzplugin_grasp_fix">




<arm_name>ur_env</arm_name>
<palm_link>gripper_finger1_inner_knuckle_link</palm_link>
<palm_link>gripper_finger2_inner_knuckle_link</palm_link>
<gripper_link>gripper_finger1_finger_tip_link</gripper_link>
<gripper_link>gripper_finger2_finger_tip_link</gripper_link>

<forces_angle_tolerance>100</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>
`
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.
<xacro:arg name="load_grasp_fix" default="false"/> ...... <xacro:if value="$(arg load_grasp_fix)"> <xacro:include filename="$(find jaco_gazebo)/urdf/gzplugin_grasp_fix.urdf.xacro"/> <xacro:gzplugin_grasp_fix/> </xacro:if>
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)?

[LuserBen]

Hi, @JenniferBuehler .
Maybe I find a reason why sometimes object slips out of the hand for robotiq two finger gripper.

If the object is in the middle of the gripper(x = y, because I get the object state by gazebo service /get_model_state, there is no position error of the object), left finger and right finger will arrive target pose at the same time but without enough collision and counting contact points in 'close' process.
So I add a position offset (x != y), one finger will contact the object first, then another finger. It works.

As I described in #18. In matlab I get the object position by kinect v1, various errors lead to x != y. In C++ the exact position of the object can be get ( x!=y). So for me why matlab client worked, but C++ client failed.

I am not sure if it will affect.

[JenniferBuehler]

Hi,

thanks for posting this. I'm not sure I quite 100% follow what you mean. So you are saying that with your robot, if the grippers both touch the object at the same time, then it fails, but if one touches it first and then the other, then it succeeds?

Cheers
Jenny

[LuserBen]

Hi, @JenniferBuehler .
Yes.
If the gripper both touch the object at the same time, object will always slip out (not every time) without a collision process (By the way, for a 6cm×6cm×cm cube I set the robotiq joint value to 0.266, two fingers will touch the object while they are stopped at the same time).
If one touches object first and then the other, there is a violent collision process in gazebo GUI and Terminal will print “GazeboGraspFix: Attach cube ······".

So I guess it have an effect on grasping performance.

Cheers,
Lu

[JenniferBuehler]

Hi Lu,

ok that's very strange indeed. I'll re-open this so I can try to reproduce it and see what the reason for this may be. It will be a while yet before I can look into this though because I have to first finish other work.
If the repository you are using to test this is public, could you please send me a link, then it will be easier for me to verify that.

Cheers
Jenny

[LuserBen]

Hi, @JenniferBuehler .
Haha, I'm not sure if it is common for others, I just provide a solution for people who meet the same problem. So it is not an urgent matter.
I have not uploaded my repository yet, maybe after a few months, but I find a repository is similar to me, https://github.com/neka-nat/ur_ws, you can replace the object with a cube.
Thanks for your consideration.

Cheers,
Lu

[JenniferBuehler]

Hi Lu,

sounds good, thank you! I will look into this as soon as I can. Thanks for the link.

Cheers
Jenny

[caiobarrosv]

Hello, @LuserBen.

I have the same exact problem. I get the final position and orientation of the object to be grasped by a CNN and it is not very precise. As a result, one finger will touch the object first and the object slips.
How did you manage to solve this issue?

[LuserBen]

Hello， @caiobarrosv,
I just added an offset (for example, x+3mm) to the position of object so that two fingers won't touch the object at the same time. It works for me.

[rhowsane]

Hi! @JenniferBuehler

I’m facing difficulty to attach my object using your pluggin. Could you help me please?
Here are the proceedings that I adopted to investigate the problem:

1. Copy the files gazebo_grasp_pluggin, gazebo_version_helpers and general-message-pkgs to my src directory. After catkin_make

2. I inserted the parameters at panda_arm_hand.urdf.xacro to point to the gzplugin_grasp_fix.urdf.xacro

3. I had tested if the function UPDATE was being published uncommenting some messages and after doing the command ‘catkin_make’ at the repo. It was working well.

4. I tested <release_tolerance> equal to 0 (to verify if the detachement was happening quickly), to 0.005 (the initial configuration that wasn’t working) and to 0.01. The last one to garantee that even if the rightfinger could move away from the mobile 1cm, I would still have a contact. But what happened in the end was that the robot would spend more time attaching the mobile and after It would dettache it as we can see at the video.

https://gfycat.com/heftydefiantcleanerwrasse

I launched the simulation and then I got the first screenshoot. When I was verifying what was happening I took the second one. From which I realized that the leftfinger was the first to touch the mobile and also the last to lose contact.
The X spawn position of the mobile was 0.50 m and the goal position for the grasping the same.

The main question in my head is why the gripper consistently loses the contact after a certain time.

If you have any ideas of what I could do to fix my grasping, It would help me a lot.

[JenniferBuehler]

Hi @rhowsane,

Hm that sounds strange that it would take longer to attach with a higher release tolerance... Can you try a hyper exaggerated value like 1 to confirm this behaviour?

It's a bit hard to see in the video whether it attached at all, but the extract from the output suggests so, though I can't see it all...
Could you maybe post the output as text file instead of image?

If you're already going into the code enabling debug prints, that's probably the best bet to follow the lead on what's going wrong. There's a number of commented debug prints which could be helpful

[Harshal82More]

Hi @JenniferBuehler, I hope you are doing fine. I would need your help regarding some issues while grasing. I have created my own robot manipulator. I used your plugin seems to be loaded in msg. I have an object (payload) to lift but it slips from fingers. I have few questions - 1. when I use position controller it slips from fingers. I do not get any output while gz topic -e ~/gazebo/default/mrm/contacts.. 2. Some references told me to use effort controller. Some how effort controller doesn't work for fingers will. May be I need some tuning. I am getting sophisticated way to do it. Your suggestions will be helpful.

Thanks

[JenniferBuehler]

Hi :) you mean that even when the robot is touching the object, you don't get output for the contacts on the topic? Did you set up collision shapes for fingers and object?

[Harshal82More]

I have 4 fingers at EE At first I started with positioncontroller/trajectory controller. some how objects slips from finger. Your plugin seems to be in output even I have rostopic list robot/contacts. Even finger is touching object I dont get any output. When I change to effort controller/tunning for manually getting difficult.

Can you tell me which is correct approach position or effort?

[JenniferBuehler]

If you don't get output for the contacts (via gz topic) it could mean you have no collision shapes (eg only visual). It then doesn't matter if the plugin is loaded or not. Did you add them in the urdf?

[Harshal82More]

Hi Jenifer, thanks for writing. Yes I have collision already added. I just modified my world file and I have gazebo contacts topics updating. It seems to be working fine. But when I deploy object on ground it seems the contact between ground and objects displaces platform with arm. Any suggestions what I can work one to tune this? Reducing effort or something?
thanks
Harshal

[JenniferBuehler]

Hi Harshal,
Bit difficult to estimate what's happening from what you're saying, if I understand correctly, you place an object on the ground next to the robot, and then the robot moves? Try to place the object in a position in which it doesn't collide already with the ground maybe (eg 1mm above ground so that it falls on it), or make it part of the world, or make it static (if it's not meant to be moved again).
Hope something like this resolves it
Jennifer

[Harshal82More]

Thanks @JenniferBuehler this trick worked. That was the same issue what I you understood. I drop it few 2 mm above ground.

[JenniferBuehler]

Glad it worked :)

[BenRJG]

@JenniferBuehler
Hi! Sorry to be adding to an old issue (I can open a new one if it'd be preferable), but I'm having the same issue as the item not gripping and no outputs :'D
Here's a short video showing it in action. I've tried to follow this post from the top but I struggle to find anywhere to change the topic type, and the line to un-comment for terminal output has changed. Any help would be really appreciated :)

Log on load. I've also checked gazebo collision output and values are showing on collide

[Msg] Loading grasp-fix plugin
[Msg] GazeboGraspFix: Using disable_collisions_on_attach 1
[Msg] GazeboGraspFix: Using update rate 10
[Msg] GazeboGraspFix: Using max_grip_count 20
[Msg] GazeboGraspFix: Using grip_count_threshold 10
[Msg] GazeboGraspFix: Using release_tolerance 0.005
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_left_gripper_left_follower::tracebot_left_gripper_left_follower_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_left_gripper_left_follower::tracebot_left_gripper_left_follower_fixed_joint_lump__tracebot_left_gripper_left_pad_collision_1
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_left_gripper_right_follower::tracebot_left_gripper_right_follower_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_left_gripper_right_follower::tracebot_left_gripper_right_follower_fixed_joint_lump__tracebot_left_gripper_right_pad_collision_1
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_right_gripper_left_follower::tracebot_right_gripper_left_follower_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_right_gripper_left_follower::tracebot_right_gripper_left_follower_fixed_joint_lump__tracebot_right_gripper_left_pad_collision_1
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_right_gripper_right_follower::tracebot_right_gripper_right_follower_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::tracebot_right_gripper_right_follower::tracebot_right_gripper_right_follower_fixed_joint_lump__tracebot_right_gripper_right_pad_collision_1
[Msg] Subscribing contact manager to topic ~/robot/contacts
[INFO] [1637314470.329002, 0.000000]: Controller Spawner: Loaded controllers: joint_group_pos_controller
[Msg] Advertising grasping events on topic grasp_events
[INFO] [1637314470.360639, 0.000000]: Loading controller: pos_joint_traj_controller
[spawn_gazebo_model-4] process has finished cleanly
log file: /home/astech/.ros/log/023af28e-4910-11ec-8d07-bf8946c88163/spawn_gazebo_model-4*.log

Plugin config

  <!-- Gazebo Gripper -->
  <gazebo>
    <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
      <arm>
        <arm_name>tracebot_left_arm</arm_name>
        <palm_link> tracebot_left_gripper_left_spring_link </palm_link>
        <palm_link> tracebot_left_gripper_right_spring_link </palm_link>
        <gripper_link> tracebot_left_gripper_left_follower </gripper_link>
        <gripper_link> tracebot_left_gripper_right_follower </gripper_link>
        
        <!--palm_link> tracebot_left_arm_wrist_3_link  </palm_link-->
        <!--gripper_link> tracebot_left_gripper_left_driver </gripper_link-->
        <!--gripper_link> tracebot_left_gripper_right_driver </gripper_link-->
      </arm>
      <arm>
        <arm_name>tracebot_right_arm</arm_name>
        <palm_link> tracebot_right_gripper_left_spring_link </palm_link>
        <palm_link> tracebot_right_gripper_right_spring_link </palm_link>
        <gripper_link> tracebot_right_gripper_left_follower </gripper_link>
        <gripper_link> tracebot_right_gripper_right_follower </gripper_link>
        
        <!--palm_link> tracebot_right_arm_wrist_3_link  </palm_link-->
        <!--gripper_link> tracebot_right_gripper_left_driver </gripper_link-->
        <!--gripper_link> tracebot_right_gripper_right_driver </gripper_link-->
      </arm>
      <forces_angle_tolerance>100</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>true</disable_collisions_on_attach>
      <contact_topic>__default_topic__</contact_topic>
    </plugin>
  </gazebo>

[JenniferBuehler]

Hi @BenRJG,
I'll try to find some time on Monday to look at it in more detail (unfortunately won't get time before). Spontaneously the only thing that comes to mind is that collisions should definitely be occurring (from the video). Maybe it's not lasting long enough / stable enough for the plugin to catch.. though I doubt it but worth a try: Did you try to decrease the grip count threshold?

Can you send the output showing on collision that you meant?

Cheers
Jennifer

[BenRJG]

Hi @JenniferBuehler
Thanks for getting back to me :)
Looking slightly better now, I decreased the count threshold to 1 and also tried it with a rectangular shaped object to help with stability and it is attaching, just now its attaching/detaching then the object slips out

[Msg] GazeboGraspFix: Attaching unit_box::link::collision to gripper tracebot_left_arm.
[Msg] GazeboGraspFix: Detaching unit_box::link::collision from gripper tracebot_left_arm.
[Msg] GazeboGraspFix: Attaching unit_box::link::collision to gripper tracebot_left_arm.
[Msg] GazeboGraspFix: Detaching unit_box::link::collision from gripper tracebot_left_arm.

[BenRJG]

Looking at it in action, it'll grip, disable collision (I've set this in the config of your plugin), then begin moving into the object, then it'll detach, cause collision, and repeat until the gripper being in the objected forces it to move away

[BenRJG]

I've tried to increase the release tolerance which has fixed the issue so now it stays gripped, however its now not releasing when the gripper is open! I'm going to play with this a little to see if theres a sweet spot. Please let me know if you have any other suggestions :)

[BenRJG]

Managed to get it to drop the box now too by increasing the count threshold again

      <forces_angle_tolerance>100</forces_angle_tolerance>
      <update_rate>4</update_rate>
      <grip_count_threshold>2</grip_count_threshold>
      <max_grip_count>8</max_grip_count>
      <release_tolerance>0.015</release_tolerance>
      <disable_collisions_on_attach>true</disable_collisions_on_attach>
      <contact_topic>__default_topic__</contact_topic>

Going back to the cylindrical object it's still not liking this very much and refusing to grip :/

EDIT:
(Didn't want to keep spamming replys!) The object did end up gripping but took some time to do so (at which point it'd already glitched into an odd position)

[JenniferBuehler]

Hi Ben,

Thanks for the details, I think that's helped me to understand better what the situation is.

What was it originally doing when you had disable_collisions_on_attach to false?
If you have this on true, it is particularly important that the grippers stop moving as soon as the object is gripped (otherwise the grippers move too far and a release is triggered, which is what's happening).

In the real robot, ideally some force-feedback would tell it to stop applying pressure on the object. But in practice often each grasp is tailored for the particular object, so the grippers just won't close more than needed to grasp the object (so that it's not squashed). If you stop moving the grippers at a position where the object just about fits between the grippers, the plugin should also not release the object again, and the object should remain attached (and you can probably leave disable_collisions_on_attach on false).

Both (a) moving the grippers into the object (by disabling collisions and keep the grippers closing) and (b) applying too much force on the object (enabled collisions while still moving grippers --> usually leads to wobbling or even exploding robots) are not right. So either of (a) or (b) should be avoided in the first place.

You can try to either

• react to the grasp by subscribing to the attached-state message (see e.g. the ROS code here) and then stop moving the grippers as soon as the object is attached. Or you can
• never close the grippers much more than what is required for the contacts to happen with this particular object (that's how it's usually done).

If you already are considering this, let me know. If not, I would try this first, with the default parameters (e.g. a grip count of 1 is not very stable either, it means it will instantly attach once the gripper touches, but usually what you want is for the condition to be true for at least a few iterations, then a good/stable position is ensured). And only if that doesn't help, then try to fine-tune by adjusting parameters afterwards.

[BenRJG]

Hi Jenifer,
Thanks for all the useful info! I have managed to get it to work now, the issues seemed to be the collision of the object was a detailed mesh so I changed this to just a cylinder which helped stabilize when being gripped and preventing the strange collision issues.
I agree that the gripper should stop when grasping the object, I'm not actually working on the grasp control for this project so my main aim was just to get something working to demo the functionality in the virtual environment, so I imagine this will work much better when the proper grasp control is implemented!
I am going to try this again with the disable collisions back to false as the initial issues again was with the mesh of the object being picked up.

[JenniferBuehler]

Hi Ben,

That's good news! I didn't even think about the possibility that it could be a mesh ;) Yes collisions with meshes are a beast.
Let me know how it goes!
Jennifer

[yding25]

I have the same issue: the 'OBJ CONTACT' cannot be detected. After a few days of debugging, I find the reason is ROSPY.SLEEP() IS NOT ENOUGH!!! Please, ensure enough processing time for the contact detection. I hope this answer can save you time!

[mco528]

WhatsApp.Video.2023-05-23.saat.16.29.43.mp4

Hi Jennifer. I'm trying to make 3 DOF robots that are voice controlled. I'm trying to get the robot to pick and place, but as seen in the video, the object doesn't get between the grippers. It looks as if the grippers are stuffed. how can i solve this problem

[JenniferBuehler]

Please make a new issue for this, I will respond as soon as I get time to take a proper look