ROS robotiq仿真开闭控制的几种方法

robotiq二指手抓在实验室和工厂广泛使用，比较普遍的就是UR机器人结合robotiq手抓。

由于手头没有实体手抓，所以都是在ROS中仿真使用的。在网上找资料的时候，也尝试了多种控制手抓开闭的方式，当然背后的东西基本都大同小异。

注：如果要在gazebo中抓起物体，需要用到JenniferBuehler的gazebo_grasp_plugin: https://github.com/JenniferBuehler/gazebo-pkgs/tree/master/gazebo_grasp_plugin

1. Gazebo中直接基于GripperActionController实现

参考了github链接：https://github.com/utecrobotics/ur5/tree/master/ur5_gazebo

配置gripper_controller

# Gripper controller
gripper_controller:
  type: position_controllers/GripperActionController
  joint: robotiq_85_left_knuckle_joint # or gripper_finger1_joint
  action_monitor_rate: 20
  goal_tolerance: 0.002
  max_effort: 100
  stall_velocity_threshold: 0.001
stall_timeout: 1.0

在gazebo的controller.launch文件中增加：

<rosparam file="$(find robotiq_85_gazebo)/controller/gripper_controller_robotiq.yaml" command="load"/> 
<node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller --shutdown-timeout 0.5" />

注意：为了简便，只给出了核心部分，这并不是完整的launch文件。

然后再写一个python脚本：

#!/usr/bin/python

import rospy
import actionlib
import control_msgs.msg

rospy.init_node('gripper_control')

# Create an action client
client = actionlib.SimpleActionClient(
    '/gripper_controller/gripper_cmd',  # namespace of the action topics
    control_msgs.msg.GripperCommandAction # action type
)
    
# Wait until the action server has been started and is listening for goals
client.wait_for_server()

# Create a goal to send (to the action server)
goal = control_msgs.msg.GripperCommandGoal()
goal.command.position = value   # From 0.0 to 0.8
goal.command.max_effort = -1.0  # Do not limit the effort
client.send_goal(goal)

client.wait_for_result()

代码来自：https://github.com/utecrobotics/ur5/blob/master/ur5_gazebo/scripts/send_gripper.py

理解：控制器提供了一个action server控制给定关节的值，然后我们写一个action client发送关节值和控制请求。

打开模型的gazebo launch文件，运行python程序，就能看到gazebo中robotiq手抓动了。这种方法比较简单。

2. 基于moveit！设置robotq关节值

将前面的controller改为：

gripper_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - gripper_finger1_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      gripper_finger1_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10

基于moveit控制的话就要给robotiq配置一个group（moveit的setup_assistant.launch），下述内容来自配置完成后的/config/xxx.srdf：

    <group name="robotiq">
        <joint name="gripper_finger1_joint" />
        <joint name="robotiq_85_base_joint" />
        <joint name="gripper_finger1_inner_knuckle_joint" />
        <joint name="gripper_finger1_finger_tip_joint" />
        <joint name="gripper_finger1_finger_joint" />
        <joint name="gripper_finger2_inner_knuckle_joint" />
        <joint name="gripper_finger2_finger_tip_joint" />
        <joint name="gripper_finger2_joint" />
        <joint name="gripper_finger2_finger_joint" />
        <joint name="ur_gripper" />
    </group>

2.1. 通过setup_assistant配置时设置的Robot Pose

参考：https://github.com/neka-nat/ur_ws/tree/master/src/ur_robotiq

设置robot pose

注意：/config/xxx.srdf可以查看设置情况，这里只有gripper_finger1_joint是实际与手抓运动相关的，其他都是passive_joint。一般上述设置后会保存6个joint的值，我们需要把其余5个joint的值删掉，如下所示。否则将会规划不成功，为什么呢？因为当<group_state>中有6个joint值时，相应的规划器就要同时规划这6个关节，但是robotiq的6个joint并不是机械臂式的串联结构，所以规划器是找不到解的，只保存gripper_finger1_joint值时，只需要规划这一个joint就够了，不需要考虑其他joint。

这也是我一开始踩得坑，6个joint时手抓一直动不了。但是用2.2的方法能规划，后来一想，应该就是这个原因。

<group_state name="close" group="robotiq">
    <joint name="gripper_finger1_joint" value="0.5" />
</group_state>

然后同样通过python程序实现moveit规划robot到预设置pose

#!/usr/bin/env python
import rospy
import moveit_commander

rospy.init_node('gripper_go')
grp = moveit_commander.MoveGroupCommander("robotiq")
grp.set_named_target('close')
grp.go(wait=True)

当然这种方法手抓闭合的值是固定的。

2.2. 通过完整的moveit C++/python程序设置闭合值

具体如何实现可以查看moveit教程，有相应的C++和python的接口函数，用于规划某一个group动作。

核心代码：

// C++
//规划robotiq手抓
moveit::planning_interface::MoveGroup group("robotiq");
std::vector<double> rbq_joint_values;

// 通过设置关节值的方式。
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()),rbq_joint_values);

// robotiq共有六个joint，只有设置gripper_finger1_joint才能生效，对应下面的rbq_joint_values[2]
rbq_joint_values[0] = 0;
rbq_joint_values[1] = 0;
rbq_joint_values[2] = 0.5; // your joint value
rbq_joint_values[3] = 0;
rbq_joint_values[4] = 0;
rbq_joint_values[5] = 0;

group.setJointValueTarget(rbq_joint_values);
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
group.execute(my_plan);

python版略。

 

总结：第一种方法最简单，不需要moveit规划，所以响应比较快。后面两种都是用的moveit，在这个过程中也顺便学习了moveit。