baxter机器人抓取杯子Baxter_pick_cup_game-master.zip

baxter机器人抓取杯子Baxter_pick_cup_game-master.zip，进行抓取杯子的操作，自动识别，自动抓取，包括物体识别加运动规划

#!/usr/bin/env python
“““
Random select a cup. Approach to it. grab the cup. lift it a liite bit and then put it back
press ctrl-c to break the loop
input topic:
/cup_baxter_coordinates
    cup center pose in Baxter coordinate
    data structure: ros custom point array message

“““
import sys
import tf
import math
import rospy
import time
import struct
import numpy as np
import baxter_interface
import baxter_pykdl
import baxter_external_devices
import argparse

from dynamixel_msgs.msg import JointState
from std_msgs.msg import Float32
from std_msgs.msg import Float64
from std_msgs.msg import Header
from baxter_kdl.kdl_parser import kdl_tree_from_urdf_model
from urdf_parser_py.urdf import URDF
from moveit_commander import conversions
from baxter_core_msgs.srv import SolvePositionIK SolvePositionIKRequest
from geometry_msgs.msg import （PointPoseStampedPoseQuaternion）
from geometry_msgs.msg import Point
from position_calibration.msg import pointarray

from pynput.keyboard import Key Controller
from random import randint

cupCoordinates = np.array（[[0.00.00.0][0.00.00.0][0.00.00.0]]）
index = 0

# Robot arm move method. Grab from Baxter offical website
def IKMove（tranx trany tranz rotax rotay rotaz rotaw）:
        right_arm = baxter_interface.Limb（‘left‘）
        ns = “ExternalTools/“ + ‘left‘ + “/PositionKinematicsNode/IKService“
        iksvc = rospy.ServiceProxy（ns SolvePositionIK）
        ikreq = SolvePositionIKRequest（）
        hdr = Header（stamp=rospy.Time.now（） frame_id=‘base‘）
        poses = {
            ‘left‘: PoseStamped（
                header=hdr
                pose=Pose（
                    position=Point（
                        x=tranx
                        y=trany
                        z=tranz
                    ）
                    orientation=Quaternion（
                        x=rotax
                        y=rotay
                        z=rotaz
                        w=rotaw
                    ）
                ）
            ）
        }
 
        ikreq.pose_stamp.append（poses[‘left‘]）
        try:
            rospy.wait_for_service（ns 5.0）
            resp = iksvc（ikreq）
        except （rospy.ServiceException rospy.ROSException） e:
            rospy.logerr（“Service call failed: %s“ % （e））
            return 1
 
        # Check if result valid and type of seed ultimately used to get solution
        # convert rospy‘s string representation of uint8[]‘s to int‘s
        resp_seeds = struct.unpack（‘<%dB‘ % len（resp.result_type）
                                resp.result_type）
        if （resp_seeds[0] != resp.RESULT_INVALID）:
            seed_str = {
                        ikreq.SEED_USER: ‘User Provided Seed‘
                        ikreq.SEED_CURRENT: ‘Current Joint Angles‘
                        ikreq.SEED_NS_MAP: ‘Nullspace Setpoints‘
                    }.get（resp_seeds[0] ‘None‘）
            print（“SUCCESS - Valid Joint Solution Found from Seed Type: %s“ %
              

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-10-15 20:04  Baxter_pick_cup_game-master\
     文件         461  2018-10-15 20:04  Baxter_pick_cup_game-master\README.md
     目录           0  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\
     文件        6843  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\CMakeLists.txt
     目录           0  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\launch\
     文件         466  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\launch\controller_manager.launch
     文件        7513  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\launch\kinect2_bridge.launch
     文件         368  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\launch\start_tilt_controller.launch
     目录           0  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\msg\
     文件          41  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\msg\pointarray.msg
     文件        3243  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\package.xml
     文件         774  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\readme.txt
     目录           0  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\src\
     文件        6358  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\src\baxter_left_arm_move_new.py
     文件        2296  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\src\cup_detector.py
     文件       20237  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\src\cup_detector.xml
     文件        4694  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\src\location_convertor_three_cups.py
     文件         280  2018-10-15 20:04  Baxter_pick_cup_game-master\baxter_pickup_cup\tilt.yaml
     文件    25868695  2018-10-15 20:04  Baxter_pick_cup_game-master\pick_cup.gif