def open_sock(): x=0.5 y=0.0 z=TABLE_HEIGHT+0.4 frame="base_footprint" #old strategy GripUtils.go_to(x=x,y=y+0.007,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/8,grip=True,frame=frame,arm="r",dur=5.0) GripUtils.go_to(x=x,y=y+0.05,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=False,frame=frame,arm="l",dur=5.0) GripUtils.go_to(x=x,y=y-0.004,z=z+0.01,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=False,frame=frame,arm="l",dur=5.0) GripUtils.go_to(x=x,y=y-0.0035,z=z+0.001,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=True,frame=frame,arm="l",dur=2.0) #changed from -0.007 to -0.0065 #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0) #GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0) GripUtils.close_gripper("l",extra_tight=True) open_amt = 0.025 GripUtils.go_to_multi (x_l=x,y_l=y+open_amt,z_l=z+0.01,roll_l=pi/2,yaw_l=-pi/2,pitch_l=0,grip_l=True,frame_l=frame ,x_r=x,y_r=y-open_amt,z_r=z+0.01,roll_r=pi/2,yaw_r=pi/2,pitch_r=0,grip_r=True,frame_r=frame ,dur=5.0)
def execute(self,userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr #if not GripUtils.go_to_pts(pt_tl, -pi/2, -pi/3, pi/4, pt_tr, pi/2, pi/3, pi/4, x_offset_l=-0.04,\ # x_offset_r=-0.04): # return FAILURE #FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE ''' if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04): return FAILURE if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04): return FAILURE ''' #FIXME: For some reason large offsets required, #DEBUG if not GripUtils.grab_points(pt_tl,roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.05, z_offset_l=-0.0025 ,point_r=pt_tr,roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.06, y_offset_r=0.00, y_offset_l=-0.00, z_offset_r=0.01): return FAILURE (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) x_l = (tl_x+bl_x)/2.0 y_l = (tl_y+bl_y)/2.0 z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0 x_r = (tr_x+br_x)/2.0 y_r = (tr_y+br_y)/2.0 z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0 pitch_l = pitch_r = pi/4 roll_l = 0 roll_r = 0 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True frame_l=frame_r = pt_bl.header.frame_id if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=4.0): return_val = FAILURE print "Folding down!" x_l = bl_x + 0.02 # overshoots fold down x_r = br_x + 0.02 y_l = bl_y-0.01 y_r = br_y+0.01 z_l = z_r = bl_z + 0.02 yaw_l = -3*pi/4 yaw_r = 3*pi/4 pitch_l=pitch_r = pi/4 roll_l = pi/2 roll_r = -pi/2 GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5) GripUtils.recall_arm("b") pr2_say(talk_pose2) return SUCCESS
def execute(self, userdata): cloth_width = userdata.cloth_width if cloth_width < 0.35: rospy.logwarn("Cannot shake at less than 0.35m apart; tried to do %s" % str(cloth_width)) return FAILURE if cloth_width > 0.65: cloth_width = 0.65 forward_amount = 0.45 height = 0.625 drop_amount = 0.3 lateral_amount = 0.1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return FAILURE duration = .45 return_val = SUCCESS if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE cloth_width = cloth_width * self.clothWidthScaleFactor if not GripUtils.go_to_multi(x_l=forward_amount+.2,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/4,grip_l=True, x_r=forward_amount+.2,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/4,grip_r=True, frame_l="table_height",frame_r="table_height",dur=4.0): return FAILURE return SUCCESS
def execute(self, userdata): 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=True, x_r=forward_amount, y_r=-lateral_amount, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, grip_r=True, frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0): return FAILURE else: return SUCCESS
def execute(self, userdata): height = 0.35 lateral_amount = 0.65 forward_amount = 0.3 if self.arm == 'b': lateral_amount_r = lateral_amount * -1 lateral_amount_l = lateral_amount if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount_l, z_l=height, roll_l=0, pitch_l=0, yaw_l=0, frame_l="torso_lift_link", grip_l=False, x_r=forward_amount, y_r=lateral_amount_r, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, frame_r="torso_lift_link", grip_r=False, dur=4.0): return FAILURE else: return SUCCESS else: if self.arm == 'r': lateral_amount = lateral_amount * -1 if not GripUtils.go_to( x=forward_amount, y=lateral_amount, z=height, roll=0, pitch=0, yaw=0, grip=False, frame="torso_lift_link", dur=4.0, arm=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 #FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04): return FAILURE if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04): return FAILURE (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) x_l = (tl_x+bl_x)/2.0 y_l = (tl_y+bl_y)/2.0 z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0 x_r = (tr_x+br_x)/2.0 y_r = (tr_y+br_y)/2.0 z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0 pitch_l = pitch_r = pi/4 roll_l = 0 roll_r = 0 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True frame_l=frame_r = pt_bl.header.frame_id if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5): return_val = FAILURE print "Folding down!" x_l = bl_x-0.015 y_l = bl_y+0.025 z_l = z_r = bl_z + 0.02 x_r = br_x-0.015 y_r = br_y-0.025 yaw_l = -3*pi/4 yaw_r = 3*pi/4 pitch_l=pitch_r = pi/4 roll_l = pi/2 roll_r = -pi/2 GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5) GripUtils.recall_arm("b") return SUCCESS
def execute(self,userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr #FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.08): return FAILURE if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.08): return FAILURE (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) x_l = (tl_x+bl_x)/2.0 y_l = (tl_y+bl_y)/2.0 z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0 x_r = (tr_x+br_x)/2.0 y_r = (tr_y+br_y)/2.0 z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0 pitch_l = pitch_r = pi/4 roll_l = 0 roll_r = 0 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True frame_l=frame_r = pt_bl.header.frame_id if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5): return_val = FAILURE print "Folding down!" x_l = bl_x-0.015 y_l = bl_y+0.025 z_l = z_r = bl_z + 0.02 x_r = br_x-0.015 y_r = br_y-0.025 yaw_l = -3*pi/4 yaw_r = 3*pi/4 pitch_l=pitch_r = pi/4 roll_l = pi/2 roll_r = -pi/2 GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_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,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5) GripUtils.recall_arm("b") return SUCCESS
def execute(self, userdata): cloth_width = userdata.cloth_width*.90 if cloth_width < MINIMUM_SHAKE_WIDTH: rospy.logwarn("Cannot shake at less than %s m apart; tried to do %d" % (MINIMUM_SHAKE_WIDTH, cloth_width)) return FAILURE forward_amount = 0.45 height = 0.625 if self.violent: drop_amount = 0.3 else: drop_amount = 0.1 lateral_amount = 0.1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE duration = .6 return_val = SUCCESS if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE return SUCCESS
def execute(self, userdata): 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=self.grip, x_r=forward_amount, y_r=-lateral_amount, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, grip_r=self.grip, frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0): return FAILURE else: return SUCCESS
def smooth(pt,side): y_offset = -0.08 x_offset = 0.02 x = pt.point.x + x_offset y = pt.point.y + y_offset z = pt.point.z frame = "base_footprint" x_l = x x_r = x y_l = y y_r = y z_l = z_r = z roll_l = roll_r = 0 yaw_l = -pi/2 yaw_r = pi/2 pitch_l = pitch_r=pi/6 grip_l = grip_r = True GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.04,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame ,x_r=x,y_r=y_r,z_r=z+0.04,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame ,dur=5.0) GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame ,x_r=x,y_r=y_r,z_r=z,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame ,dur=3.0) y_l += 0.06 y_r -= 0.06 pitch_l = pi/4 pitch_r = pi/4 GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.01,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame ,x_r=x,y_r=y_r,z_r=z+0.01,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame ,dur=5.0)
def put_on_dopple(depth=0.28): open_amt = 0.02 x = DOPPLE_X+0.0075 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 GripUtils.go_to_multi (x_l=x_l-0.1,y_l=y_l,z_l=z_l,roll_l=roll_l+pi/2,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r-0.1,y_r=y_r,z_r=z_r,roll_r=roll_r-pi/2,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.05,y_l=y_l,z_l=z_l,roll_l=roll_l+pi/4,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r-0.05,y_r=y_r,z_r=z_r,roll_r=roll_r-pi/4,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,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=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) z_l = z_r = TABLE_HEIGHT + DOPPLE_HEIGHT-depth 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=5.0)
def execute(self, userdata): height = 0.35 lateral_amount = 0.65 forward_amount = 0.3 if self.arm == 'b': lateral_amount_r = lateral_amount * -1 lateral_amount_l = lateral_amount if not GripUtils.go_to_multi(x_l=forward_amount, y_l=lateral_amount_l, z_l=height, roll_l=0, pitch_l=0, yaw_l=0, frame_l="torso_lift_link", grip_l=False, x_r=forward_amount, y_r=lateral_amount_r, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, frame_r="torso_lift_link", grip_r=False, dur=4.0): return FAILURE else: return SUCCESS else: if self.arm == 'r': lateral_amount = lateral_amount * -1 if not GripUtils.go_to(x=forward_amount, y=lateral_amount, z=height, roll=0, pitch=0, yaw=0, grip=False, frame="torso_lift_link", dur=4.0, arm=self.arm): return FAILURE else: return SUCCESS
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
def executeDrag(self,gripPts,endPts,color='blue'): """ drag from gripPts to endPts """ startpoints = [] for vertex in gripPts: pt_world = self.convert_to_world_frame(vertex) now = rospy.Time.now() self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0)) pt_transformed = self.listener.transformPoint("base_footprint",pt_world) startpoints.append(pt_transformed) #print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True, point_r=startpoints[1],roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005): print "failure" return False else: print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z print "done" midpoints = [] for vertex in endPts: pt_world = self.dupl_PointStamped(vertex)#self.convert_to_world_frame(vertex) midpoints.append(pt_world) frame_l = frame_r = midpoints[0].header.frame_id if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=-pi/2,yaw_l=-pi/2,pitch_l=pi/4,grip_l=True,frame_l=frame_l ,x_r=midpoints[1].point.x, y_r= midpoints[1].point.y ,z_r= midpoints[1].point.z ,roll_r=pi/2,yaw_r=pi/2,pitch_r=pi/4,grip_r=True,frame_r=frame_r,dur=5): print "failure" #return False else: print "dragging done" if (color == 'blue'): GripUtils.open_grippers() return True
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): cloth_width = userdata.cloth_width * .90 if cloth_width < MINIMUM_SHAKE_WIDTH: rospy.logwarn( "Cannot shake at less than %s m apart; tried to do %d" % (MINIMUM_SHAKE_WIDTH, cloth_width)) return FAILURE forward_amount = 0.45 height = 0.625 if self.violent: drop_amount = 0.3 else: drop_amount = 0.1 lateral_amount = 0.1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to_multi(x_l=forward_amount, y_l=cloth_width / 2.0, z_l=height, roll_l=0, pitch_l=0, yaw_l=-pi / 2, grip_l=True, x_r=forward_amount, y_r=-cloth_width / 2.0, z_r=height, roll_r=0, pitch_r=0, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=duration): return_val = FAILURE duration = .6 return_val = SUCCESS if not GripUtils.go_to_multi( x_l=forward_amount, y_l=cloth_width / 2.0 - lateral_amount, z_l=height - drop_amount, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=forward_amount, y_r=-cloth_width / 2.0 + lateral_amount, z_r=height - drop_amount, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=duration): return_val = FAILURE if not GripUtils.go_to_multi(x_l=forward_amount, y_l=cloth_width / 2.0, z_l=height, roll_l=0, pitch_l=0, yaw_l=-pi / 2, grip_l=True, x_r=forward_amount, y_r=-cloth_width / 2.0, z_r=height, roll_r=0, pitch_r=0, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=duration): return_val = FAILURE return SUCCESS
def executeFold(self,gripPts,endPts,color_current='blue',color_next='blue'): """ execute a fold """ #--- TODO--- convert gripPts,endPts to current frame of robot gripPts_new = [] for pt in gripPts: if pt == None: continue pt_world = self.convert_to_world_frame(pt) now = rospy.Time.now() self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0)) pt_transformed = self.listener.transformPoint("base_footprint",pt_world) gripPts_new.append(pt_transformed) endPts_new = [] for pt in endPts: if pt == None: continue pt_world = self.convert_to_world_frame(pt) now = rospy.Time.now() self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0)) pt_transformed = self.listener.transformPoint("base_footprint",pt_world) endPts_new.append(pt_transformed) gripPts = gripPts_new endPts = endPts_new if(gripPts[1] == None): self.executeFold_left(gripPt=gripPts[0],endPt=endPts[0],color_current=color_current,color_next=color_next) return elif(gripPts[0] == None): self.executeFold_right(gripPt=gripPts[1],endPt=endPts[1],color_current=color_current,color_next=color_next) return # if blue fold, move arms to gripping position. (if red fold, arms already holding cloth at gripping position) if (color_current == 'blue'): if (len(gripPts) > 2) or (len(endPts) > 2): rospy.loginfo("Requires too many grippers") return False startpoints = [] for pt in gripPts: #pt_world = self.convert_to_world_frame(pt) pt_world = self.dupl_PointStamped(pt) startpoints.append(pt_world) # determine approach yaws #yaw_l = -pi/3 if gripPts[0].point.x < endPts[0].point.x else -4*pi/3 # left gripper #yaw_r = pi/3 if gripPts[1].point.x > endPts[1].point.x else 4*pi/3 # right gripper # move both arms if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True, point_r=startpoints[1],roll_r=pi/2,yaw_r=pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005): print "failure" return False midpoints1 = [] midpoints = [] for pt in gripPts: pt_world = pt# self.convert_to_world_frame(pt) midpoints1.append(pt_world) i = 0 for pt in endPts: pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt) pt_world.point.x = (pt_world.point.x + midpoints1[i].point.x)/2.0 pt_world.point.y = (pt_world.point.y + midpoints1[i].point.y)/2.0 pt_world.point.z = pt_world.point.z + 0.1 midpoints.append(pt_world) i +=1 pitch_l = pitch_r = pi/4 roll_l = -pi/2 roll_r = pi/2 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True frame_l=frame_r = midpoints[0].header.frame_id if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l, x_r=midpoints[1].point.x,y_r=midpoints[1].point.y,z_r=midpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r,dur=7.5): print "failure" #return False endpoints = [] for pt in endPts: pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt) endpoints.append(pt_world) frame_l=frame_r = endpoints[0].header.frame_id if not GripUtils.go_to_multi (x_l=endpoints[0].point.x,y_l=endpoints[0].point.y,z_l=endpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l ,x_r=endpoints[1].point.x,y_r=endpoints[1].point.y,z_r=endpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5): print "failure" #return False if(color_next == 'blue'): initRobot() return True
def execute(self, userdata): cloth_width = userdata.cloth_width if cloth_width < 0.35: rospy.logwarn( "Cannot shake at less than 0.35m apart; tried to do %s" % str(cloth_width)) return FAILURE if cloth_width > 0.65: cloth_width = 0.65 forward_amount = 0.45 height = 0.625 drop_amount = 0.3 lateral_amount = 0.1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to_multi(x_l=forward_amount, y_l=cloth_width / 2.0, z_l=height, roll_l=0, pitch_l=0, yaw_l=-pi / 2, grip_l=True, x_r=forward_amount, y_r=-cloth_width / 2.0, z_r=height, roll_r=0, pitch_r=0, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=duration): return FAILURE duration = .45 return_val = SUCCESS if not GripUtils.go_to_multi( x_l=forward_amount, y_l=cloth_width / 2.0 - lateral_amount, z_l=height - drop_amount, roll_l=0, pitch_l=pi / 4, yaw_l=-pi / 2, grip_l=True, x_r=forward_amount, y_r=-cloth_width / 2.0 + lateral_amount, z_r=height - drop_amount, roll_r=0, pitch_r=pi / 4, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=duration): return_val = FAILURE if not GripUtils.go_to_multi(x_l=forward_amount, y_l=cloth_width / 2.0, z_l=height, roll_l=0, pitch_l=0, yaw_l=-pi / 2, grip_l=True, x_r=forward_amount, y_r=-cloth_width / 2.0, z_r=height, roll_r=0, pitch_r=0, yaw_r=pi / 2, grip_r=True, frame_l="table_height", frame_r="table_height", dur=duration): return_val = FAILURE cloth_width = cloth_width * self.clothWidthScaleFactor if not GripUtils.go_to_multi(x_l=forward_amount + .2, y_l=cloth_width / 2.0, z_l=height, roll_l=0, pitch_l=0, yaw_l=-pi / 4, grip_l=True, x_r=forward_amount + .2, y_r=-cloth_width / 2.0, z_r=height, roll_r=0, pitch_r=0, yaw_r=pi / 4, grip_r=True, frame_l="table_height", frame_r="table_height", dur=4.0): return FAILURE return SUCCESS
def open_sock(): x = 0.5 y = 0.0 z = TABLE_HEIGHT + 0.4 frame = "base_footprint" #old strategy GripUtils.go_to(x=x, y=y + 0.007, z=z, roll=pi / 2, yaw=pi / 2, pitch=-pi / 8, grip=True, frame=frame, arm="r", dur=5.0) GripUtils.go_to(x=x, y=y + 0.05, z=z, roll=pi / 2, yaw=-pi / 2, pitch=0, grip=False, frame=frame, arm="l", dur=5.0) GripUtils.go_to(x=x, y=y - 0.004, z=z + 0.01, roll=pi / 2, yaw=-pi / 2, pitch=pi / 5, grip=False, frame=frame, arm="l", dur=5.0) GripUtils.go_to(x=x, y=y - 0.0035, z=z + 0.001, roll=pi / 2, yaw=-pi / 2, pitch=pi / 5, grip=True, frame=frame, arm="l", dur=2.0) #changed from -0.007 to -0.0065 #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0) #GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0) GripUtils.close_gripper("l", extra_tight=True) open_amt = 0.025 GripUtils.go_to_multi(x_l=x, y_l=y + open_amt, z_l=z + 0.01, roll_l=pi / 2, yaw_l=-pi / 2, pitch_l=0, grip_l=True, frame_l=frame, x_r=x, y_r=y - open_amt, z_r=z + 0.01, roll_r=pi / 2, yaw_r=pi / 2, pitch_r=0, grip_r=True, frame_r=frame, dur=5.0)
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 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 put_on_dopple(depth=0.28): open_amt = 0.02 x = DOPPLE_X + 0.0075 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 GripUtils.go_to_multi(x_l=x_l - 0.1, y_l=y_l, z_l=z_l, roll_l=roll_l + pi / 2, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r - 0.1, y_r=y_r, z_r=z_r, roll_r=roll_r - pi / 2, 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.05, y_l=y_l, z_l=z_l, roll_l=roll_l + pi / 4, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame_l, x_r=x_r - 0.05, y_r=y_r, z_r=z_r, roll_r=roll_r - pi / 4, 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, 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=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) z_l = z_r = TABLE_HEIGHT + DOPPLE_HEIGHT - depth 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=5.0)
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 smooth(pt, side): y_offset = -0.08 x_offset = 0.02 x = pt.point.x + x_offset y = pt.point.y + y_offset z = pt.point.z frame = "base_footprint" x_l = x x_r = x y_l = y y_r = y z_l = z_r = z roll_l = roll_r = 0 yaw_l = -pi / 2 yaw_r = pi / 2 pitch_l = pitch_r = pi / 6 grip_l = grip_r = True GripUtils.go_to_multi(x_l=x, y_l=y_l, z_l=z + 0.04, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame, x_r=x, y_r=y_r, z_r=z + 0.04, roll_r=roll_l, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame, dur=5.0) GripUtils.go_to_multi(x_l=x, y_l=y_l, z_l=z, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame, x_r=x, y_r=y_r, z_r=z, roll_r=roll_l, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame, dur=3.0) y_l += 0.06 y_r -= 0.06 pitch_l = pi / 4 pitch_r = pi / 4 GripUtils.go_to_multi(x_l=x, y_l=y_l, z_l=z + 0.01, roll_l=roll_l, yaw_l=yaw_l, pitch_l=pitch_l, grip_l=grip_l, frame_l=frame, x_r=x, y_r=y_r, z_r=z + 0.01, roll_r=roll_l, yaw_r=yaw_r, pitch_r=pitch_r, grip_r=grip_r, frame_r=frame, dur=5.0)