示例#1
0
def graspGripper(move_pos, move_speed=50):
    # move_pos in meter, move_speed in mm/s
    #WE should make grasp gripper force controlled--Nikhil
    # call gripper node, grasp
    move_pos=1000*move_pos
    gripper.move(move_pos, move_speed)
    return 1
def graspGripper(move_pos, move_speed=50):
    # move_pos in meter, move_speed in mm/s
    # WE should make grasp gripper force controlled--Nikhil
    # call gripper node, grasp
    move_pos = 1000 * move_pos
    gripper.move(move_pos, move_speed)
    return 1
示例#3
0
文件: helper.py 项目: fhogan/rgrasp
def moveGripper(move_pos, move_speed=50):
    #~publis gripper state

    jnames = ['gripper_command']
    gripper_msgs = sensor_msgs.msg.JointState()
    gripper_msgs.name = jnames
    gripper_msgs.position = [move_pos]
    gripper_msgs.velocity = [move_speed / 1000]
    gripper_msgs.effort = [np.inf]
    gripper_command_pub.publish(gripper_msgs)

    # move_pos in meter, move_speed in mm/s
    # call gripper node, grasp
    move_pos = 1000 * move_pos
    gripper.move(move_pos, move_speed)
    return 1
示例#4
0
    def run_grasping(self, container=None):
        self.getBestGraspingPoint(container)

        #~Publish grasp proposal information
        grasp_proposal_msgs = Float32MultiArray()
        grasp_proposal_msgs.data = self.grasp_point
        if self.grasp_point is not None:
            self.grasp_proposal_pub.publish(grasp_proposal_msgs)
            comments_msgs = String()
            comments_msgs.data = self.experiment_description
            experiment_type_msgs = String()
            experiment_type_msgs.data = self.experiment_type
            self.experiment_comments_pub.publish(comments_msgs)
            self.experiment_type_pub.publish(experiment_type_msgs)
        else:
            print('There are no grasp proposal')
            self.execution_possible = False
            return

        #~Visualize grasp proposals
        markers_msg = MarkerArray()
        m0 = createDeleteAllMarker('pick_proposals')
        markers_msg.markers.append(m0)
        for i in range(10):
            self.proposal_viz_array_pub.publish(markers_msg)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, self.all_grasp_proposals,
            self.listener, self.br)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
            self.listener, self.br, True)

        #execute for grasp. Stop when the gripper is closed
        self.back_img_list = self.capture_images()

        #self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]

        self.grasping_output = grasp(objInput=self.grasp_point,
                                     listener=self.listener,
                                     br=self.br,
                                     isExecute=self.isExecute,
                                     binId=container,
                                     withPause=self.withPause,
                                     viz_pub=self.proposal_viz_array_pub,
                                     recorder=self.gdr)
        num_it = 0
        print('gripper open: ', gripper.getGripperopening())
        is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
        while is_in_wrong_pose:
            '''
            self.retrieve_output = retrieve(listener=self.listener, br=self.br,
                                       isExecute=self.isExecute, binId=container,
                                       withPause=self.withPause, viz_pub=self.proposal_viz_array_pub,
                                       recorder=self.gdr, ws_object=self.weightSensor, isShake=False)
            self.grasping_output = grasp(objInput=self.grasp_point, listener=self.listener, br=self.br,
                                    isExecute=self.isExecute, binId=container, withPause=self.withPause,
                                    viz_pub=self.proposal_viz_array_pub, recorder=self.gdr, open_hand=False)
            '''
            # Motion heuristic
            initial_dz = 0.05
            dz = .022  #should be related to object length
            ik.helper.move_cart(dz=initial_dz)
            rospy.sleep(0.5)
            ik.helper.move_cart(dz=-initial_dz)
            rospy.sleep(0.5)
            gripper.move(90)
            ik.helper.move_cart(dz=dz)
            rospy.sleep(0.5)
            #should be done in the direction of the gripper plane
            dx = 0.038  #.02*num_it
            ik.helper.move_cart_hand(self.listener,
                                     dx=dx,
                                     dy=0,
                                     dz=0,
                                     speedName='fastest')
            rospy.sleep(0.5)
            ik.helper.move_cart(dz=-dz)
            rospy.sleep(0.5)
            ik.helper.move_cart_hand(self.listener,
                                     dx=-dx,
                                     dy=0,
                                     dz=0,
                                     speedName='fastest')
            rospy.sleep(0.5)
            gripper.close()
            rospy.sleep(0.5)
            print('gripper_open', gripper.getGripperopening())

            is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
            if is_in_wrong_pose:
                gripper.move(90)
                rospy.sleep(0.5)
                ik.helper.move_cart(dz=dz)
                rospy.sleep(0.5)
                ik.helper.move_cart_hand(self.listener,
                                         dx=-dx,
                                         dy=0,
                                         dz=0,
                                         speedName='fastest')
                rospy.sleep(0.5)
                ik.helper.move_cart(dz=-dz)
                rospy.sleep(0.5)
                ik.helper.move_cart_hand(self.listener,
                                         dx=dx,
                                         dy=0,
                                         dz=0,
                                         speedName='fastest')
                rospy.sleep(0.5)

                gripper.close()
                rospy.sleep(0.5)
                self.gdr.save_data_recorded = False
                num_it += 1
                is_in_wrong_pose = (
                    gripper.getGripperopening() < 0.04
                )  #and (gripper.getGripperopening() > 0.015)

        self.retrieve_output = retrieve(listener=self.listener,
                                        br=self.br,
                                        isExecute=self.isExecute,
                                        binId=container,
                                        withPause=self.withPause,
                                        viz_pub=self.proposal_viz_array_pub,
                                        recorder=self.gdr,
                                        ws_object=self.weightSensor,
                                        isShake=False)
        self.execution_possible = self.retrieve_output['execution_possible']
示例#5
0
def flush_grasp(objPose = [1.95,1.25,1.4,0,0,0,1],
            binNum=4,
            objId = 0,
            bin_contents = None,
            robotConfig = None, 
            forceThreshold = 1, 
            isExecute = True,
            withPause = False):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp
    joint_topic = '/joint_states'  
    ## initialize listener rospy
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035
    
    #tcp_offset_variables
    
    
    #set robot speed
    setSpeedByName(speedName = 'fast')
    
    # plan store
    plans = []
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
    obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7])
    obj_pose_tfm=np.array(obj_pose_tfm_list)
    
    obj_pose_orient=obj_pose_tfm[0:3,0:3]
    vertical_index=np.argmax(np.fabs(obj_pose_orient[2,0:3]))
    
    object_dim=get_obj_dim(objId)
    vertical_dim=object_dim[vertical_index]
    
    
    s1=np.fabs(obj_pose_orient[1,0])*object_dim[0]
    s2=np.fabs(obj_pose_orient[1,1])*object_dim[1]
    s3=np.fabs(obj_pose_orient[1,2])*object_dim[2]
    s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index]
    
    horizontal_dim=s1+s2+s3-s4

    hand_gap=0
    gripper.close()
        
    while True:
        APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() 
        q0 = APCrobotjoints.position
        if len(q0) < 6:
            continue
        else:
            break
    
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp
    vert_offset=.035
    l2 = 0.43  #change this to edit where you think the cup is
    l2 =.44
    
    tip_hand_transform = [-vert_offset, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    # for i in range(5):
        # rospy.sleep(0.1)
        # br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
    # rospy.sleep(0.1)
    pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    
    #this may cause issues!!!
    tcpPosHome = tcpPos
    
    
    # set scoop orientation (rotate wrist)
    
    
    
    
    
    distFromShelf = 0.05
    wristWidth = 0.0725 # this is actually half the wrist width
    (binMouth,bin_height,bin_width) = getBinMouth(distFromShelf, binNum)
    pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
    binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    
    
    object_depth=objPosition[0]-binMouth[0]
    
    
    finger_length=.23     
    finger_width=.06
    
    up_offset=.05
    down_offset=0.005
    cup_to_spatula=.08
    hand_width=.15
    max_gripper_width=.110
    hand_top_offset=hand_width/2+vert_offset+.03
    hand_bot_offset=hand_width/2-vert_offset+.015
    
    
    side_offset=.03
    
    binFloorHeight=binMouth[2]-bin_height/2
    binCeilHeight=binMouth[2]+bin_height/2
    
    
    min_height=binFloorHeight+lipHeight+finger_width/2+down_offset
    desired_height=objPosition[2]
    max_height=binCeilHeight-lipHeight-finger_width/2-down_offset
    
    target_height=desired_height
    if target_height<min_height:
        target_height=min_height
    if target_height>max_height:
        target_height=max_height
    
    bin_sideways=binMouth[1]
    sidepos=objPosition[1]
    
    horz_offset=0.015
    
    
    #this determines bin sides based on bin input
    binSmall=binMouth[1]-bin_width/2
    binLarge=binMouth[1]+bin_width/2
    
    #this determines bin sides based on object
    #(binSmall,binLarge)=getSides(objPosition[1],listener)
    
    
    
    
    
    binRight=binSmall
    binLeft=binLarge
    
    
    
    final_hand_gap=110
    
    if sidepos>bin_sideways:
        #this stuff is what happens if the object is to the left
        print 'Object is to the left'
        
        if binNum==0 or binNum==3 or binNum==6 or binNum==9:
            print 'Object is in that weird rail corner'
            return False
        
        #turn the suction cup so that it is sideways left
        #scoopOrientation = [.5,-.5,.5,-.5]
        scoopOrientation = [0.5,.5,.5,.5]
        #side_waypoint1=binRight+cup_to_spatula+.01
        #side_waypoint2=sidepos+horizontal_dim-side_offset
        #side_waypoint2=binLeft-horizontal_dim-horz_offset
        side_waypoint1=binLeft-hand_top_offset+horz_offset
        if sidepos-horizontal_dim<binLeft-final_hand_gap:
            print 'this is not a flush object'
            return False

        
        
    else:
        #this stuff is what happens if the object is to the right
        print 'Object is to the right'
        
        if binNum==2 or binNum==5 or binNum==8 or binNum==11:
            print 'Object is in that weird rail corner'
            return False
        
        #turn the suction cup so that it is sideways right
        #scoopOrientation = [0.5,.5,.5,.5]
        scoopOrientation = [.5,-.5,.5,-.5]
        #side_waypoint1=binLeft-cup_to_spatula-.01
        #side_waypoint2=sidepos-horizontal_dim+side_offset
        #side_waypoint2=binRight+horizontal_dim+horz_offset
        side_waypoint1=binRight+hand_top_offset-horz_offset
        
        if sidepos+horizontal_dim>binRight+final_hand_gap:
            print 'this is not a flush object'
            return False
        


        
    targetPositionList=[    
    [binMouth[0]-.15, side_waypoint1, min_height],
    [binMouth[0]+.04, side_waypoint1, min_height],
    [binMouth[0]+.3, side_waypoint1, min_height],
    [binMouth[0]+.3, side_waypoint1, min_height+.03],
    [binMouth[0]-.1, side_waypoint1, min_height+.03]]
    
    
    qf=q0
    
    
    for tp_index in range(0, len(targetPositionList)):
    
        targetPosition = targetPositionList[tp_index]
        frontOfObjectPtOutOfBin = targetPosition
        q_initial = qf
        planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            print 'move to COM in y successful'
            print 'tcp at:'
            print(targetPosition)
            #plan.visualize(hand_param=hand_gap)
            plans.append(plan)
            #if isExecute:
            #    pauseFunc(withPause)
            #    plan.execute()
        else:
            print 'move to COM in y fail'
            return False
            
        #print plan.q_traj
        qf = plan.q_traj[-1]
        print qf
    
    
    for numOfPlan in range(0, 2):
        if isExecute:
            plans[numOfPlan].visualize(hand_param=final_hand_gap)
            pauseFunc(withPause)
            plans[numOfPlan].execute()
            
    #time.sleep(2.5)
    
    #suction.start()
    
    final_hand_gap=110
    
    gripper.set_force(20)
    gripper.grasp(move_pos=final_hand_gap)
    
    for numOfPlan in range(2, 3):
        if isExecute:
            plans[numOfPlan].visualize(hand_param=hand_gap)
            pauseFunc(withPause)
            plans[numOfPlan].execute()
            
    gripper.set_force(50)
    rospy.sleep(0.1)
    gripper.move(move_pos=0)
    
    hand_gap=final_hand_gap
    print hand_gap
    
    ## retreat
    #for numOfPlan in range(0, len(plans)-1):
    #    plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
    #    if isExecute:
    #        pauseFunc(withPause)
    #        plans[len(plans)-numOfPlan-1].executeBackward()
    #        suction.stop()
    
    for numOfPlan in range(3, len(plans)):
        if isExecute:
            plans[numOfPlan].visualize(hand_param=hand_gap)
            pauseFunc(withPause)
            plans[numOfPlan].execute()
    
    return True
示例#6
0
def moveGripper(move_pos, move_speed=50):
    # move_pos in meter, move_speed in mm/s
    # call gripper node, grasp
    move_pos=1000*move_pos
    gripper.move(move_pos, move_speed)
    return 1
示例#7
0
def topple(objPose = [1.55,0.25,1.1,0,0,0,0],
            binNum=0,
            objId = 0, 
            bin_contents = None,
            robotConfig = None,
            shelfPosition = [1.9116,-0.012498,-0.4971],
            forceThreshold = 1,
            binDepthReach = 0.40,
            isExecute = True,
            withPause = True,
            withVisualize = True):
    ## objPose: world position and orientation frame attached to com of object
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp
    planSuccess = True
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    setSpeedByName(speedName = 'faster')
    joint_topic = '/joint_states'
        
    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035
    
    temp_bias = 0.00 # TO-DO remove this:
    
    # plan store
    plans = []
    
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp
    l1 = 0.018
    l2 = 0.470  
    tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    
    # broadcast frame attached to tcp
    pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
    
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
    closeGripper(forceThreshold) # if gripper open close it
    
    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf   = 0.05
    wristWidth      = 0.0725 # this is actually half the wrist width
    (binMouth, binFloorHeight)  = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world                  = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
    binFloorHeight              = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
    binFloorHeight              = binFloorHeight.pose.position.z
    binMouth                    = [pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    # set offset past COM
    # offset is 1/4 of object's height + half finger width
    # note: (COM height - bin floor height) is half of the object's height
    fingerWidth     = 0.06
    offsetFinger    = fingerWidth/2
    targetHeight    = (objPosition[2] - binFloorHeight)*2 # to be changed to be from an object
    
    offsetEffect = targetHeight*3/4

    offset = offsetEffect
    
    vertToppleHeight = binFloorHeight+offset-temp_bias
    
    ## bounding box constraint
    minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum) # shelf frame
    # trying to topple an object that is too tall?
    maxHeight = coordinateFrameTransform([0,0,maxHeight], 'shelf', 'map', listener)
    
    #pdb.set_trace()
    
    targetPosition = [binMouth[0], objPosition[1], vertToppleHeight]
    binDepthReach = 0.3 
    
    binHeightSaftey = 0.01
    if vertToppleHeight + binHeightSaftey > maxHeight.pose.position.z:
        fingerLength = 0.26
        binDepth = 0.42
        binDepthReach = binDepth - fingerLength + 0.08
        toppleHeightWhenTall = binMouth[2] + 0.03
        targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenTall]
        print '[Topple] Object too tall, topple will not go all the way in'
    
    # trying to topple an object that is too short?
    minHeight = coordinateFrameTransform([0,0,minHeight], 'shelf', 'map', listener)
    
    closeGripper(forceThreshold)
    
    if vertToppleHeight - binHeightSaftey < minHeight.pose.position.z:
        print '[Topple] Object too short, resetting topple height'
        toppleHeightWhenShort = minHeight.pose.position.z + binHeightSaftey 
        targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenShort]
        gripper.move(80,100)
    
    # too far left or right?
    leftWall = coordinateFrameTransform([leftWall,0,0], 'shelf', 'map', listener)
    leftWall = leftWall.pose.position.y
    rightWall = coordinateFrameTransform([rightWall,0,0], 'shelf', 'map', listener)
    rightWall = rightWall.pose.position.y
    
    binLengthSafety = 0.02
    horizontalSaftey = 0.01
    if targetPosition[1] + binLengthSafety > leftWall:
        targetPosition[1] = leftWall - horizontalSaftey
    
    if targetPosition[1] - binLengthSafety < rightWall:
        targetPosition[1] = rightWall + horizontalSaftey

    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    
    effect = 'topple'
    if s and effect == 'topple':
        print '[Topple] move to COM in y and above COM in z successful'
    elif s and effect == 'slide':
        print '[Topple] move to COM in y and below COM in z successful'
    else:
        print '[Topple] move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility
    
    if s:
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] move to COM in y and above COM in z fail'
        planSuccess = False
        
    qf = plan.q_traj[-1]
        
    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [binDepthReach,0,0])
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[Topple] stroke to end of bin success'
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] stroke to end of bin fail'
        planSuccess = False
    qf = plan.q_traj[-1]
    
    ## close gripper
    closeGripper(forceThreshold)
    
    ## retreat
    # go back along stroke
    q_initial = qf
    targetPosition = frontOfObjectPtOutOfBin
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[Topple] stroke to bin start success'
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] stroke to bin start fail'
        planSuccess = False
    qf = plan.q_traj[-1]
    
    # go back to initial tcp position
    q_initial = qf
    targetPosition = frontOfObjectPtOutOfBin
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[Topple] return to mouth success'
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] return to mouth fail'
        planSuccess = False
    
    if not planSuccess:
        return False, False
    
    ## execute
    #~ for numOfPlan in range(0, len(plans)):
        #~ plans[numOfPlan].visualize()
        #~ if isExecute:
            #~ pauseFunc(withPause)
            #~ plans[numOfPlan].execute()
    
    ## execute
    isNotInCollision = True
    for numOfPlan in range(0, len(plans)):
        plans[numOfPlan].visualize()
        if isExecute:
            pauseFunc(withPause)
            isNotInCollision = plans[numOfPlan].execute()
            if not isNotInCollision:
                print '[Topple] collision detected'
                planFailNum = numOfPlan
                break
                
    if not isNotInCollision:
        for numOfPlan in range(0, len(planFailNum)):
            plans[planFailNum-numOfPlan].executeBackward()
            
    ## retreat
    
    for numOfPlan in range(0, len(plans)):
        if withVisualize:
            plans[len(plans)-numOfPlan-1].visualizeBackward()
        if isExecute:
            pauseFunc(withPause)
            plans[len(plans)-numOfPlan-1].executeBackward()
            
    return True, False
def moveGripper(move_pos, move_speed=50):
    # move_pos in meter, move_speed in mm/s
    # call gripper node, grasp
    move_pos = 1000 * move_pos
    gripper.move(move_pos, move_speed)
    return 1