写在前面
更正一下，前面一系列博客提到的“六自由度”机械臂实际上是arm部分五自由度+gripper部分一个自由度。我购买的机械臂手抓部分是一个舵机控制两个手爪开合，但是仿真时就得将gripper分成两个单独joint来看。
我查到过可以使用Mimic标签来模拟这种一个舵机控制两个对称手爪，貌似在gazebo中可以仿真，但是可能会在moveit中出现关节丢失的现象（我还没试过，估计比较麻烦）。所以，仿真就简单将gripper分成两个单独joint看。
演示
图中演示花费比较长时间，因为计算量比较大，加上五自由度arm部分在运动规划上存在很多限制，很容易出现pick and place failed。
更换运动学插件ikfast
我用了几个pick_and_place的例子，换了一些object pose，但一直都是抓取失败。看了这位大神的博客ros下diy手臂rviz演示抓取放置，他和我一样都是五自由度的arm，用了个新包moveit_simple_grasps抓取成功了，嘻嘻天助我也，那我也来试试呗。试来试去，包啥的都配置成功了，结果rosrun pick_and_place.py仍然抓取失败，算好久好久还是失败，换pose也不对。接着baidu、google，查到这个帖子Using moveit with 5-DOF arm(需要fq)，同样也是always fails to grasp object，下面有个评论，如图。
意思就是moveit中的KDL运动学插件可能对少于六自由度的机械臂不太友好，可以换成ikfast。虽然这是2013年的帖子，但是查了下IKfast插件，看起来很好用的样子，干脆把插件也给换了算了。
ROS进阶——MoveIt!运动学插件IKFAST配置，参考这个来安装ikfast。参数名字都换成自己相应配置的名称，另外需要注意的是，在进行生成ikfast文件这个步骤时，要让iktype=translationdirection5d。我生成这个文件时出错了，改变了一下end effector link的坐标方向就好了。
插件如其名，用起来so fast～换了几个pose之后，抓取终于成功了。
使用moveit_simple_grasps包
包地址
包简介
A basic grasp generator for simple objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline. Does not consider friction cones or other dynamics.
Its current implementation simple takes as input a pose vector (postition and orientation) and generates a large number of potential grasp approaches and directions. Also includes a grasp filter for removing kinematically infeasible grasps via threaded IK solvers.
作者说这个包还有很多东西要维护，所以实际使用起来会出现一些问题。
先要注释掉几行代码，参见https://github.com/qboticslabs/mastering_ros/issues/3，原因是包以及系统版本的问题，下载下面的moveit_simple_grasps_1.zip，然后根据相应修改注释掉几行代码，这个亲测有效。
建立自己的(arg robot)_grasp_data.yaml
base_link: 'base_link'
# ====================================================
gripper:
end_effector_name: gripper #ee group name
#actuated joints in end effector
joints : [finger_joint1, finger_joint2]
#open position
pregrasp_posture : [0.0, 0.0]
pregrasp_time_from_start : 4.0
#close position
grasp_posture : [0.0225, 0.0225]
grasp_time_from_start : 4.0
#???
#desired pose from end effector to grasp - [x,y,z]
grasp_pose_to_eef : [0.0, 0.0, 0.0]
#desired pose from end effector to grasp - [r, p, y]
grasp_pose_to_eef_rotation : [0.0, 0.0, 0.0] # 1.5707 = PI/2
#根据自己moveit_config文件夹中的srdf进行设置
end_effector_parent_link: gripper_frame
这个文件定义了用于抓取对象的夹持器以及抓取前、后的位姿，注意这里第三行代码名称为gripper，对应simple_grasps_server.cpp中第193行开始的代码，如下。同时，将该文件中的planning_group_name_(side_+"_arm")修改为planning_group_name_("arm")。
int main(int argc, char *argv[])
{
ros::init(argc, argv, "grasp_generator_server");
moveit_simple_grasps::GraspGeneratorServer grasp_generator_server("generate", "gripper");
ros::spin();
return 0;
}
pick_and_place.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface
from moveit_commander import roscpp_initialize, roscpp_shutdown
from actionlib import SimpleActionClient, GoalStatus
from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion
from moveit_msgs.msg import PickupAction, PickupGoal
from moveit_msgs.msg import PlaceAction, PlaceGoal
from moveit_msgs.msg import PlaceLocation
from moveit_msgs.msg import MoveItErrorCodes
from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GraspGeneratorOptions
from tf.transformations import quaternion_from_euler
import sys
import copy
import numpy
# Create dict with human readable MoveIt! error codes:
moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
if not name[:1] == '_':
code = MoveItErrorCodes.__dict__[name]
moveit_error_dict[code] = name
class PickAndPlace:
def __init__(self):
# Retrieve params:
self._table1_object_name = rospy.get_param('~table_object_name', 'table1')
self._table2_object_name = rospy.get_param('~table_object_name', 'table2')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'coke_can')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.015)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.15)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.01)
rospy.loginfo('_approach_retreat_desired_dist=%f',self._approach_retreat_desired_dist)
rospy.loginfo('_approach_retreat_min_dist=%f',self._approach_retreat_min_dist)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table1_object_name)
self._scene.remove_world_object(self._table2_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table1 = self._add_table1(self._table1_object_name)
self._pose_table2 = self._add_table2(self._table2_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_table2.position.x
self._pose_place.position.y = self._pose_table2.position.y
self._pose_place.position.z = self._pose_coke_can.position.z-0.005
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(2.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(2.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table1_object_name)
self._scene.remove_world_object(self._table2_object_name)
def _add_table1(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = -0.20
p.pose.position.z = 0.20
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.15, 0.08, 0.003))
return p.pose
def _add_table2(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.y = 0.20
p.pose.position.z = 0.20
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.15, 0.08, 0.003))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = -0.20+0.02
p.pose.position.y = 0.01#0.0117890518355#0.0#-0.015 - 0.01
p.pose.position.z = 0.20 + (0.030) / 2.0
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.015, 0.015, 0.030))
#self._scene.add_sphere (name, p, (0.015))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(2.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
#q = quaternion_from_euler(0.0,angle, 0.0 )
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table1_object_name
# Configure goal planning options:
goal.allowed_planning_time = 15.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
goal.support_surface_name = self._table2_object_name
# Configure goal planning options:
goal.allowed_planning_time = 15.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
self.grasps=grasps
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def main():
p = PickAndPlace()
rospy.spin()
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('pick_and_place')
main()
roscpp_shutdown()
moveit_simple_grasps包中写了simple_grasp_server.cpp，即创建了一个grasp生成服务器，消息类型为GenerateGraspsAction。pick_and_place.py的主要职责就是创建一个名为/moveit_simple_grasps_server/generate的action客户端来查询grasp位姿，消息类型为GenerateGraspsAction，同时创建move group的/pickup客户端和/place客户端，待成功生成grasps后发送给move group相应服务器进行处理。具体可以看生成的rosgraph。
另外，还需要在grasp_data.cpp中把相关数据修改成自己的配置
approach_retreat_desired_dist_ = 0.15; // 0.3;
approach_retreat_min_dist_ = 0.01;
// distance from center point of object to end effector
grasp_depth_ = 0.01;// in negative or 0 this makes the grasps on the other side of the object! (like from below)
// generate grasps at PI/angle_resolution increments
angle_resolution_ = 16; //TODO parametrize this, or move to action interface
运行
修改grasp_generator_server.launch为自己的配置，如下
<launch>
<arg name="robot" default="myrobot" />
<arg name="group" default="arm" />
<arg name="end_effector" default="gripper" />
<!-- Start the test -->
<node name="moveit_simple_grasps_server" pkg="moveit_simple_grasps" type="moveit_simple_grasps_server" output="screen">
<param name="group" value="$(arg group) " />
<param name="end_effector" value="$(arg end_effector)" />
<rosparam command="load" file="$(find moveit_simple_grasps)/config/$(arg robot)_grasp_data.yaml"/>
</node>
</launch>
然后运行命令：
roslaunch myrobot_moveit_config demo.launch
roslaunch moveit_simple_graspsp grasp_generator_server.launch
rosrun moveit_simple_grasps pick_and_place.py
评论（0）
您还未登录，请登录后发表或查看评论