demo_graspsuccessExp.py 文件源码

def main(limb_name, reset):
    """
    Parameters
    ----------
    limb : str
        Which limb to use. Choices are {'left', 'right'}
    reset : bool
        Whether to use previously saved picking and placing poses or
        to save new ones by using 0g mode and the OK cuff buttons.
    """
    # Initialise ros node
    rospy.init_node("pick_and_place", anonymous=False)

    # Either load picking and placing poses from the parameter server
    # or save them using the 0g mode and the circular buttons on
    # baxter's cuffs
    if reset or not rospy.has_param('~pick_and_place_poses'):
        rospy.loginfo(
            'Saving picking pose for %s limb' % limb_name)
        pick_pose = limb_pose(limb_name)
        rospy.sleep(1)
        place_pose = limb_pose(limb_name)
        # Parameter server can't store numpy arrays, so make sure
        # they're lists of Python floats (specifically not
        # numpy.float64's). I feel that's a bug in rospy.set_param.
        rospy.set_param('~pick_and_place_poses',
                        {'pick': pick_pose.tolist(),
                         'place': place_pose.tolist()})
        #rospy.loginfo('pick_pose is %s' % pick_pose)
        #rospy.loginfo('place_pose is %s' % place_pose)
    else:
        pick_pose = rospy.get_param('~pick_and_place_poses/pick')
        place_pose = rospy.get_param('~pick_and_place_poses/place')

    b = Baxter(limb_name)

    c = Controller()
    c1 = Controller_1()
    c2 = Controller_2()
    #f = FilterValues()
    #f.start_recording()
    for i in range(20):
        print ('this iss the intial pick pose')
        pick_pose[1]= 0.25286245 #change this for every new exp
        print (pick_pose)
        #pick_pose[1] = 0.30986200091872873
        pick_pose[1] += random.uniform(-1,1)*0.00 ##introduce error in endposition (y axis)
        print ('ERROR introduced the intial pick pose')
        print (pick_pose)
        b.pick(pick_pose, controller=c, controller_1=None, controller_2 = c2)
        b.place(place_pose)
    #f.stop_recording()
    #f.filter()
    #f.plot()
    rospy.spin()