Example #1
0
class pick_and_place():
    def __init__(self, pick_id=0, place_id=1):
        
        self.pick_pose = [-999]*7
        self.pick_data = False
        self.place_pose = [-999]*7
        self.place_data = False
        
        self.subscriber = rospy.Subscriber('ar_pose_marker', AlvarMarkers, \
                self.ar_callback)
        self.ctrl = ArmController(event_detector=True)
        self.tf_transformer = TfTransformer(self.ctrl.tf_listener)
        self.joint_move_arm_to_side('r', blocking=False)
        self.joint_move_arm_to_side('l')
        self.default_frame = "base_link"
        self.grasper = ArghGrasp()
    """
    ### experimental code
    def pick_up(self, whicharm='l', top_grasp = False):
        rospy.loginfo("pre pickup pose")
        self.open_gripper(whicharm)

        first =    [1.042181865195266, 0.05674577918307219, 1.7141703410128142, -1.2368941648991192, 44.01523148337816, -1.3485333030083617, 58.22465995394929]
        self.ctrl.joint_movearm(whicharm, first)
        start = [0.6169387251649058, 0.13840014757000843, 0.7266251135486242, 0.7131394766246717, -0.7008370665851971, 0.008851073879117963, 0.013459252242671303]

        end = [0.591262509439721, -0.19429620128938668, 0.748909628486445, 0.7368007601058577, -0.6756313110616471, -0.01694214819160356, -0.01897195391107014]

        
        self.move_cartesian_step(whicharm, start)
        self.move_cartesian_step(whicharm, end, blocking=False)

        self.ctrl.gripper.start_gripper_event_detector(\
            whicharm, blocking=True,timeout = 5)
        self.ctrl.cart_freeze_arm(whicharm)
        self.close_gripper(whicharm)
        
        curr = self.ctrl.get_cartesian_pose()[whicharm]
        curr[2] += 0.05
        self.move_cartesian_step(whicharm,curr)
        self.constrained_move_arm_to_side(whicharm)
        return True
    """        


    def pick_up(self, whicharm='l'):
        if self.pick_data:
            self.open_gripper(whicharm) 
            if not self.grasper.pick_up(whicharm,5, self.pick_pose):
                return False
            # move arm to side
            rospy.loginfo("moving arm to side")
            self.constrained_move_arm_to_side(whicharm)
            return self.ctrl.gripper.is_grasping()[whicharm]
        else:
            return False
            
    def reset_ar(self):
        self.pick_data = False
        self.place_data = False

    def is_close(self, desired = .664, threshold=.76):
        x = self.place_pose[0]
        if x > threshold:
            return (False, x - desired)
        else:
            return (True, 0)

    def place(self, whicharm="l", top_grasp = True, use_offset = True):
        if self.place_data:
            # get place offset from cooler
            if use_offset:
                trans, _ = self.tf_transformer.get_transform(\
                'base_link', 'ar_marker_1')
                if not trans:
                    return False
            else:
                print "not finding place pose"
                trans = self.place_pose[:3]     
            curr_pose = self.ctrl.get_cartesian_pose()[whicharm]
            rot = curr_pose[3:]
            #_,rot = self.ctrl.get_cartesian_pose()[whicharm]

            place_pose = trans + rot
            over_pose, under_pose = list(place_pose), list(place_pose)
            over_pose[2] += .2
            under_pose[2] -= .2
            self.reset_ar()
            print "%s\n%s\n%s"%(place_pose, over_pose, under_pose)
            rospy.loginfo("moving arm over place pose")
            result = self.move_cartesian_step(whicharm,over_pose) 
            rospy.loginfo("waiting a second before placing")
            rospy.sleep(1) # wait a second before placing 
            if result:

                rospy.loginfo("guarded move arm to place pose")
                result = self.guarded_cartesian_move(whicharm, under_pose)
                
                if result:
                    rospy.loginfo("placing can")
                    self.open_gripper(whicharm)
            
                    rospy.loginfo("moving arm up over place pose")
                    self.move_cartesian_step(whicharm, over_pose)
                else:
                    rospy.loginfo("failed doing guarded move")    
            else:
                rospy.loginfo("failed moving arm over")

            """
            rospy.loginfo("moving arm up over place pose")
            tmp = False
            for i in range (5):
                if tmp:
                    break
                tmp = self.move_cartesian_step(whicharm, over_pose)
                if no`t tmp:
                    rospy.loginfo("having difficulty moving arm up "
                    "over place pose")
            if not tmp:
                raw_input("can't find over pose")
            """

            self.joint_move_arm_to_side(whicharm)
            rospy.loginfo("place result succes is: %s " % result)
            return result
        else:
            rospy.loginfo("no place data")
            return False
    
    def ar_callback(self, data):
        for markers in data.markers: 
            try:
                px = markers.pose.pose.position.x
                py = markers.pose.pose.position.y
                pz = markers.pose.pose.position.z
                ox = markers.pose.pose.orientation.x
                oy = markers.pose.pose.orientation.y
                oz = markers.pose.pose.orientation.z
                ow = markers.pose.pose.orientation.w

                if markers.id == 0: # pick
                    self.pick_pose = [px,py,pz,ox,oy,oz,ow]
                    self.pick_data = True
                if markers.id == 1: # place
                    self.place_pose = [px, py, pz, ox, oy, oz, ow]
                    self.place_data = True
            except:
                pass
    
    #open the gripper 
    def open_gripper(self, whicharm):
        self.ctrl.open_gripper(whicharm)


    #close the gripper 
    def close_gripper(self, whicharm):
        self.ctrl.close_gripper(whicharm)


    def constrained_move_arm_to_side(self, whicharm):
        location={'l': [0.05, 0.65, .7], 'r': [0.05, -0.65, .7]}
        poses = self.ctrl.get_cartesian_pose()
        rot = poses[whicharm][3:]
        side_pose = location[whicharm] + rot
        result = self.move_cartesian_step(whicharm, side_pose, blocking=True)
        if not result:
            rospy.loginfo("cannot constrain joint moving to side")
            self.joint_move_arm_to_side(whicharm)

   
    def joint_move_arm_to_side(self, whicharm, blocking = 1):
        self.arm_to_side_angles = \
                {'r': [-2.02, 0.014, -1.73, -1.97, 10.91, -1.42, 14.17],
                 'l': [2.02, 0.014, 1.73, -1.97, -10.91, -1.42, 14.17]}
        self.ctrl.joint_movearm(whicharm,self.arm_to_side_angles[whicharm],\
                blocking=blocking)
 
    def guarded_cartesian_move(self, whicharm, pose, frame="base_link",\
            move_duration = 15):
        start_pose = self.ctrl.get_cartesian_pose()[whicharm][:3]
        self.move_cartesian_step(whicharm, pose, move_duration=7.0,\
                    frame_id=frame, blocking=False)
        self.ctrl.gripper.start_gripper_event_detector(\
            whicharm, blocking=True,timeout=move_duration)
        self.ctrl.cancel_goal(whicharm)
        # did arm even move?    
        curr_pose = self.ctrl.get_cartesian_pose()[whicharm][:3]
        diff = np.linalg.norm(np.array(start_pose) - np.array(curr_pose))
        not_at_start = diff > 0.1
        return not_at_start
        
    #move to a Cartesian pose goal
    def move_cartesian_step(self, whicharm, pose, frame_id="base_link",\
            move_duration=5.0, ik_timeout=5.0, blocking=True):

        result = self.ctrl.cart_movearm(whicharm, pose, frame_id, \
        move_duration, ik_timeout, blocking) 
        return result
Example #2
0
class Primitives:
    block_size = 1e-3 * 13.0 # mm
    W,H = 10.0, 20.0 # tetris grid units
    z_above = 0.07
    z_down = 0.03
    sqr2 = np.sqrt(2.0)/2.0
    sqr7 = np.sqrt(7.0)/4.0

    vert = [-.5, .5, .5, .5]
    horz = [0, sqr2, 0, sqr2]
    #rot_r = [-sqr7, -.25, sqr7, -.25]
    rot_r = [-.25, sqr7, 0.25, sqr7]
    rot_l = [sqr7, -.25, -sqr7, -.25]

    init_pose = {
        #'r': [-0.73, -0.17, -1.60, -1.46, -32.69, -1.31, -16.94],
        #'l': [ 0.71, -0.04, 1.56, -1.46, -4.72, -1.36, 3.86]}
        'l': [ 0.95, 0.0, np.pi/2., -np.pi/2., -np.pi*1.5, -np.pi/2.0, np.pi],
        'r': [ -0.7, 0.0, -np.pi/2., -np.pi/2., -20.424894658897493, -np.pi/2.0, np.pi]}
    min_time = 1.0
    max_time = 5.0
    frame = 'tetris'

    def __init__(self, whicharm='l'):
        self.arm = ArmController()
        self.objects = None
        self.obj_sub = rospy.Subscriber("/table_publisher/object_poses", PoseArray, self.objCb)
        self.arm.command_torso(0.26)
        self.whicharm = whicharm
        self.move_arm_to_side()
      
    def move_arm_to_side(self):
        self.arm.joint_movearm(self.whicharm, self.init_pose[self.whicharm],
                3, True)
   
    def lift_arm(self, rating=1.0):
        rospy.sleep(0.1)
        curr_pos  = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
        curr_pos[2] = self.z_above *rating
        goal =[ self.stiff_pose(curr_pos, 0.1) + [0.25*self.min_time*rating], 
                 self.stiff_pose(curr_pos, 0.5) + [0.5*self.min_time*rating],
                 self.stiff_pose(curr_pos, 1.0) + [self.min_time*rating] ]
        self.arm.cart_movearm(self.whicharm, goal , self.frame, True)
        #raw_input("done lift")
   
    def stiff_pose(self, pose, rating = 0.2):
        pose_force = int(100*rating)
        torque_force = int(50*rating)
        return list(pose) + [pose_force]*3 + [torque_force]*3 + [False]*6
    
       
    def distance_between_two_pts(self, p1,p2):
        pts = zip(p1,p2)
        dx = [(x1 - x2)**2 for (x1,x2) in pts]
        return sum(dx)  

    def clip_time(self, t):
        if t < self.min_time:
            return self.min_time
        elif t > self.max_time:
            return self.max_time
        else:
            return t

    def approach_pose(self, pose):
        curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
        time = self.clip_time(15*self.distance_between_two_pts(curr_pose, pose))
        curr_pose[2] = self.z_above
        middle_pose = list(pose)
        middle_pose[2] = self.z_above

        traj = [
                self.stiff_pose(curr_pose, 0.1) + [time*.1],
                self.stiff_pose(curr_pose, 1.0) + [time*.2],
                self.stiff_pose(middle_pose, 1.5) + [time*.8],
                self.free_push(pose, 1.5) + [time]
                ]

        self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
        rospy.sleep(0.1)
        #final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
        #dist = self.distance_between_two_pts(pose, final_pose)

        
    def do_force_control(self, cmd, timeout=4.0):
        self.approach_pose(cmd[:7])
        rospy.sleep(0.1)

        curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]

        ros_timeout = rospy.Time.now() + rospy.Duration(timeout)
        self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False)
        rospy.sleep(0.2)
        while rospy.Time.now() < ros_timeout and not rospy.is_shutdown():
            rospy.sleep(0.1)
            if not self.arm.is_moving(self.whicharm):
                self.arm.cancel_goal(self.whicharm)
                break
        
        """ 
        final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
        if self.distance_between_two_pts(curr_pose, final_pose) < self.block_size:
            ros_timeout = rospy.Time.now() + rospy.Duration(timeout)
            self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False)
            rospy.sleep(1.0)
            while rospy.Time.now() < ros_timeout:
                rospy.sleep(0.1)
                if not self.arm.is_moving(self.whicharm):
                    self.arm.cancel_goal(self.whicharm)
                    break
        """

        self.lift_arm()

    def free_space_push_down(self, pos1, pos2):
        pose1 =  list(pos1) + self.horz
        pose2 =  list(pos2) + self.horz
        time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2))
        self.approach_pose(pose1)

        traj = [
                self.free_push(pose1) + [time*.1],
                self.free_push(pose2) + [time]
                ]

        self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
        self.lift_arm(1.)

    def free_space_push_right(self, pos1, pos2):
        pose1 =  list(pos1) + self.vert
        pose2 =  list(pos2) + self.vert
        time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2))
        self.approach_pose(pose1)

        traj = [
                self.free_push(pose1) + [time*.1],
                self.free_push(pose2) + [time]
                ]

        self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
        self.lift_arm(1.)


    def free_push(self, pose, rating=1.0):  # position controlled
        fx,fy,fz = [300*rating]*3  # max stiffness
        ox,oy,oz = [10*rating]*3
        fz = -5*rating  # force in downard direction
        states = [False]*6
        states[2] = True # use force control for Z
        return list(pose) + [fx,fy,fz,ox,oy,oz] + states 

    def right_contact_slide_down(self, pos, rating=1.0):
        pose = list(pos) + self.rot_r
        #use force control
        fx = -7*rating
        fy =  -3 * rating
        fz = -2* rating
        #keep gripper rotated
        ox,oy,oz = [30]*3
        states =[True, True, True] +  [False]*3
        cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states 
        self.do_force_control(cmd)
    
    def left_contact_slide_down(self, pos, rating=1.0):
        pose = list(pos) + self.rot_l
        #use force control
        fx = -7*rating
        fy =  2*rating
        fz = -2*rating
        #keep gripper rotated
        ox,oy,oz = [30]*3
        states =[True, True, True] +  [False]*3
        cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states 
        self.do_force_control(cmd)
    
    def right_contact_slide_right(self, pos, rating=1.0):
        pose = list(pos) + self.rot_r
        #use force control
        fx = -2*rating
        fy =  -5*rating
        fz = -2*rating
        #keep gripper rotated
        ox,oy,oz = [30]*3
        states =[True, True, True] +  [False]*3
        cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states 
        self.do_force_control(cmd)

    def push_down(self, pos, rating=1.0):
        pose =  list(pos) + self.horz
        #use force control
        fx = -7*rating
        fy = 300*rating
        fz = -3*rating
        #keep gripper rotated
        ox,oy,oz = [30]*3
        states =[True, False, True] +  [False]*3
        cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states 
        self.do_force_control(cmd)

    
    def push_right(self, pos, rating=1.0):
        pose =  list(pos) + self.vert
        #use force control
        fx = 400*rating
        fy =  -5 *rating
        fz = -3*rating
        #keep gripper rotated
        ox,oy,oz = [30]*3
        states =[False, True, True] +  [False]*3
        cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states 
        self.do_force_control(cmd)

    def push_left(self, pos, rating=1.0):
        pose =  list(pos) + self.vert
        #use force control
        fx = 300*rating
        fy =  5 *rating
        fz = -3*rating
        #keep gripper rotated
        ox,oy,oz = [30]*3
        states =[False, True, True] +  [False]*3
        cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states 
        self.do_force_control(cmd)



    def convert_xy(self, (x,y) ):
        return (x*self.block_size, y*self.block_size, self.z_down)