MoveIt!五自由度机械臂pick_and_place抓取规划演示

ROS探索 专栏收录该内容
12 篇文章 3 订阅

写在前面

更正一下,前面一系列博客提到的“六自由度”机械臂实际上是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

参考

ros下diy手臂rviz演示抓取放置

davetcoleman/moveit_simple_grasps

turtlebot/turtlebot_arm

Moveit_simple_grasping #3

Pick and Place Tutorial

  • 3
    点赞
  • 17
    评论
  • 30
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

相关推荐
©️2020 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值