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。