robotiq夹爪仿真物体抓取以及配置fix插件

之前把模型仿真都配置好了，并且moveit和gazebo也能进行联动。

问题1 gazebo中模型位置

原本以为配置完运动环境就可以直接进行抓取了。

这里原本是需要利用摄像头对物体进行识别，定位，抓取位姿估计。

但是gazebo中已经默认知道了物体的位置，这里只需要进行转换。
https://blog.csdn.net/qq_38427871/article/details/105042871

rostopic list
rostopic list /gazebo
rostopic info /gazebo/model_states
rostopic echo /gazebo/model_states
name: [ground_plane, table_marble, beer, bowl, coke_can, plastic_cup, bookshelf, block,
  banana, dropbox, dropbox_1, biscuits, book, eraser, glue, snacks, soap, soap2, sticky_notes,
  dropbox_2, robot]
pose: 
  - 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  - 
    position: 
      x: -1.80133
      y: 0.258191
      z: 0.533526
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  - 
    position: 
  

这里只需要订阅相关话题，获取想要抓取物体的位置信息，之后通过tf坐标转换进行抓取就行了。

问题2 物体模型弹开

夹爪一碰到模型，物体就弹开。
部分模型设置的碰撞检测部分体积远大于所显示的部分。如奶油盒等，碰撞检测体积是一个正方体。
夹爪一碰到检测部分，两者就弹开了。

问题3 机器模型乱动

机械臂夹爪运动过程中发生碰撞，模型就开始乱窜。

问题4 无法对物体进行有效抓取

选择了一个碰转检测体积和实际显示体积一样的物体：红色方块作为抓取实验的物体。
然而夹爪顺利夹住后物体开始不停的抖动，滑开。导致无法有效抓取。
该类问题解决方案网上比较多。采用将物体与夹爪接触时让物体与夹爪fix形成一个整体。

问题5 夹爪力控方式

gazebo中有力控的接口，上述对夹爪都只是使用了位置控制方式。然而网上对于力控的资料几乎没有。

问题6 如何有效进行检测抓取

上述都是对已知物体位置进行的直接抓取。
如何结合视觉功能进行检测。

力控

http://wiki.ros.org/joint_trajectory_controller#Controller_configuration_examples

Complete description, effort interface

head_controller:
  type: "effort_controllers/JointTrajectoryController"
  joints:
    - head_1_joint
    - head_2_joint

  constraints:
    goal_time: 0.5                   # Override default
    stopped_velocity_tolerance: 0.02 # Override default
    head_1_joint:
      trajectory: 0.05               # Not enforced if unspecified
      goal: 0.02                     # Not enforced if unspecified
    head_2_joint:
      goal: 0.01                     # Not enforced if unspecified

  gains: # Required because we're controlling an effort interface
    head_1_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}
    head_2_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}

  state_publish_rate:  25            # Override default
  action_monitor_rate: 30            # Override default
  stop_trajectory_duration: 0        # Override default

http://docs.ros.org/en/melodic/api/hardware_interface/html/c++/classhardware__interface_1_1EffortJointInterface.html

https://classic.gazebosim.org/tutorials?tut=ros_control
rrbot:

effort_controllers - Command a desired effort(force/torque) to the
Hardware Interface.

joint_position_controller - Receives a position input and sends an
effort output, using a PID controller. joint_group_position_controller

• Set multiple joint positions at once. joint_velocity_controller - Receives a velocity input and sends an effort output, using a PID
  controller. joint_effort_controller - Receives an effort input and
  sends an effort output, just transferring the input with the
  forward_command_controller.

joint_group_effort_controller - Set multiple joint efforts at once.

  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

物体有效抓取

针对上述模型乱动问题。
https://github.com/JenniferBuehler/gazebo-pkgs/wiki/The-Gazebo-grasp-fix-plugin

install

general-message-pkgs

Dependencies:
object_recognition_msgs
This repository itself: general-message-pkgs

sudo apt-get install \
    ros-<distro>-object-recognition-msgs
cd <your-catkin-ws>/src
git clone https://github.com/JenniferBuehler/general-message-pkgs.git 
cd ..
catkin_make
success!! 100%!!

gazebo-pkgs

Dependencies:
gazebo_ros
eigen_conversions
object_recognition_msgs
roslint (only while the packages are still in beta)
My repository general-message-pkgs
This repository gazebo-pkgs

sudo apt-get install \
    ros-<distro>-gazebo-ros \
    ros-<distro>-eigen-conversions \
    ros-<distro>-object-recognition-msgs \
    ros-<distro>-roslint
    cd <your-catkin-ws>/src
git clone https://github.com/JenniferBuehler/general-message-pkgs.git
git clone https://github.com/JenniferBuehler/gazebo-pkgs.git
cd ..
catkin_make



sudo apt-get install ros-kinetic-gazebo-ros
正在读取软件包列表... 完成
正在分析软件包的依赖关系树       
正在读取状态信息... 完成       
ros-kinetic-gazebo-ros 已经是最新版 。
升级了 0 个软件包，新安装了 0 个软件包，要卸载 0 个软件包，有 46 个软件包未被升级。

sudo apt-get install ros-kinetic-eigen-conversions
正在读取软件包列表... 完成
正在分析软件包的依赖关系树       
正在读取状态信息... 完成       
ros-kinetic-eigen-conversions 已经是最新版 (1.11.9-0xenial-20210503-100904-0800)。
升级了 0 个软件包，新安装了 0 个软件包，要卸载 0 个软件包，有 46 个软件包未被升

~/grasp_fix/src$ cd ..
~/grasp_fix$ source ./devel/setup.bash
catkin_make  failed

error 1

gazebo_grasp_plugin/msgs/PROTOBUF_PROTOC_EXECUTABLE-NOTFOUND', needed by 'gazebo-pkgs/gazebo_grasp_plugin/msgs/grasp_event.pb.cc'。

solution:

sudo apt-get install protobuf-compiler

warn 1

catkin_package() DEPENDS on 'gazebo' but neither 'gazebo_INCLUDE_DIRS' nor
  'gazebo_LIBRARIES' is defined.

不管

error 2

this file requires compiler and library support for the iso c++ 2011 standard 

解决：
当使用CMAKE编译工程时，出现这个错误，解决办法：
在工程主目录的CMakeLists.txt中添加如下，可以编译成功：

https://blog.csdn.net/huoxingrenhdh/article/details/84032663

/grasp_fix/src$ sudo chmod +x *.txt //修改权限
 sudo chmod +r  *.txt


sudo gedit CMakeLists.txt //保存退出 
SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")

正确编译

catkin_make 100%!!! 终于成功了

gzplugin_grasp_fix.urdf.xacro

https://github.com/JenniferBuehler/gazebo-pkgs/wiki/The-Gazebo-grasp-fix-plugin

<?xml version="1.0" encoding="UTF-8"?>
<root 
 xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
 xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
 xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
 xmlns:xacro="http://ros.org/wiki/xacro">


<!-- MACRO FOR THE ROBOT ARM ON THE TABLE-->
<xacro:macro name="gzplugin_grasp_fix">
	<gazebo>
		<plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
			<!--
            NOTE: The finger tips are linked together with the links before, because they are connected with a
			fixed joint. Gazebo makes one whole link out of this automatically. When listing the 9_*_tip links
			here, they won't be found in the SDF.
            -->
            <arm>
                <arm_name>JacoArm1</arm_name>
                <palm_link>jaco_6_hand_limb</palm_link>
                <gripper_link>jaco_8_finger_index</gripper_link>
                <gripper_link>jaco_8_finger_pinkie</gripper_link>
                <gripper_link>jaco_8_finger_thumb</gripper_link>
            </arm>
			<forces_angle_tolerance>100</forces_angle_tolerance>
			<update_rate>4</update_rate>
			<grip_count_threshold>4</grip_count_threshold>
			<max_grip_count>8</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>
	</gazebo>
</xacro:macro>

</root>

Important

The link names you specify are not necessarily the ones which are
published on /tf, if you are using ROS. They must be the link names
used in Gazebo. Gazebo joins chains of fixed links into one, so if you
have a chain of several links joined by fixed joints, it will become
one single link in Gazebo, and the name may change. To find out which
link names you have to put into the <palm_link> and <gripper_link>
tags, it is easiest if you just launch your robot in Gazebo, and look
up how the links are called on the left-hand side properties window.
In the “World” tab, expand “Models” to find your robot, then expand
this to see the links of your robot (some joints are listed there as
well). You can click on them to highlight them in Gazebo. For example,
for the Jaco robot on the table, it looks like this:

配置过程

将之前成功配置的机械臂拿来
https://blog.csdn.net/qq_44642372/article/details/124119100

https://blog.csdn.net/qq_44642372/article/details/123995916#t12

  <gazebo>
    <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
          <arm>
            <arm_name>ur3_robotiq</arm_name>
            <palm_link> robotiq_ft_frame_id  </palm_link>
            <gripper_link> gripper_finger1_finger_tip_link </gripper_link>
            <gripper_link> gripper_finger1_finger_link </gripper_link>
            <gripper_link> gripper_finger2_finger_tip_link </gripper_link>
            <gripper_link> gripper_finger2_finger_link </gripper_link>
            <gripper_link> gripper_finger1_inner_knuckle_link </gripper_link>
            <gripper_link> gripper_finger1_knuckle_link </gripper_link>
            <gripper_link> gripper_finger2_inner_knuckle_link </gripper_link>
            <gripper_link> gripper_finger2_knuckle_link </gripper_link>
          </arm>
        <forces_angle_tolerance>150</forces_angle_tolerance>
        <update_rate>130</update_rate>
        <grip_count_threshold>4</grip_count_threshold>
        <max_grip_count>8</max_grip_count>
        <release_tolerance>0.01</release_tolerance>
        <disable_collisions_on_attach>true</disable_collisions_on_attach>
        <contact_topic>__default_topic__</contact_topic>
      </plugin>
  </gazebo>

没用啊 配置了 还是会抖动弹开。
后面发现终端中报错信息：
部分link在gazebo中无法找到，无法构建抓取插件。

Err] [GazeboGraspGripper.cpp:64] GazeboGraspGripper: Palm link robotiq_ft_frame_id not found. The gazebo grasp plugin will not work.
[Err] [GazeboGraspFix.cpp:231] GazeboGraspFix: Could not initialize arm gripper_arm. Skipping.
[Err] [GazeboGraspFix.cpp:261] ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix because no arms were configured successfully. Plugin will not work.

[Err] [GazeboGraspGripper.cpp:76] GazeboGraspGripper ERROR: Link gripper_finger1_finger_link can't be found in gazebo for GazeboGraspGripper model plugin. Skipping.
[Err] [GazeboGraspGripper.cpp:76] GazeboGraspGripper ERROR: Link gripper_finger2_finger_link can't be found in gazebo for GazeboGraspGripper model plugin. Skipping.

把两个finger link注释掉同时把 ft sensor 传感器去掉，让爪子直接连接机械臂。
注意爪子连接关节为 wrist_3_link

rosrun tf tf_echo wrist_3_link ee_link

再次运行gazebo没有大的报错，只有：

[Err] [ContactManager.cc:282] Filter with the same name already exists! Aborting

讲道理应该没问题了，但貌似还是不能用

[Msg] GazeboGraspFix: Using max_grip_count 8
[Msg] GazeboGraspFix: Using grip_count_threshold 4
[Msg] GazeboGraspFix: Using release_tolerance 0.01
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger1_finger_tip_link::gripper_finger1_finger_tip_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger1_inner_knuckle_link::gripper_finger1_inner_knuckle_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger1_knuckle_link::gripper_finger1_knuckle_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger1_knuckle_link::gripper_finger1_knuckle_link_fixed_joint_lump__gripper_finger1_finger_link_collision_1
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger2_finger_tip_link::gripper_finger2_finger_tip_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger2_inner_knuckle_link::gripper_finger2_inner_knuckle_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger2_knuckle_link::gripper_finger2_knuckle_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name robot::gripper_finger2_knuckle_link::gripper_finger2_knuckle_link_fixed_joint_lump__gripper_finger2_finger_link_collision_1
[Err] [ContactManager.cc:282] Filter with the same name already exists! Aborting
[Msg] Subscribing contact manager to topic 
[Msg] Advertising grasping events on topic grasp_events

又看了遍代码 robotiq85的模型文件中自带了一个fix插件 相当于创建了两次。
参考：https://blog.csdn.net/weixin_47266712/article/details/124985969
他这里：

<gazebo>
	  <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
            <arm>
              <arm_name>gripper_arm</arm_name>
              <palm_link>${parent}</palm_link>
              <gripper_link>${prefix}gripper_finger1_finger_tip_link</gripper_link>
              <gripper_link>${prefix}gripper_finger2_finger_tip_link</gripper_link>
            </arm>
	    <forces_angle_tolerance>100</forces_angle_tolerance>
	    <update_rate>2</update_rate>
	    <grip_count_threshold>1</grip_count_threshold>
	    <max_grip_count>2</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>

参照他的重新进行配置，这次编译不报错。并且单独启动gazebo手动抓取测试，能够成功抓取。

上面的报错 部分关节找不到

https://github.com/JenniferBuehler/gazebo-pkgs/issues/31

Just want to post an answer here if people runs into this issue:
Gazebo combines all links connected by fixed joints together to reduce
computational complexity so the link specified in <palm_link> or
<gripper_link> may not exist in the model Gazebo has of your robot
(answered here). In the OP’s case, replacing wrist_3_link with
gripper_finger1_inner_knuckle_link works:

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

成功 完结

sudo apt-get install \ ros-kinetic-gazebo-ros \ros-kinetic-eigen-conversions \ ros-kinetic-object-recognition-msgs \ ros-kinetic-roslint



sudo apt-get install ros-kinetic-gazebo-ros

sudo apt-get install ros-kinetic-eigen-conversions


git clone -b master  https://github.com/wg-perception/object_recognition_msgs.git
git clone -b master  https://github.com/ros/roslint.git 

git clone https://github.com/JenniferBuehler/general-message-pkgs.git
git clone https://github.com/JenniferBuehler/gazebo-pkgs.git