Пример #1
0
def take_off_dopple(pt,arm):
    x = DOPPLE_X
    y = DOPPLE_Y
    if arm == "l":
        yaw = -pi/2
        y -= 0.005
    else:
        yaw = pi/2
        y += 0.005
    z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045
    frame = "base_footprint"
    GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm=arm,grip=False,frame=frame,dur=5.0)
    GripUtils.close_gripper(arm,extra_tight=False)
    GripUtils.go_to(x=x,y=y,z=z+0.1,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
    GripUtils.go_to(x=x,y=y,z=z+0.2,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
    roll = 0
    if arm=="l":
        yaw = -pi/2
    else:
        yaw = pi/2
    GripUtils.go_to(x=x-0.15,y=y,z=z+0.2,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
    #Spreads out
    GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,z_offset = 0.2,y_offset=0.2,dur=5.0) # previously z_o= 0.08 y_o 0.1
    GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/2,arm=arm,grip=True,dur=7.5,y_offset = -0.2,z_offset=0.015)
    GripUtils.open_gripper(arm)
    GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/4,arm=arm,grip=False,dur=2.5,y_offset = -0.2,z_offset=0.025)
    StanceUtils.call_stance("arms_up",5.0)
Пример #2
0
 def adjust_fold(self, req):
     #Go to viewing stance
     StanceUtils.call_stance("open_both", 5.0)
     StanceUtils.call_stance("viewing", 5.0)
     last_model = pickle.load(open("/tmp/last_model.pickle"))
     camera_model = image_geometry.PinholeCameraModel()
     info = RosUtils.get_next_message("wide_stereo/left/camera_info",
                                      CameraInfo)
     cam_frame = info.header.frame_id
     camera_model.fromCameraInfo(info)
     now = rospy.Time.now()
     req.start.header.stamp = now
     req.end.header.stamp = now
     self.listener.waitForTransform(cam_frame, req.start.header.frame_id,
                                    now, rospy.Duration(20.0))
     start_cam = self.listener.transformPoint(cam_frame, req.start)
     end_cam = self.listener.transformPoint(cam_frame, req.end)
     start = camera_model.project3dToPixel(
         (start_cam.point.x, start_cam.point.y, start_cam.point.z))
     end = camera_model.project3dToPixel(
         (end_cam.point.x, end_cam.point.y, end_cam.point.z))
     folded_model = Models.Point_Model_Folded(last_model, start, end)
     folded_model.image = None
     folded_model.initial_model.image = None
     pickle.dump(folded_model, open("/tmp/last_model.pickle", 'w'))
     adjust_folded_model = rospy.ServiceProxy(
         "fold_finder_node/process_mono", ProcessMono)
     resp = adjust_folded_model("wide_stereo/left")
     new_start = resp.pts3d[0]
     new_end = resp.pts3d[1]
     return AdjustFoldResponse(start=new_start, end=new_end)
Пример #3
0
def main(args):
    rospy.init_node("unfolding_click")

    StanceUtils.call_stance("look_down", 3.0)
    StanceUtils.call_stance("arms_up", 3.0)
    while True and not rospy.is_shutdown():
        process_mono = rospy.ServiceProxy("click_node/process_mono",
                                          ProcessMono)
        resp = process_mono("wide_stereo/left")
        pt = resp.pts3d[0]
        z_offset = 0.06
        link_frame = "r_wrist_roll_link"
        GripUtils.go_to_pt(pt,
                           roll=pi / 2,
                           yaw=0,
                           pitch=pi / 2,
                           arm="r",
                           z_offset=z_offset,
                           grip=True,
                           dur=5.0,
                           link_frame=link_frame)
        z_offset = 0.00
        GripUtils.go_to_pt(pt,
                           roll=pi / 2,
                           yaw=0,
                           pitch=pi / 2,
                           arm="r",
                           z_offset=z_offset,
                           grip=True,
                           dur=5.0,
                           link_frame=link_frame)
        rospy.sleep(5.0)
Пример #4
0
 def adjust_fold(self,req):
     #Go to viewing stance
     StanceUtils.call_stance("open_both",5.0)
     StanceUtils.call_stance("viewing",5.0)
     last_model = pickle.load(open("/tmp/last_model.pickle"))
     camera_model = image_geometry.PinholeCameraModel()
     info = RosUtils.get_next_message("wide_stereo/left/camera_info",CameraInfo)
     cam_frame = info.header.frame_id
     camera_model.fromCameraInfo(info)
     now = rospy.Time.now()
     req.start.header.stamp = now
     req.end.header.stamp=now
     self.listener.waitForTransform(cam_frame,req.start.header.frame_id,now,rospy.Duration(20.0))
     start_cam = self.listener.transformPoint(cam_frame,req.start)
     end_cam = self.listener.transformPoint(cam_frame,req.end)
     start = camera_model.project3dToPixel((start_cam.point.x,start_cam.point.y,start_cam.point.z))
     end = camera_model.project3dToPixel((end_cam.point.x,end_cam.point.y,end_cam.point.z))
     folded_model = Models.Point_Model_Folded(last_model,start,end)
     folded_model.image = None
     folded_model.initial_model.image = None
     pickle.dump(folded_model,open("/tmp/last_model.pickle",'w'))
     adjust_folded_model = rospy.ServiceProxy("fold_finder_node/process_mono",ProcessMono)
     resp = adjust_folded_model("wide_stereo/left")
     new_start = resp.pts3d[0]
     new_end = resp.pts3d[1]
     return AdjustFoldResponse(start=new_start,end=new_end)
Пример #5
0
def grab_far_right(arm):
    StanceUtils.call_stance("look_down",5.0)
    rospy.sleep(2.5)
    process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",ProcessMono)
    resp = process_mono("wide_stereo/left")
    pt = resp.pts3d[0]
    GripUtils.grab_point(pt,roll=-pi/2,yaw=pi/2,arm=arm,z_offset = 0.02,x_offset = -0.01)
    return pt
Пример #6
0
def pickup_clump(arm):
    init_pt = initial_pickup("r")
    GripUtils.go_to_pt(init_pt,
                       roll=pi / 2,
                       yaw=0,
                       pitch=pi / 2,
                       arm="r",
                       grip=True,
                       z_offset=0.08,
                       dur=3.0)
    GripUtils.go_to_pt(init_pt,
                       roll=pi / 2,
                       yaw=0,
                       pitch=pi / 2,
                       arm="r",
                       grip=True,
                       z_offset=0.01,
                       y_offset=0.1,
                       dur=3.0)
    GripUtils.go_to_pt(init_pt,
                       roll=pi / 2,
                       yaw=0,
                       pitch=pi / 2,
                       arm="r",
                       grip=True,
                       z_offset=0.01,
                       y_offset=-0.1,
                       dur=3.0)
    StanceUtils.call_stance("arms_up", 5.0)
    seam_pt = grab_far_left("l")
    GripUtils.go_to_pt(seam_pt,
                       roll=pi / 2,
                       yaw=0,
                       pitch=pi / 2,
                       arm="l",
                       grip=True,
                       z_offset=0.08,
                       dur=3.0)
    GripUtils.go_to_pt(seam_pt,
                       roll=pi / 2,
                       yaw=0,
                       pitch=pi / 2,
                       arm="l",
                       grip=True,
                       z_offset=0.01,
                       y_offset=+0.1,
                       dur=3.0)
    GripUtils.go_to_pt(seam_pt,
                       roll=pi / 2,
                       yaw=0,
                       pitch=pi / 2,
                       arm="l",
                       grip=True,
                       z_offset=0.01,
                       y_offset=-0.1,
                       dur=3.0)
    StanceUtils.call_stance("arms_up", 5.0)
Пример #7
0
def pickup_clump(arm):
    init_pt = initial_pickup("r")
    GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.08,dur=3.0)
    GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.01,y_offset=0.1,dur=3.0)
    GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.01,y_offset=-0.1,dur=3.0)
    StanceUtils.call_stance("arms_up",5.0)
    seam_pt = grab_far_left("l")
    GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.08,dur=3.0)
    GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.01,y_offset=+0.1,dur=3.0)
    GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.01,y_offset=-0.1,dur=3.0)
    StanceUtils.call_stance("arms_up",5.0)
Пример #8
0
 def locate_polygon(self,req):
     #Go to viewing stance
     StanceUtils.call_stance("open_both",2.0)
     StanceUtils.call_stance("viewing",2.0)
     locate_poly = rospy.ServiceProxy("shape_fitter_node/process_mono",ProcessMono)
     print "Calling locate_poly"
     resp = locate_poly("wide_stereo/left")
     print "Received response"
     pts = resp.pts3d
     for pt in pts:
         self.publish(pt)
     return LocatePolygonResponse()
Пример #9
0
 def locate_polygon(self, req):
     #Go to viewing stance
     StanceUtils.call_stance("open_both", 2.0)
     StanceUtils.call_stance("viewing", 2.0)
     locate_poly = rospy.ServiceProxy("shape_fitter_node/process_mono",
                                      ProcessMono)
     print "Calling locate_poly"
     resp = locate_poly("wide_stereo/left")
     print "Received response"
     pts = resp.pts3d
     for pt in pts:
         self.publish(pt)
     return LocatePolygonResponse()
Пример #10
0
def grab_far_right(arm):
    StanceUtils.call_stance("look_down", 5.0)
    rospy.sleep(2.5)
    process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",
                                      ProcessMono)
    resp = process_mono("wide_stereo/left")
    pt = resp.pts3d[0]
    GripUtils.grab_point(pt,
                         roll=-pi / 2,
                         yaw=pi / 2,
                         arm=arm,
                         z_offset=0.02,
                         x_offset=-0.01)
    return pt
Пример #11
0
def pair_socks():
    
    pickup_sock("l")
    open_sock()
    put_on_dopple(depth=0.28)
    StanceUtils.call_stance("arms_up",5.0)
    pickup_sock("l") #Node: we do "l" because this is now the only sock on the table. It is now the "left" one.
    open_sock()
    
    put_on_dopple(depth=DOPPLE_HEIGHT-0.1)
    #put_on_dopple(depth=0.29)
    StanceUtils.call_stance("arms_up",5.0)
    take_off_dopple_pair()
    return
Пример #12
0
def main(args):
    rospy.init_node("unfolding_click")

    StanceUtils.call_stance("look_down",3.0)
    StanceUtils.call_stance("arms_up",3.0)
    while True and not rospy.is_shutdown():
        process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono)
        resp = process_mono("wide_stereo/left")
        pt = resp.pts3d[0]
        z_offset = 0.06
        link_frame = "r_wrist_roll_link"
        GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame)
        z_offset = 0.00
        GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame)
        rospy.sleep(5.0)
Пример #13
0
def pair_socks():

    pickup_sock("l")
    open_sock()
    put_on_dopple(depth=0.28)
    StanceUtils.call_stance("arms_up", 5.0)
    pickup_sock(
        "l"
    )  #Node: we do "l" because this is now the only sock on the table. It is now the "left" one.
    open_sock()

    put_on_dopple(depth=DOPPLE_HEIGHT - 0.1)
    #put_on_dopple(depth=0.29)
    StanceUtils.call_stance("arms_up", 5.0)
    take_off_dopple_pair()
    return
 def execute_primitive(self, primitive, req):
     for weight_point in primitive.weight_points:
         stance_name = weight_point.stance_name
         dur = weight_point.dur
         params = weight_point.params
         resp = StanceUtils.call_stance(stance_name, dur, params)
         #if not resp:
         #    return ExecutePrimitiveResponse(False)
     return ExecutePrimitiveResponse(True)
Пример #15
0
def initial_pickup(arm):
    success = True
    StanceUtils.call_stance("look_down",5.0)
    rospy.sleep(2.5)
    """
    process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
    resp = process_mono("wide_stereo/left")
    pt = resp.pts3d[0]
    z_offset = 0.02
    GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
    GripUtils.close_gripper(arm)
    while not GripUtils.has_object(arm):
        z_offset -= 0.005
        GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
        GripUtils.close_gripper(arm)
    """
    pt = grab_far_right(arm)
    return pt
Пример #16
0
def initial_pickup(arm):
    success = True
    StanceUtils.call_stance("look_down", 5.0)
    rospy.sleep(2.5)
    """
    process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
    resp = process_mono("wide_stereo/left")
    pt = resp.pts3d[0]
    z_offset = 0.02
    GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
    GripUtils.close_gripper(arm)
    while not GripUtils.has_object(arm):
        z_offset -= 0.005
        GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm=arm,z_offset=z_offset,grip=False,dur=5.0)
        GripUtils.close_gripper(arm)
    """
    pt = grab_far_right(arm)
    return pt
 def execute_primitive(self,primitive,req):
     for weight_point in primitive.weight_points:
         stance_name = weight_point.stance_name
         dur = weight_point.dur
         params = weight_point.params
         resp = StanceUtils.call_stance(stance_name,dur,params)
         #if not resp:
         #    return ExecutePrimitiveResponse(False)
     return ExecutePrimitiveResponse(True)
Пример #18
0
   def executeSingleGrip(self, fold_traj):
       print "hi from single grip"
       approach_point = fold_traj.approach_points[0]
       grip_point = fold_traj.grip_points[0]
       quarter_point = fold_traj.quarter_points[0]
       vertical_point = fold_traj.vertical_points[0]
       goal_point = fold_traj.goal_points[0]
       smooth_center = fold_traj.smooth_center
       smooth_edges = fold_traj.smooth_edges
       first_adjustment = False
       tilt = fold_traj.tilts[0]
       roll = -pi/2 #pi/2
       pitch = pi/4
       if fold_traj.red:
           tilt = self.last_traj.tilts[0]%(2*pi)
       rospy.loginfo("Tilt: %s"%tilt)
       
       #We do this so that we don't let go of the vertex during a red fold
       if not fold_traj.red:
           self.executeSmooth(self.last_traj)
           StanceUtils.open_gripper("b")
           self.fold_stance("b")
           arm = self.getArm(fold_traj)
           #Move the unused arm out of the way
           if arm=="l":
               StanceUtils.call_stance("viewing_right",5.0)
           else:
               StanceUtils.call_stance("viewing_left",5.0)
 
           (target,nothing) = self.gripPoints(point=approach_point,arm=arm,tilt=tilt,grip=False,preferred_pitch = pi/4,preferred_roll=roll,red=fold_traj.red)
           pitch = target.pitch
           self.gripPoints(point=grip_point,arm=arm,tilt=tilt,grip=False,preferred_pitch = pi/4,preferred_roll=roll,red=fold_traj.red,grab=True)
           
           StanceUtils.close_gripper(arm)
           
       else:
           arm = self.last_arm
       StanceUtils.close_gripper(arm)
       self.gripPoints(point=vertical_point,grip=True,tilt=tilt,arm=arm,preferred_pitch=pi/2,preferred_roll=roll,red=fold_traj.red)
       self.gripPoints(point=goal_point,grip=True,tilt=tilt,arm=arm,preferred_pitch=3*pi/5,preferred_roll=roll,red=fold_traj.red)
       
       self.last_traj = fold_traj
       self.last_arm = arm
Пример #19
0
def get_grip_point(sock):
    StanceUtils.call_stance("look_down",5.0)
    rospy.sleep(2.5)
    print "Give me the grip point"
    process_mono = rospy.ServiceProxy("click_point_node/process_mono",ProcessMono)
    resp = process_mono("wide_stereo/left")
    resp.param_names = ("l","r")
    resp.params = (0.0,0.0)
    output = {}
    for i,val in enumerate(resp.params):
        output[resp.param_names[i]] = val
    
    if sock == "l":
        pt = resp.pts3d[0]
        angle = -1*output["l"]
    else:
        pt = resp.pts3d[1]
        angle = -1*output["r"]
    return (pt,angle)
Пример #20
0
def pickup_sock(sock):
    z_off = 0.0
    """
    while not rospy.is_shutdown():
        StanceUtils.call_stance("close_right",5.0)
        (grip_pt,angle) = get_grip_point(sock)
        GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off)
        rospy.sleep(5.0)
        z_off = float(raw_input("What is the new z_offset?"))
        StanceUtils.call_stance("arms_up",5.0)
    """
    (grip_pt,angle) = get_grip_point(sock)
    if MODE==THICK_SOCK:
        OFFSET = 0.02
    else:
        OFFSET = 0.025
    
    y_offset = OFFSET*cos(angle)
    x_offset = OFFSET*sin(angle)
    #x_offset += 0.005
    z_offset = 0.02
    pitch = pi/2
    roll = pi/2
    yaw = pi/2-angle
    GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
    z_offset = -0.02
    z_offset -= 0.001
    GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
    #Drag thin socks
    if MODE == THIN_SOCK:
        y_offset += 0.02
        GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
    while not GripUtils.has_object("r"):
        StanceUtils.call_stance("open_right",2.0)
        pitch -= ANGLE_INCREMENT
        y_offset -= 0.005*cos(angle)
        x_offset -= 0.005*sin(angle)
        z_offset += 0.0015
        GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
        GripUtils.close_gripper("r",extra_tight=True)
        break
    return grip_pt
Пример #21
0
def get_grip_point(sock):
    StanceUtils.call_stance("look_down", 5.0)
    rospy.sleep(2.5)
    print "Give me the grip point"
    process_mono = rospy.ServiceProxy("click_point_node/process_mono",
                                      ProcessMono)
    resp = process_mono("wide_stereo/left")
    resp.param_names = ("l", "r")
    resp.params = (0.0, 0.0)
    output = {}
    for i, val in enumerate(resp.params):
        output[resp.param_names[i]] = val

    if sock == "l":
        pt = resp.pts3d[0]
        angle = -1 * output["l"]
    else:
        pt = resp.pts3d[1]
        angle = -1 * output["r"]
    return (pt, angle)
Пример #22
0
 def fold_stance(self,arm):
     if arm=="b":
         StanceUtils.call_stance("folding",3.0)
     elif arm=="l":
         StanceUtils.call_stance("folding_left",3.0)
     elif arm=="r":
         StanceUtils.call_stance("folding_right",3.0)
Пример #23
0
 def fold_stance(self, arm):
     if arm == "b":
         StanceUtils.call_stance("folding", 3.0)
     elif arm == "l":
         StanceUtils.call_stance("folding_left", 3.0)
     elif arm == "r":
         StanceUtils.call_stance("folding_right", 3.0)
Пример #24
0
def initRobot():
    # Look Down
    if not StanceUtils.call_stance('look_down',5.0):
        print "Look Down: Failure"
        return False
      
    # ARMS UP
    height = 0.35
    lateral_amount = 0.65
    forward_amount = 0.3
    if not GripUtils.go_to_multi(   x_l=forward_amount, y_l=lateral_amount, z_l=height,
                                    roll_l=0, pitch_l=0, yaw_l=0, grip_l=False,
                                    x_r=forward_amount, y_r=-lateral_amount, z_r=height,
                                    roll_r=0, pitch_r=0, yaw_r=0, grip_r=False,
                                    frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0):
        return False
    else:
        return True
Пример #25
0
 def viewing_stance(self,arm):
     StanceUtils.call_stance("viewing",3.0)
Пример #26
0
 def execute(self, userdata):
     if not StanceUtils.call_stance(self.stance_name, self.dur):
         return FAILURE
     else:
         return SUCCESS
Пример #27
0
def take_off_dopple(pt, arm):
    x = DOPPLE_X
    y = DOPPLE_Y
    if arm == "l":
        yaw = -pi / 2
        y -= 0.005
    else:
        yaw = pi / 2
        y += 0.005
    z = DOPPLE_HEIGHT + TABLE_HEIGHT - 0.045
    frame = "base_footprint"
    GripUtils.go_to(x=x,
                    y=y,
                    z=z,
                    roll=0,
                    yaw=yaw,
                    pitch=pi / 4,
                    arm=arm,
                    grip=False,
                    frame=frame,
                    dur=5.0)
    GripUtils.close_gripper(arm, extra_tight=False)
    GripUtils.go_to(x=x,
                    y=y,
                    z=z + 0.1,
                    roll=0,
                    yaw=yaw,
                    pitch=0,
                    arm=arm,
                    grip=True,
                    frame=frame,
                    dur=5.0)
    GripUtils.go_to(x=x,
                    y=y,
                    z=z + 0.2,
                    roll=0,
                    yaw=yaw,
                    pitch=0,
                    arm=arm,
                    grip=True,
                    frame=frame,
                    dur=5.0)
    roll = 0
    if arm == "l":
        yaw = -pi / 2
    else:
        yaw = pi / 2
    GripUtils.go_to(x=x - 0.15,
                    y=y,
                    z=z + 0.2,
                    roll=roll,
                    yaw=yaw,
                    pitch=0,
                    arm=arm,
                    grip=True,
                    frame=frame,
                    dur=5.0)
    #Spreads out
    GripUtils.go_to_pt(pt,
                       roll=roll,
                       yaw=yaw,
                       pitch=0,
                       arm=arm,
                       grip=True,
                       z_offset=0.2,
                       y_offset=0.2,
                       dur=5.0)  # previously z_o= 0.08 y_o 0.1
    GripUtils.go_to_pt(pt,
                       roll=roll,
                       yaw=yaw,
                       pitch=pi / 2,
                       arm=arm,
                       grip=True,
                       dur=7.5,
                       y_offset=-0.2,
                       z_offset=0.015)
    GripUtils.open_gripper(arm)
    GripUtils.go_to_pt(pt,
                       roll=roll,
                       yaw=yaw,
                       pitch=pi / 4,
                       arm=arm,
                       grip=False,
                       dur=2.5,
                       y_offset=-0.2,
                       z_offset=0.025)
    StanceUtils.call_stance("arms_up", 5.0)
Пример #28
0
def flip_sock(pt, arm):
    wiggle_down_dopple()
    StanceUtils.call_stance("arms_up", 5.0)
    take_off_dopple(pt, arm=arm)
    smooth(pt, arm)
    StanceUtils.call_stance("arms_up", 5.0)
Пример #29
0
    def executeSingleGrip(self, fold_traj):
        approach_point = fold_traj.approach_points[0]
        grip_point = fold_traj.grip_points[0]
        quarter_point = fold_traj.quarter_points[0]
        vertical_point = fold_traj.vertical_points[0]
        goal_point = fold_traj.goal_points[0]
        smooth_center = fold_traj.smooth_center
        smooth_edges = fold_traj.smooth_edges
        first_adjustment = False
        tilt = fold_traj.tilts[0]
        roll = pi / 2
        pitch = pi / 4
        if fold_traj.red:
            tilt = self.last_traj.tilts[0] % (2 * pi)
        rospy.loginfo("Tilt: %s" % tilt)

        #We do this so that we don't let go of the vertex during a red fold
        if not fold_traj.red:
            self.executeSmooth(self.last_traj)
            StanceUtils.open_gripper("b")
            self.fold_stance("b")
            arm = self.getArm(fold_traj)
            #Move the unused arm out of the way
            if arm == "l":
                StanceUtils.call_stance("viewing_right", 5.0)
            else:
                StanceUtils.call_stance("viewing_left", 5.0)

            (target, nothing) = self.gripPoints(point=approach_point,
                                                arm=arm,
                                                tilt=tilt,
                                                grip=False,
                                                preferred_pitch=pi / 4,
                                                preferred_roll=roll,
                                                red=fold_traj.red)
            pitch = target.pitch
            self.gripPoints(point=grip_point,
                            arm=arm,
                            tilt=tilt,
                            grip=False,
                            preferred_pitch=pi / 4,
                            preferred_roll=roll,
                            red=fold_traj.red,
                            grab=True)

            StanceUtils.close_gripper(arm)

        else:
            arm = self.last_arm
        StanceUtils.close_gripper(arm)
        self.gripPoints(point=vertical_point,
                        grip=True,
                        tilt=tilt,
                        arm=arm,
                        preferred_pitch=pi / 2,
                        preferred_roll=roll,
                        red=fold_traj.red)
        self.gripPoints(point=goal_point,
                        grip=True,
                        tilt=tilt,
                        arm=arm,
                        preferred_pitch=3 * pi / 5,
                        preferred_roll=roll,
                        red=fold_traj.red)

        self.last_traj = fold_traj
        self.last_arm = arm
Пример #30
0
def flip_sock(pt,arm):
    wiggle_down_dopple()
    StanceUtils.call_stance("arms_up",5.0)
    take_off_dopple(pt,arm=arm)
    smooth(pt,arm)
    StanceUtils.call_stance("arms_up",5.0)
Пример #31
0
 def viewing_stance(self, arm):
     StanceUtils.call_stance("viewing", 3.0)
Пример #32
0
def pickup_sock(sock):
    z_off = 0.0
    """
    while not rospy.is_shutdown():
        StanceUtils.call_stance("close_right",5.0)
        (grip_pt,angle) = get_grip_point(sock)
        GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off)
        rospy.sleep(5.0)
        z_off = float(raw_input("What is the new z_offset?"))
        StanceUtils.call_stance("arms_up",5.0)
    """
    (grip_pt, angle) = get_grip_point(sock)
    if MODE == THICK_SOCK:
        OFFSET = 0.02
    else:
        OFFSET = 0.025

    y_offset = OFFSET * cos(angle)
    x_offset = OFFSET * sin(angle)
    #x_offset += 0.005
    z_offset = 0.02
    pitch = pi / 2
    roll = pi / 2
    yaw = pi / 2 - angle
    GripUtils.go_to_pt(grip_pt,
                       roll=roll,
                       yaw=yaw,
                       pitch=pitch,
                       arm="r",
                       x_offset=x_offset,
                       y_offset=y_offset,
                       z_offset=z_offset,
                       grip=False,
                       dur=5.0)
    z_offset = -0.02
    z_offset -= 0.001
    GripUtils.go_to_pt(grip_pt,
                       roll=roll,
                       yaw=yaw,
                       pitch=pitch,
                       arm="r",
                       x_offset=x_offset,
                       y_offset=y_offset,
                       z_offset=z_offset,
                       grip=False,
                       dur=5.0)
    #Drag thin socks
    if MODE == THIN_SOCK:
        y_offset += 0.02
        GripUtils.go_to_pt(grip_pt,
                           roll=roll,
                           yaw=yaw,
                           pitch=pitch,
                           arm="r",
                           x_offset=x_offset,
                           y_offset=y_offset,
                           z_offset=z_offset,
                           grip=False,
                           dur=5.0)
    while not GripUtils.has_object("r"):
        StanceUtils.call_stance("open_right", 2.0)
        pitch -= ANGLE_INCREMENT
        y_offset -= 0.005 * cos(angle)
        x_offset -= 0.005 * sin(angle)
        z_offset += 0.0015
        GripUtils.go_to_pt(grip_pt,
                           roll=roll,
                           yaw=yaw,
                           pitch=pitch,
                           arm="r",
                           x_offset=x_offset,
                           y_offset=y_offset,
                           z_offset=z_offset,
                           grip=False,
                           dur=5.0)
        GripUtils.close_gripper("r", extra_tight=True)
        break
    return grip_pt
Пример #33
0
 def execute(self,userdata):
     if not StanceUtils.call_stance(self.stance_name,self.dur):
         return FAILURE
     else:
         return SUCCESS