Gazebo中Robotiq使用和是否抓取物体判断

Gazebo中Robotiq使用和是否抓取物体判断

由于课题需要使用robotiq夹爪，但是在仿真环境中Robotiq如何抓取物体，怎么判断是否抓住了是需要解决的一个问题。并且大多现有的例子都是抓固定大小方块，抓不定宽度的物体就没有方法或教程出现，为解决这个问题，下面梳理一下我查找并实现的例子。

1、gazebo中的仿真使用

• 在rviz中仿真和实际使用，直接用github里的直接可以用。官方链接

• 官方缺少gazebo的描述文件和使用，在下面这个链接有使用gazebo仿真

• 上面这个也有问题，虽然在gazebo中能看到模型，但是不能控制其动作，因此用到了gazebo插件，链接如下gazebo插件

• 在配置好gazebo模型和控制插件后，进一步判断gazebo中是如何抓取的。

2、 仿真中如何判断抓取

• 在gazebo中，插件是将当夹爪接触物体产生一定的碰撞力后达到阈值时，将其附在夹爪上成为一体，以实现抓取，几个主要的参数如下：

<forces_angle_tolerance>100</forces_angle_tolerance>
<update_rate>100</update_rate>
<grip_count_threshold>2</grip_count_threshold>
<max_grip_count>8</max_grip_count>
<release_tolerance>0.01</release_tolerance> <!-- 0.01977<0.0198<0.01999 -->
<disable_collisions_on_attach>false</disable_collisions_on_attach>
<contact_topic>__default_topic__</contact_topic>

• 由于其是在gazebo中实现抓取的，存在一个信号，物体是否被抓取。插件在GazeboGraspFix.cpp实现，但其信号是gazebo信号，而我们经常使用的是ROS信号，因此需要一个gazebo 信号转ROS 的话题，在文件grasp_event_republisher.cpp实现了，完整代码如下：

#include <gazebo/transport/transport.hh>
#include <gazebo_grasp_plugin/msgs/grasp_event.pb.h>
#include <gazebo_grasp_plugin_ros/GazeboGraspEvent.h>
#include <ros/ros.h>
using GraspEventPtr = boost::shared_ptr<const gazebo::msgs::GraspEvent>;
ros::Publisher gGraspEventPublisher;

void ReceiveGraspMsg(const GraspEventPtr& gzMsg)
{
  ROS_INFO_STREAM("Re-publishing grasp event: " << gzMsg->DebugString());
  gazebo_grasp_plugin_ros::GazeboGraspEvent rosMsg;
  rosMsg.arm      = gzMsg->arm();
  rosMsg.object   = gzMsg->object();
  rosMsg.attached = gzMsg->attached();
  gGraspEventPublisher.publish(rosMsg);
}
int main(int argc, char** argv)
{
  // Initialize ROS and Gazebo transport
  ros::init(argc, argv, "gazebo_grasp_plugin_event_republisher");
  if (!gazebo::transport::init())
  {
    ROS_ERROR("Unable to initialize gazebo transport - is gazebo running?");
    return 1;
  }
  gazebo::transport::run();

  // Subscribe to Gazebo grasp event message
  gazebo::transport::NodePtr gzNode(new gazebo::transport::Node());
  gzNode->Init();
  gazebo::transport::SubscriberPtr subscriber;
  try
  {
    subscriber = gzNode->Subscribe("~/grasp_events", &ReceiveGraspMsg);
  }
  catch (std::exception e)
  {
    ROS_ERROR_STREAM("Error subscribing to topic: " << e.what());
    return 1;
  }

  // Initialize ROS publisher
  ros::NodeHandle   rosNode("~");
  const std::string pubTopic = "grasp_events";
  gGraspEventPublisher =
    rosNode.advertise<gazebo_grasp_plugin_ros::GazeboGraspEvent>(pubTopic, 1);

  ros::spin();
  gazebo::transport::fini();
  return 0;
}

使用: rosrun gazebo_grasp_plugin_ros grasp_event_republisher

• 简单写一个接收话题如下，通过该话题我们就可以判断物体是否被抓取到了

#! /usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy
from gazebo_grasp_plugin_ros.msg import GazeboGraspEvent
def doMsg(p):
    print(p.attached)
    if(p.attached):
        rospy.loginfo("Attached grasp target!")
if __name__ == "__main__":
    #1.初始化节点
    rospy.init_node("GazeboGraspMsgRead")
    #2.创建订阅者对象
    sub = rospy.Subscriber("/gazebo_grasp_plugin_event_republisher/grasp_events", GazeboGraspEvent, doMsg, queue_size=1)
    rospy.spin() #4.循环

3、例子

下面是通过gazebo下控制robotiq实现抓取一个位置宽度的胶带。

在启动程序中出现绿色字体日志，显示抓取吸附了，下面个两个终端是我们分别用gazebo信号转ROS话题并发布和订阅显示已经抓取成功。

4、完结撒花