def execute(self, userdata): if self.side == 'l': arm = 'l' processor = 'far_left_finder_node' yaw = -pi/2 else: arm = 'r' processor = 'far_right_finder_node' yaw = pi/2 process_mono = rospy.ServiceProxy("%s/process_mono"%processor,ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt_opp = resp.pts3d[1] userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping homogeneity = resp.params[resp.param_names.index("homogeneity")] if homogeneity > 0.13: print "Too homogeneous: %f"%homogeneity return FAILURE userdata.cloth_width = sqrt( (pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2 ) #userdata.cloth_width = abs(pt_opp.point.y - pt.point.y) userdata.cloth_height = None if arm == "l": x_offset = -0.02 #was -0.01 else: x_offset = -0.01 if not GripUtils.grab_point(pt,roll=-pi/2,yaw=yaw,arm=arm,x_offset=x_offset): return FAILURE else: if self.let_go: GripUtils.open_gripper(opp_arm(arm)) return SUCCESS
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)
def execute(self, userdata): if self.side == 'l': arm = 'l' processor = 'far_left_finder_node' yaw = -pi / 2 else: arm = 'r' processor = 'far_right_finder_node' yaw = pi / 2 process_mono = rospy.ServiceProxy("%s/process_mono" % processor, ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt_opp = resp.pts3d[1] userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping homogeneity = resp.params[resp.param_names.index("homogeneity")] if homogeneity > 0.13: print "Too homogeneous: %f" % homogeneity return FAILURE userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2) #userdata.cloth_width = abs(pt_opp.point.y - pt.point.y) userdata.cloth_height = None if arm == "l": x_offset = -0.02 #was -0.01 else: x_offset = -0.01 if not GripUtils.grab_point( pt, roll=-pi / 2, yaw=yaw, arm=arm, x_offset=x_offset): return FAILURE else: if self.let_go: GripUtils.open_gripper(opp_arm(arm)) return SUCCESS
def execute(self,userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr (bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z) (tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z) (br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z) (tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z) ctr_l_x = .75*bl_x + .25*tl_x ctr_l_y = .75*bl_y + .25*tl_y z = bl_z yaw = -pi/2 roll = -pi/2 pitch = pi/4 frame = pt_bl.header.frame_id ctr_r_x = .75*br_x + .25*tr_x ctr_r_y = .75*br_y + .25*tr_y + 0.01 # bit too far right alpha = 0.333 ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0 yaw = pi/2 roll = pi/2 pitch = pi/4 if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y,z=z+0.0025,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame): GripUtils.open_gripper("r") GripUtils.recall_arm("r") return FAILURE pitch = pi/2 if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0): GripUtils.open_gripper("r") GripUtils.recall_arm("r") return FAILURE if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0): GripUtils.open_gripper("r") GripUtils.recall_arm("r") return FAILURE z = bl_z+0.01 pitch = 3*pi/4 if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0): GripUtils.open_gripper("r") GripUtils.recall_arm("r") return FAILURE GripUtils.open_gripper("r") if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.06,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0): GripUtils.recall_arm("r") return FAILURE GripUtils.recall_arm("r") pr2_say(talk_done) return SUCCESS
def execute(self,userdata): arm = userdata.arm multiplier = 1 if arm == 'r' else -1 if not GripUtils.go_to(x=0.4,y=0,z=0.35,roll=0,yaw=pi/2*multiplier,pitch=pi/4,arm=arm,grip=True,frame="table_height",dur=3.0): return FAILURE GripUtils.open_gripper(arm) GripUtils.recall_arm("b") return SUCCESS
def execute(self, userdata): process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.16 GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",z_offset=z_offset,grip=False,dur=5.0) GripUtils.open_gripper("l") return SUCCESS
def add_default_stances(self): rospy.Service("stances/pause", ExecuteStance, self.pause) rospy.Service("stances/open_left", ExecuteStance, lambda req: GripUtils.open_gripper("l")) rospy.Service("stances/open_right", ExecuteStance, lambda req: GripUtils.open_gripper("r")) rospy.Service("stances/open_both", ExecuteStance, lambda req: GripUtils.open_gripper("b")) rospy.Service("stances/close_left", ExecuteStance, lambda req: GripUtils.close_gripper("l")) rospy.Service("stances/close_right", ExecuteStance, lambda req: GripUtils.close_gripper("r")) rospy.Service("stances/close_both", ExecuteStance, lambda req: GripUtils.close_gripper("b")) self.default_stances = ()
def go_to_folding_station(): print "Going to folding station" DryerNavigationUtils.dryerToTable1() GripUtils.go_to(arm="l", x=0.5, y=0, z=0.35, roll=0, yaw=0, pitch=pi / 2, grip=True, frame="table_height") GripUtils.open_gripper("l") return True
def execute(self, userdata): if self.arm == "l": arm = "l" processor = "far_left_finder_node" yaw = -pi / 2 roll = -pi / 2 else: arm = "r" processor = "far_right_finder_node" yaw = pi / 2 roll = pi / 2 process_mono = rospy.ServiceProxy("%s/process_mono" % processor, ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt_opp = resp.pts3d[1] userdata.arm = "l" if arm == "r" else "r" # Arm returns the arm which is currently gripping homogeneity = resp.params[resp.param_names.index("homogeneity")] if homogeneity > 0.13: print "Too homogeneous: %f" % homogeneity return FAILURE userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x) ** 2 + (pt_opp.point.y - pt.point.y) ** 2) # userdata.cloth_width = abs(pt_opp.point.y - pt.point.y) userdata.cloth_height = None # Offsets for safety if arm == "l": y_offset = -0.02 z_offset = 0.01 else: y_offset = 0.02 x_offset = 0.02 if self.side == "t": x_offset = -0.01 elif self.side == "b": x_offset = 0.01 else: x_offset = 0 if not GripUtils.grab_point(pt, roll=roll, yaw=yaw, arm=arm, x_offset=x_offset, y_offset=y_offset): return FAILURE else: if self.let_go: GripUtils.open_gripper(opp_arm(arm)) if self.recall: GripUtils.recall_arm(arm, grip=True) return SUCCESS
def go_to_folding_station(): print "Going to folding station" DryerNavigationUtils.dryerToTable1() GripUtils.go_to(arm="l",x=0.5,y=0,z=0.35,roll=0,yaw=0,pitch=pi/2,grip=True,frame="table_height") GripUtils.open_gripper("l") return True
def execute(self, userdata): GripUtils.go_to(0.58, 0, 0.15, 0, pi / 4, 0, True, "torso_lift_link", "l") GripUtils.open_gripper("l") GripUtils.go_to(0.3, 0.65, 0.35, 0, 0, 0, True, "torso_lift_link", "l") return SUCCESS
def execute(self, userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr (bl_x, bl_y, bl_z) = (pt_bl.point.x, pt_bl.point.y, pt_bl.point.z) (tl_x, tl_y, tl_z) = (pt_tl.point.x, pt_tl.point.y, pt_tl.point.z) (br_x, br_y, br_z) = (pt_br.point.x, pt_br.point.y, pt_br.point.z) (tr_x, tr_y, tr_z) = (pt_tr.point.x, pt_tr.point.y, pt_tr.point.z) ctr_l_x = .75 * bl_x + .25 * tl_x #Make more centered ctr_l_x -= 0.01 ctr_l_y = .75 * bl_y + .25 * tl_y z = bl_z yaw = -pi / 2 roll = -pi / 2 pitch = pi / 4 frame = pt_bl.header.frame_id if not GripUtils.grab(x=ctr_l_x, y=ctr_l_y, z=z, roll=roll, yaw=yaw, pitch=pitch, arm="l", frame=frame): return FAILURE ctr_r_x = .75 * br_x + .25 * tr_x #Make more centered ctr_r_x -= 0.01 ctr_r_y = .75 * br_y + .25 * tr_y alpha = 0.333 ctr_ml_x = (1 - alpha) * ctr_l_x + alpha * ctr_r_x ctr_ml_y = (1 - alpha) * ctr_l_y + alpha * ctr_r_y ctr_mr_x = (1 - alpha) * ctr_r_x + alpha * ctr_l_x ctr_mr_y = (1 - alpha) * ctr_r_y + alpha * ctr_l_y up_z = z + sqrt((ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0 pitch = pi / 2 if not GripUtils.go_to(x=ctr_ml_x, y=ctr_ml_y, z=up_z, roll=roll, yaw=yaw, pitch=pitch, arm="l", frame=frame, grip=True, dur=5.0): return FAILURE if not GripUtils.go_to(x=(ctr_ml_x + ctr_mr_x) / 2.0, y=(ctr_ml_y + ctr_mr_y + 0.02) / 2.0, z=(up_z + bl_z) / 2.0, roll=roll, yaw=yaw, pitch=(pitch + 3 * pi / 4) / 2.0, arm="l", frame=frame, grip=True, dur=5.0): return FAILURE z = bl_z pitch = 3 * pi / 4 if not GripUtils.go_to(x=ctr_mr_x, y=ctr_mr_y + 0.02, z=z, roll=roll, yaw=yaw, pitch=pitch, arm="l", frame=frame, grip=True, dur=5.0): return FAILURE yaw = pi / 2 roll = pi / 2 pitch = pi / 4 GripUtils.open_gripper("l") if not GripUtils.go_to(x=ctr_mr_x, y=ctr_mr_y - 0.05, z=z + 0.02, roll=roll, yaw=yaw, pitch=pitch, arm="l", frame=frame, grip=False, dur=1.0): return FAILURE GripUtils.recall_arm("l") if not GripUtils.grab(x=ctr_r_x, y=ctr_r_y - 0.01, z=z, roll=roll, yaw=yaw, pitch=pitch, arm="r", frame=frame): return FAILURE pitch = pi / 2 if not GripUtils.go_to(x=ctr_mr_x, y=ctr_mr_y - 0.02, z=up_z, roll=roll, yaw=yaw, pitch=pitch, arm="r", frame=frame, grip=True, dur=5.0): return FAILURE if not GripUtils.go_to(x=(ctr_mr_x + ctr_ml_x) / 2.0, y=(ctr_mr_y - 0.02 + ctr_ml_y - 0.02) / 2.0, z=(up_z + bl_z + 0.01) / 2.0, roll=roll, yaw=yaw, pitch=(pitch + 3 * pi / 4) / 2.0, arm="r", frame=frame, grip=True, dur=5.0): return FAILURE z = bl_z + 0.01 pitch = 3 * pi / 4 if not GripUtils.go_to(x=ctr_ml_x, y=ctr_ml_y - 0.02, z=z, roll=roll, yaw=yaw, pitch=pitch, arm="r", frame=frame, grip=True, dur=5.0): return FAILURE GripUtils.open_gripper("r") if not GripUtils.go_to(x=ctr_ml_x, y=ctr_ml_y + 0.05, z=z + 0.02, roll=roll, yaw=yaw, pitch=pitch, arm="r", frame=frame, grip=False, dur=1.0): return FAILURE GripUtils.recall_arm("r") """ if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame ,dur=3.0): return FAILURE if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame ,dur=3.0): return FAILURE GripUtils.close_gripper("b") if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame ,dur=3.0): return FAILURE """ return SUCCESS
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)
def execute(self, userdata): GripUtils.go_to(0.6,0,0.15,0,0,0,True,"torso_lift_link","l") GripUtils.open_gripper("l") return SUCCESS
def execute(self,userdata): #cloth_width = userdata.cloth_width*.85 cloth_width = userdata.cloth_width * 0.885 cloth_height = userdata.cloth_height start_x = 0.20 end_x = start_x + self.forward_amount down_height = 0.03 up_height = 0.625 return_val = SUCCESS if not GripUtils.go_to_multi(x_l=start_x,y_l=cloth_width/2.0,z_l=up_height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=start_x,y_r=-cloth_width/2.0,z_r=up_height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=4.0): return_val = FAILURE if not cloth_height: if not GripUtils.go_to_multi(x_l=start_x,y_l=cloth_width/2.0,z_l=(0.75*up_height + 0.25*down_height), roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=start_x,y_r=-cloth_width/2.0,z_r=(0.75*up_height + 0.25*down_height), roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=(start_x + end_x)/2.5,y_l=cloth_width/2.0,z_l=0.25*up_height + 0.75*down_height, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=(start_x+end_x)/2.5,y_r=-cloth_width/2.0,z_r=0.25*up_height + 0.75*down_height, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=end_x-0.1,y_l=cloth_width/2.0,z_l=0.25*up_height + 0.75*down_height, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=end_x-0.1,y_r=-cloth_width/2.0,z_r=0.25*up_height + 0.75*down_height, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=end_x+0.1,y_l=cloth_width/2.0,z_l=down_height+0.03, roll_l=0,pitch_l=pi/4,yaw_l=-pi/5,grip_l=True, x_r=end_x+0.1,y_r=-cloth_width/2.0,z_r=down_height+0.03, roll_r=0,pitch_r=pi/4,yaw_r=pi/5,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE else: if not GripUtils.go_to_multi(x_l=start_x,y_l=cloth_width/2.0,z_l=cloth_height+down_height, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=start_x,y_r=-cloth_width/2.0,z_r=cloth_height+down_height, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=end_x - 0.1 - cloth_height/1.5,y_l=cloth_width/2.0,z_l=cloth_height/1.5+down_height+0.03, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=end_x - 0.1 - cloth_height/1.5,y_r=-cloth_width/2.0,z_r=cloth_height/1.5+down_height+0.03, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=end_x-0.1,y_l=cloth_width/2.0,z_l=down_height+0.1, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=end_x-0.1,y_r=-cloth_width/2.0,z_r=down_height+0.1, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE cloth_width *= 1.05 if not GripUtils.go_to_multi(x_l=end_x+0.1,y_l=cloth_width/2.0,z_l=down_height+0.03, roll_l=0,pitch_l=pi/4,yaw_l=-pi/5,grip_l=True, x_r=end_x+0.1,y_r=-cloth_width/2.0,z_r=down_height+0.03, roll_r=0,pitch_r=pi/4,yaw_r=pi/5,grip_r=True, frame_l="table_height",frame_r="table_height",dur=3.0): return_val = FAILURE if self.recall: GripUtils.open_gripper("b") GripUtils.recall_arm("b") return SUCCESS #was return_val
def wiggle_down_dopple(back_up=True): open_amt = 0.02 #original 0.025 x = DOPPLE_X y = DOPPLE_Y z = DOPPLE_HEIGHT + TABLE_HEIGHT x_l = x x_r = x y_l = y + open_amt y_r = y - open_amt z_l = z z_r = z roll_l = pi / 2 roll_r = pi / 2 yaw_l = -pi / 2 yaw_r = pi / 2 pitch_l = 0 pitch_r = 0 frame_l = "base_footprint" frame_r = "base_footprint" grip_l = grip_r = True offsetToLeft = 0.01 GripUtils.go_to_multi(x_l=x_l + 0.005, y_l=y_l - offsetToLeft, z_l=z_l, roll_l=roll_l + 1.0 / 5, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + 0.005, y_r=y_r - offsetToLeft, z_r=z_r, roll_r=roll_r - 1.0 / 5, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=5.0) GripUtils.go_to_multi(x_l=x_l + 0.005, y_l=y_l, z_l=z_l, roll_l=roll_l + 1.0 / 5, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + 0.005, y_r=y_r, z_r=z_r, roll_r=roll_r - 1.0 / 5, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=5.0) z_l -= 0.04 z_r -= 0.04 GripUtils.go_to_multi(x_l=x_l, y_l=y_l, z_l=z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r, y_r=y_r, z_r=z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=2.0) x_step = 0.015 #z_step = 0.0175 z_step = 0.02 #max_iters = (DOPPLE_HEIGHT - 0.15) / z_step * 2 max_iters = 27 new_z_l = z_l new_z_r = z_r print "Going for %d wiggle iterations" % max_iters for i in range(3): new_z_l -= 2.0 * z_step new_z_r -= 2.0 * z_step GripUtils.go_to_multi(x_l=x_l, y_l=y_l, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r, y_r=y_r, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=1) new_z_l += 1 * z_step new_z_r += 1 * z_step GripUtils.go_to_multi(x_l=x_l, y_l=y_l, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r, y_r=y_r, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=1) xWiggleList = [0, 0.02, 0, -0.02] yWiggleList = [0.02, 0, -0.02, 0] new_z_l += 2.5 * z_step new_z_r += 2.5 * z_step for i in range(max_iters): currentXoffset = xWiggleList[i % 4] currentYoffset = yWiggleList[i % 4] new_z_l -= 5.0 * z_step new_z_r -= 5.0 * z_step GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=2) new_z_l += 5.0 * z_step new_z_r += 5.0 * z_step GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=2) new_z_l -= 5.0 * z_step new_z_r -= 5.0 * z_step GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=2) new_z_l += 4.75 * z_step new_z_r += 4.75 * z_step GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=2) new_z_l = TABLE_HEIGHT + 0.2 new_z_r = TABLE_HEIGHT + 0.2 GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=3) new_z_l = TABLE_HEIGHT + 0.3 new_z_r = TABLE_HEIGHT + 0.3 GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=3) #new_z_l = TABLE_HEIGHT + 0.05 #new_z_r = TABLE_HEIGHT + 0.05 # #GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=5) new_z_l = TABLE_HEIGHT + 0.1 new_z_r = TABLE_HEIGHT + 0.1 GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=3) new_z_l = TABLE_HEIGHT + 0.2 new_z_r = TABLE_HEIGHT + 0.2 GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=3) new_z_l = TABLE_HEIGHT + 0.05 new_z_r = TABLE_HEIGHT + 0.05 GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame_r, dur=3) new_z_l = TABLE_HEIGHT + 0.15 new_z_r = TABLE_HEIGHT + 0.15 GripUtils.go_to_multi(x_l=x_l + currentXoffset, y_l=y_l + currentYoffset, z_l=new_z_l, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l - pi / 8, grip_l=grip_l, frame_l=frame_l, x_r=x_r + currentXoffset, y_r=y_r + currentYoffset, z_r=new_z_r, roll_r=roll_r, yaw_r=yaw_r, pitch_r=pitch_r - pi / 8, grip_r=grip_r, frame_r=frame_r, dur=3) #amplitude= 2.0 #new_z_l -= 0.5*amplitude*z_step; #new_z_r += 0.5*amplitude*z_step; # new wiggle algorithm #for i in range(max_iters): # if (i>3): # new_z_l += 0.5*amplitude*z_step; # new_z_r -= 0.5*amplitude*z_step; # amplitude= 5.0 # new_z_l -= 0.5*amplitude*z_step; # new_z_r += 0.5*amplitude*z_step; # new_z_l += amplitude*z_step # new_z_r -= amplitude*z_step # new_z_l -= 0.25*z_step # new_z_r -= 0.25*z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=2) # # new_z_l -= amplitude*z_step # new_z_r += amplitude*z_step # new_z_l -= 0.25*z_step # new_z_r -= 0.25*z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=2) # for i in range(max_iters): # roll_l = roll_r = pi/2 # new_z_l -= z_step # new_z_r -= z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75*(i/3 + 1)) # # x_l += x_step # x_r -= x_step # roll_l = 2*pi/5 # roll_r = 3 *pi/5 # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75) # x_l -= 2*x_step # x_r += 2*x_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75) # roll_l = roll_r = pi/2 # x_l += x_step # x_r -= x_step # if back_up: # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75*(i/3 + 1)) # # #End down # new_z_l -= z_step # new_z_r -= z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=5.0) GripUtils.open_gripper("b")
def wiggle_down_dopple(back_up = True): open_amt = 0.02 #original 0.025 x = DOPPLE_X y = DOPPLE_Y z = DOPPLE_HEIGHT+TABLE_HEIGHT x_l = x x_r = x y_l = y+open_amt y_r = y-open_amt z_l = z z_r = z roll_l = pi/2 roll_r = pi/2 yaw_l = -pi/2 yaw_r = pi/2 pitch_l = 0 pitch_r = 0 frame_l = "base_footprint" frame_r = "base_footprint" grip_l = grip_r = True offsetToLeft= 0.01 GripUtils.go_to_multi (x_l=x_l+0.005,y_l=y_l-offsetToLeft,z_l=z_l,roll_l=roll_l+1.0/5,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+0.005,y_r=y_r-offsetToLeft,z_r=z_r,roll_r=roll_r-1.0/5,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=5.0) GripUtils.go_to_multi (x_l=x_l+0.005,y_l=y_l,z_l=z_l,roll_l=roll_l+1.0/5,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+0.005,y_r=y_r,z_r=z_r,roll_r=roll_r-1.0/5,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=5.0) z_l -= 0.04 z_r -= 0.04 GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=2.0) x_step = 0.015 #z_step = 0.0175 z_step = 0.02 #max_iters = (DOPPLE_HEIGHT - 0.15) / z_step * 2 max_iters = 27; new_z_l = z_l new_z_r = z_r print "Going for %d wiggle iterations"%max_iters for i in range(3): new_z_l -= 2.0*z_step; new_z_r -= 2.0*z_step; GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=1) new_z_l += 1*z_step; new_z_r += 1*z_step; GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=1) xWiggleList = [0, 0.02, 0, -0.02] yWiggleList = [0.02, 0, -0.02, 0] new_z_l += 2.5*z_step; new_z_r += 2.5*z_step; for i in range(max_iters): currentXoffset= xWiggleList[i%4]; currentYoffset= yWiggleList[i%4]; new_z_l -= 5.0*z_step; new_z_r -= 5.0*z_step; GripUtils.go_to_multi (x_l=x_l + currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r + currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=2) new_z_l += 5.0*z_step; new_z_r += 5.0*z_step; GripUtils.go_to_multi (x_l=x_l + currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r + currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=2) new_z_l -= 5.0*z_step; new_z_r -= 5.0*z_step; GripUtils.go_to_multi (x_l=x_l + currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r + currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=2) new_z_l += 4.75*z_step; new_z_r += 4.75*z_step; GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=2) new_z_l = TABLE_HEIGHT + 0.2 new_z_r = TABLE_HEIGHT + 0.2 GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=3) new_z_l = TABLE_HEIGHT + 0.3 new_z_r = TABLE_HEIGHT + 0.3 GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=3) #new_z_l = TABLE_HEIGHT + 0.05 #new_z_r = TABLE_HEIGHT + 0.05 # #GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=5) new_z_l = TABLE_HEIGHT + 0.1 new_z_r = TABLE_HEIGHT + 0.1 GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=3) new_z_l = TABLE_HEIGHT + 0.2 new_z_r = TABLE_HEIGHT + 0.2 GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=3) new_z_l = TABLE_HEIGHT + 0.05 new_z_r = TABLE_HEIGHT + 0.05 GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r ,dur=3) new_z_l = TABLE_HEIGHT + 0.15 new_z_r = TABLE_HEIGHT + 0.15 GripUtils.go_to_multi (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l-pi/8,grip_l=grip_l,frame_l=frame_l ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r-pi/8,grip_r=grip_r,frame_r=frame_r ,dur=3) #amplitude= 2.0 #new_z_l -= 0.5*amplitude*z_step; #new_z_r += 0.5*amplitude*z_step; # new wiggle algorithm #for i in range(max_iters): # if (i>3): # new_z_l += 0.5*amplitude*z_step; # new_z_r -= 0.5*amplitude*z_step; # amplitude= 5.0 # new_z_l -= 0.5*amplitude*z_step; # new_z_r += 0.5*amplitude*z_step; # new_z_l += amplitude*z_step # new_z_r -= amplitude*z_step # new_z_l -= 0.25*z_step # new_z_r -= 0.25*z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=2) # # new_z_l -= amplitude*z_step # new_z_r += amplitude*z_step # new_z_l -= 0.25*z_step # new_z_r -= 0.25*z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=2) # for i in range(max_iters): # roll_l = roll_r = pi/2 # new_z_l -= z_step # new_z_r -= z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75*(i/3 + 1)) # # x_l += x_step # x_r -= x_step # roll_l = 2*pi/5 # roll_r = 3 *pi/5 # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75) # x_l -= 2*x_step # x_r += 2*x_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75) # roll_l = roll_r = pi/2 # x_l += x_step # x_r -= x_step # if back_up: # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=0.75*(i/3 + 1)) # # #End down # new_z_l -= z_step # new_z_r -= z_step # GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l # ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r # ,dur=5.0) GripUtils.open_gripper("b")
def execute(self, userdata): if not GripUtils.open_gripper(self.arm): return FAILURE else: return SUCCESS
def execute(self,userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr (bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z) (tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z) (br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z) (tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z) ctr_l_x = .75*bl_x + .25*tl_x #Make more centered ctr_l_x -= 0.01 ctr_l_y = .75*bl_y + .25*tl_y z = bl_z yaw = -pi/2 roll = -pi/2 pitch = pi/4 frame = pt_bl.header.frame_id if not GripUtils.grab(x=ctr_l_x,y=ctr_l_y,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame): return FAILURE ctr_r_x = .75*br_x + .25*tr_x #Make more centered ctr_r_x -= 0.01 ctr_r_y = .75*br_y + .25*tr_y alpha = 0.333 ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0 pitch = pi/2 if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0): return FAILURE if not GripUtils.go_to(x=(ctr_ml_x+ctr_mr_x)/2.0,y=(ctr_ml_y+ctr_mr_y+0.02)/2.0,z=(up_z+bl_z)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="l",frame=frame,grip=True,dur=5.0): return FAILURE z = bl_z pitch = 3*pi/4 if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y+0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0): return FAILURE yaw = pi/2 roll = pi/2 pitch = pi/4 GripUtils.open_gripper("l") if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=False,dur=1.0): return FAILURE GripUtils.recall_arm("l") if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y-0.01,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame): return FAILURE pitch = pi/2 if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0): return FAILURE if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0): return FAILURE z = bl_z+0.01 pitch = 3*pi/4 if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0): return FAILURE GripUtils.open_gripper("r") if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0): return FAILURE GripUtils.recall_arm("r") """ if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame ,dur=3.0): return FAILURE if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame ,dur=3.0): return FAILURE GripUtils.close_gripper("b") if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame ,dur=3.0): return FAILURE """ return SUCCESS
def execute(self, userdata): #cloth_width = userdata.cloth_width*.85 cloth_width = userdata.cloth_width * 0.885 cloth_height = userdata.cloth_height start_x = 0.20 end_x = start_x + self.forward_amount down_height = 0.03 up_height = 0.625 return_val = SUCCESS if not GripUtils.go_to_multi(x_l=start_x, y_l=cloth_width / 2.0, z_l=up_height, roll_l=0, pitch_l=0, yaw_l=-pi / 2, grip_l=True, x_r=start_x, y_r=-cloth_width / 2.0, z_r=up_height, roll_r=0, pitch_r=0, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=4.0): return_val = FAILURE if not cloth_height: if not GripUtils.go_to_multi( x_l=start_x, y_l=cloth_width / 2.0, z_l=(0.75 * up_height + 0.25 * down_height), roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=start_x, y_r=-cloth_width / 2.0, z_r=(0.75 * up_height + 0.25 * down_height), roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi( x_l=(start_x + end_x) / 2.5, y_l=cloth_width / 2.0, z_l=0.25 * up_height + 0.75 * down_height, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=(start_x + end_x) / 2.5, y_r=-cloth_width / 2.0, z_r=0.25 * up_height + 0.75 * down_height, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi( x_l=end_x - 0.1, y_l=cloth_width / 2.0, z_l=0.25 * up_height + 0.75 * down_height, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=end_x - 0.1, y_r=-cloth_width / 2.0, z_r=0.25 * up_height + 0.75 * down_height, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=end_x + 0.1, y_l=cloth_width / 2.0, z_l=down_height + 0.03, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 5, grip_l=True, x_r=end_x + 0.1, y_r=-cloth_width / 2.0, z_r=down_height + 0.03, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 5, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE else: if not GripUtils.go_to_multi(x_l=start_x, y_l=cloth_width / 2.0, z_l=cloth_height + down_height, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=start_x, y_r=-cloth_width / 2.0, z_r=cloth_height + down_height, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi( x_l=end_x - 0.1 - cloth_height / 1.5, y_l=cloth_width / 2.0, z_l=cloth_height / 1.5 + down_height + 0.03, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=end_x - 0.1 - cloth_height / 1.5, y_r=-cloth_width / 2.0, z_r=cloth_height / 1.5 + down_height + 0.03, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE if not GripUtils.go_to_multi(x_l=end_x - 0.1, y_l=cloth_width / 2.0, z_l=down_height + 0.1, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=end_x - 0.1, y_r=-cloth_width / 2.0, z_r=down_height + 0.1, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE cloth_width *= 1.05 if not GripUtils.go_to_multi(x_l=end_x + 0.1, y_l=cloth_width / 2.0, z_l=down_height + 0.03, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 5, grip_l=True, x_r=end_x + 0.1, y_r=-cloth_width / 2.0, z_r=down_height + 0.03, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 5, grip_r=True, frame_l="table_height", frame_r="table_height", dur=3.0): return_val = FAILURE if self.recall: GripUtils.open_gripper("b") GripUtils.recall_arm("b") return SUCCESS #was return_val