ROS robotiq仿真开闭控制的几种方法
注:装载自http://www.javashuo.com/article/p-dwzvehxw-ns.html,仅供自己学习使用,侵权联系删除
robotiq二指手抓在实验室和工厂普遍使用,比较广泛的就是UR机器人结合robotiq手抓。
注:若是要在gazebo中抓起物体,须要用到JenniferBuehler的gazebo_grasp_plugin: https://github.com/JenniferBuehler/gazebo-pkgs/tree/master/gazebo_grasp_plugingit
1. Gazebo中直接基于GripperActionController实现github
参考了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文件中增长:spa
<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文件。code
而后再写一个python脚本:server
#!/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.pyblog
理解:控制器提供了一个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。