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

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



moveit_simple_grasps
官方
https://github.com/davetcoleman/moveit_simple_grasps/

简介:


SUPPORT FOR THIS PACKAGE HAS ENDED

Sorry, too many things to maintain. I'll still merge PRs and am happy to share maintainership of this package with someone interested.



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.

使用Moveit pick和 place管线实现对简单物体如方块 圆柱的基本抓取生成器,不考虑摩擦或其他动态.

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.


当前实现简单的接受输入向量位姿（位置和方向），会生成大量潜在的抓取途径和方向。
包含一个抓取过滤器,去除逆运动学解算器算过的动力学上不可行的抓取

This package includes:

    Simple pose-based grasp generator for a block 简单的基于位姿的抓取生成器 对方块的
    Separate grasp generators for custom objects such as rectanguar or cylindrical objects自定义如方形圆柱等物体的独立抓取器
    Grasp filter 抓取过滤器
    Test code and visualizations

Developed by Dave Coleman at the Correll Robotics Lab, University of Colorado Boulder with outside contributors.

由Dave Coleman开发于Correll Robotics Lab,University of Colorado Boulder with outside contributors.



正如简介所说这个包好处是能生成很多抓取位姿,有过滤功能,可视化.
作者说了很多东西需要维护.实际使用时进行配置会碰到一些小问题

moveit_simple_grasps_server

simple_grasps_server.cpp

line192开始的代码:

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "grasp_generator_server");
  moveit_simple_grasps::GraspGeneratorServer grasp_generator_server("generate", "right");//这一句 right参数
  ros::spin();
  return 0;
}

参数传入GraspData类loadRobotGraspData时,会从
moveit_simple_grasps_server/right名称空间下取参数值.找不到就会报错.


zzz_arm_grasp_data.yaml

base_link: 'base_link'
# right 名称空间
right:
  end_effector_name: 'grap'#注意是group的 name不是end_effector的name

  # Default grasp params
  joints: ['finger_1_joint', 'finger_2_joint']

  pregrasp_posture:          [0.0, 0.0]
  pregrasp_time_from_start:  &time_from_start 4.0

  grasp_posture:             [1.0, 1.0]
  grasp_time_from_start:     *time_from_start

  postplace_time_from_start: *time_from_start

  # Desired pose from end effector to grasp [x, y, z] + [R, P, Y]
  grasp_pose_to_eef:          [0.0, 0.0, 0.0]
  grasp_pose_to_eef_rotation: [0.0, 0.0, 0.0]

  end_effector_parent_link: 'hand_zhuan_link'

moveit 生成的srdf如下

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="zzz_arm">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="arm2">
        <joint name="shoulder_zhuan_joint" />
        <joint name="upper_arm_joint" />
        <joint name="fore_arm_joint" />
        <joint name="hand_wan_joint" />
        <joint name="hand_zhuan_joint" />
        <joint name="grasping_frame_joint" />
    </group>
    <group name="grap">
        <joint name="finger_1_joint" />
        <joint name="finger_2_joint" />
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="home" group="arm2">
        <joint name="fore_arm_joint" value="0" />
        <joint name="hand_wan_joint" value="0" />
        <joint name="hand_zhuan_joint" value="0" />
        <joint name="shoulder_zhuan_joint" value="0" />
        <joint name="upper_arm_joint" value="0" />
    </group_state>
    <group_state name="rest" group="arm2">
        <joint name="fore_arm_joint" value="-2.3461" />
        <joint name="hand_wan_joint" value="-1.4875" />
        <joint name="hand_zhuan_joint" value="0" />
        <joint name="shoulder_zhuan_joint" value="0" />
        <joint name="upper_arm_joint" value="1.5607" />
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="grap_eef" parent_link="grasping_frame" group="grap"  />
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="shoulder_zhuan_link" reason="Adjacent" />
    <disable_collisions link1="base_link" link2="upper_arm_link" reason="Never" />
    <disable_collisions link1="finger_1_link" link2="finger_2_link" reason="Never" />
    <disable_collisions link1="finger_1_link" link2="fore_arm_link" reason="Never" />
    <disable_collisions link1="finger_1_link" link2="hand_wan_link" reason="Never" />
    <disable_collisions link1="finger_1_link" link2="hand_zhuan_link" reason="Adjacent" />
    <disable_collisions link1="finger_2_link" link2="fore_arm_link" reason="Never" />
    <disable_collisions link1="finger_2_link" link2="hand_wan_link" reason="Never" />
    <disable_collisions link1="finger_2_link" link2="hand_zhuan_link" reason="Adjacent" />
    <disable_collisions link1="fore_arm_link" link2="hand_wan_link" reason="Adjacent" />
    <disable_collisions link1="fore_arm_link" link2="hand_zhuan_link" reason="Never" />
    <disable_collisions link1="fore_arm_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="hand_wan_link" link2="hand_zhuan_link" reason="Adjacent" />
    <disable_collisions link1="shoulder_zhuan_link" link2="upper_arm_link" reason="Adjacent" />
</robot>

测试抓取和放置要根据diy手臂的自由度和运动范围来规划场景,合理安排桌面和目标的位姿.

以经典的可乐罐抓放python代码为参考
源码

#!/usr/bin/env python

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 CokeCanPickAndPlace:
    def __init__(self):
        # Retrieve params:
        self._table_object_name = rospy.get_param('~table_object_name', 'table')
        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', 'arm2')
        self._gripper_group = rospy.get_param('~gripper', 'grap')

        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)
        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._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table    = self._add_table(self._table_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_coke_can.position.x -0.05
        self._pose_place.position.y = self._pose_coke_can.position.y-0.05
        self._pose_place.position.z = self._pose_coke_can.position.z

        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._table_object_name)

    def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.15
        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.185 - 0.01
        p.pose.position.y = 0.0#-0.015 - 0.01
        p.pose.position.z = 0.20 + (0.030 + 0.003) / 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; based on
        server_test.py example on moveit_simple_grasps pkg.
        """

        # Create goal:
        goal = GenerateGraspsGoal()

        goal.pose  = pose
        goal.width = width

        options = GraspGeneratorOptions()
        # simple_graps.cpp doesn't implement GRASP_AXIS_Z!
        #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 (place locations), based on
        https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
        baxter_pick_place/src/block_pick_place.cpp
        """

        # 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)
            #zzz
            #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._table_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)

        # 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.zzzgrasps=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 = CokeCanPickAndPlace()

    rospy.spin()

if __name__ == '__main__':
    roscpp_initialize(sys.argv)
    rospy.init_node('pick_and_place')

    main()

    roscpp_shutdown()

pick_and_place.py 中CokeCanPickAndPlace类数值配置

#抓取目标宽度
        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.015)
#规划手臂组名称
        self._arm_group     = rospy.get_param('~arm', 'arm2')
#抓手名称
        self._gripper_group = rospy.get_param('~gripper', 'grap')
#接近和退回期望距离和最小距离
        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)
        self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.01)

注意这个接近和退回期望距离和最小距离同上

moveit_simple_grasps包源码中也需要更改一些数值

要根据diy手臂的有效运动范围,和抓取物体的大小合理进行配置
在grasp_data.cpp中
初始赋值为
 

approach_retreat_desired_dist_(0.6),
  approach_retreat_min_dist_(0.4),

自己的设置
 

// Nums接近和退回期望距离和最小距离
  approach_retreat_desired_dist_ = 0.2; // 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)负值是反方向?没测试
  //我理解这个depth需要根据抓手的大小和抓取物体的大小合理进行配置,如果距离过大物体过小可能会有抓不住的现象
  // 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="zzz_arm"/>

  <arg name="group"        default="arm2"/>
  <arg name="end_effector" default="grap"/>

  <node pkg="moveit_simple_grasps" type="moveit_simple_grasps_server" name="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 zzz_arm_2_pick_and_place)/config/$(arg robot)_grasp_data.yaml"/>
  </node>
</launch>

最后

运行moveit生成的demo.launch (moveit rviz可视化)

运行grasp_generator_server.launch ( 抓取位姿server )
运行pick_and_place.py (场景规划,抓放client,放置位姿)


规划运算量比较大,适当等待就出现自动抓放的演示了,(抓取目标时抓手会因zzz_arm_grasp_data.yaml设置的开合距离不当产生脱离模型的位移,这是插件演示时的正常现象).