class pick_and_place(): def __init__(self, pick_id=0, place_id=1): self.pick_pose = [-999]*7 self.pick_data = False self.place_pose = [-999]*7 self.place_data = False self.subscriber = rospy.Subscriber('ar_pose_marker', AlvarMarkers, \ self.ar_callback) self.ctrl = ArmController(event_detector=True) self.tf_transformer = TfTransformer(self.ctrl.tf_listener) self.joint_move_arm_to_side('r', blocking=False) self.joint_move_arm_to_side('l') self.default_frame = "base_link" self.grasper = ArghGrasp() """ ### experimental code def pick_up(self, whicharm='l', top_grasp = False): rospy.loginfo("pre pickup pose") self.open_gripper(whicharm) first = [1.042181865195266, 0.05674577918307219, 1.7141703410128142, -1.2368941648991192, 44.01523148337816, -1.3485333030083617, 58.22465995394929] self.ctrl.joint_movearm(whicharm, first) start = [0.6169387251649058, 0.13840014757000843, 0.7266251135486242, 0.7131394766246717, -0.7008370665851971, 0.008851073879117963, 0.013459252242671303] end = [0.591262509439721, -0.19429620128938668, 0.748909628486445, 0.7368007601058577, -0.6756313110616471, -0.01694214819160356, -0.01897195391107014] self.move_cartesian_step(whicharm, start) self.move_cartesian_step(whicharm, end, blocking=False) self.ctrl.gripper.start_gripper_event_detector(\ whicharm, blocking=True,timeout = 5) self.ctrl.cart_freeze_arm(whicharm) self.close_gripper(whicharm) curr = self.ctrl.get_cartesian_pose()[whicharm] curr[2] += 0.05 self.move_cartesian_step(whicharm,curr) self.constrained_move_arm_to_side(whicharm) return True """ def pick_up(self, whicharm='l'): if self.pick_data: self.open_gripper(whicharm) if not self.grasper.pick_up(whicharm,5, self.pick_pose): return False # move arm to side rospy.loginfo("moving arm to side") self.constrained_move_arm_to_side(whicharm) return self.ctrl.gripper.is_grasping()[whicharm] else: return False def reset_ar(self): self.pick_data = False self.place_data = False def is_close(self, desired = .664, threshold=.76): x = self.place_pose[0] if x > threshold: return (False, x - desired) else: return (True, 0) def place(self, whicharm="l", top_grasp = True, use_offset = True): if self.place_data: # get place offset from cooler if use_offset: trans, _ = self.tf_transformer.get_transform(\ 'base_link', 'ar_marker_1') if not trans: return False else: print "not finding place pose" trans = self.place_pose[:3] curr_pose = self.ctrl.get_cartesian_pose()[whicharm] rot = curr_pose[3:] #_,rot = self.ctrl.get_cartesian_pose()[whicharm] place_pose = trans + rot over_pose, under_pose = list(place_pose), list(place_pose) over_pose[2] += .2 under_pose[2] -= .2 self.reset_ar() print "%s\n%s\n%s"%(place_pose, over_pose, under_pose) rospy.loginfo("moving arm over place pose") result = self.move_cartesian_step(whicharm,over_pose) rospy.loginfo("waiting a second before placing") rospy.sleep(1) # wait a second before placing if result: rospy.loginfo("guarded move arm to place pose") result = self.guarded_cartesian_move(whicharm, under_pose) if result: rospy.loginfo("placing can") self.open_gripper(whicharm) rospy.loginfo("moving arm up over place pose") self.move_cartesian_step(whicharm, over_pose) else: rospy.loginfo("failed doing guarded move") else: rospy.loginfo("failed moving arm over") """ rospy.loginfo("moving arm up over place pose") tmp = False for i in range (5): if tmp: break tmp = self.move_cartesian_step(whicharm, over_pose) if no`t tmp: rospy.loginfo("having difficulty moving arm up " "over place pose") if not tmp: raw_input("can't find over pose") """ self.joint_move_arm_to_side(whicharm) rospy.loginfo("place result succes is: %s " % result) return result else: rospy.loginfo("no place data") return False def ar_callback(self, data): for markers in data.markers: try: px = markers.pose.pose.position.x py = markers.pose.pose.position.y pz = markers.pose.pose.position.z ox = markers.pose.pose.orientation.x oy = markers.pose.pose.orientation.y oz = markers.pose.pose.orientation.z ow = markers.pose.pose.orientation.w if markers.id == 0: # pick self.pick_pose = [px,py,pz,ox,oy,oz,ow] self.pick_data = True if markers.id == 1: # place self.place_pose = [px, py, pz, ox, oy, oz, ow] self.place_data = True except: pass #open the gripper def open_gripper(self, whicharm): self.ctrl.open_gripper(whicharm) #close the gripper def close_gripper(self, whicharm): self.ctrl.close_gripper(whicharm) def constrained_move_arm_to_side(self, whicharm): location={'l': [0.05, 0.65, .7], 'r': [0.05, -0.65, .7]} poses = self.ctrl.get_cartesian_pose() rot = poses[whicharm][3:] side_pose = location[whicharm] + rot result = self.move_cartesian_step(whicharm, side_pose, blocking=True) if not result: rospy.loginfo("cannot constrain joint moving to side") self.joint_move_arm_to_side(whicharm) def joint_move_arm_to_side(self, whicharm, blocking = 1): self.arm_to_side_angles = \ {'r': [-2.02, 0.014, -1.73, -1.97, 10.91, -1.42, 14.17], 'l': [2.02, 0.014, 1.73, -1.97, -10.91, -1.42, 14.17]} self.ctrl.joint_movearm(whicharm,self.arm_to_side_angles[whicharm],\ blocking=blocking) def guarded_cartesian_move(self, whicharm, pose, frame="base_link",\ move_duration = 15): start_pose = self.ctrl.get_cartesian_pose()[whicharm][:3] self.move_cartesian_step(whicharm, pose, move_duration=7.0,\ frame_id=frame, blocking=False) self.ctrl.gripper.start_gripper_event_detector(\ whicharm, blocking=True,timeout=move_duration) self.ctrl.cancel_goal(whicharm) # did arm even move? curr_pose = self.ctrl.get_cartesian_pose()[whicharm][:3] diff = np.linalg.norm(np.array(start_pose) - np.array(curr_pose)) not_at_start = diff > 0.1 return not_at_start #move to a Cartesian pose goal def move_cartesian_step(self, whicharm, pose, frame_id="base_link",\ move_duration=5.0, ik_timeout=5.0, blocking=True): result = self.ctrl.cart_movearm(whicharm, pose, frame_id, \ move_duration, ik_timeout, blocking) return result
class Primitives: block_size = 1e-3 * 13.0 # mm W,H = 10.0, 20.0 # tetris grid units z_above = 0.07 z_down = 0.03 sqr2 = np.sqrt(2.0)/2.0 sqr7 = np.sqrt(7.0)/4.0 vert = [-.5, .5, .5, .5] horz = [0, sqr2, 0, sqr2] #rot_r = [-sqr7, -.25, sqr7, -.25] rot_r = [-.25, sqr7, 0.25, sqr7] rot_l = [sqr7, -.25, -sqr7, -.25] init_pose = { #'r': [-0.73, -0.17, -1.60, -1.46, -32.69, -1.31, -16.94], #'l': [ 0.71, -0.04, 1.56, -1.46, -4.72, -1.36, 3.86]} 'l': [ 0.95, 0.0, np.pi/2., -np.pi/2., -np.pi*1.5, -np.pi/2.0, np.pi], 'r': [ -0.7, 0.0, -np.pi/2., -np.pi/2., -20.424894658897493, -np.pi/2.0, np.pi]} min_time = 1.0 max_time = 5.0 frame = 'tetris' def __init__(self, whicharm='l'): self.arm = ArmController() self.objects = None self.obj_sub = rospy.Subscriber("/table_publisher/object_poses", PoseArray, self.objCb) self.arm.command_torso(0.26) self.whicharm = whicharm self.move_arm_to_side() def move_arm_to_side(self): self.arm.joint_movearm(self.whicharm, self.init_pose[self.whicharm], 3, True) def lift_arm(self, rating=1.0): rospy.sleep(0.1) curr_pos = self.arm.get_cartesian_pose(self.frame)[self.whicharm] curr_pos[2] = self.z_above *rating goal =[ self.stiff_pose(curr_pos, 0.1) + [0.25*self.min_time*rating], self.stiff_pose(curr_pos, 0.5) + [0.5*self.min_time*rating], self.stiff_pose(curr_pos, 1.0) + [self.min_time*rating] ] self.arm.cart_movearm(self.whicharm, goal , self.frame, True) #raw_input("done lift") def stiff_pose(self, pose, rating = 0.2): pose_force = int(100*rating) torque_force = int(50*rating) return list(pose) + [pose_force]*3 + [torque_force]*3 + [False]*6 def distance_between_two_pts(self, p1,p2): pts = zip(p1,p2) dx = [(x1 - x2)**2 for (x1,x2) in pts] return sum(dx) def clip_time(self, t): if t < self.min_time: return self.min_time elif t > self.max_time: return self.max_time else: return t def approach_pose(self, pose): curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm] time = self.clip_time(15*self.distance_between_two_pts(curr_pose, pose)) curr_pose[2] = self.z_above middle_pose = list(pose) middle_pose[2] = self.z_above traj = [ self.stiff_pose(curr_pose, 0.1) + [time*.1], self.stiff_pose(curr_pose, 1.0) + [time*.2], self.stiff_pose(middle_pose, 1.5) + [time*.8], self.free_push(pose, 1.5) + [time] ] self.arm.cart_movearm(self.whicharm, traj, self.frame, True) rospy.sleep(0.1) #final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm] #dist = self.distance_between_two_pts(pose, final_pose) def do_force_control(self, cmd, timeout=4.0): self.approach_pose(cmd[:7]) rospy.sleep(0.1) curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm] ros_timeout = rospy.Time.now() + rospy.Duration(timeout) self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False) rospy.sleep(0.2) while rospy.Time.now() < ros_timeout and not rospy.is_shutdown(): rospy.sleep(0.1) if not self.arm.is_moving(self.whicharm): self.arm.cancel_goal(self.whicharm) break """ final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm] if self.distance_between_two_pts(curr_pose, final_pose) < self.block_size: ros_timeout = rospy.Time.now() + rospy.Duration(timeout) self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False) rospy.sleep(1.0) while rospy.Time.now() < ros_timeout: rospy.sleep(0.1) if not self.arm.is_moving(self.whicharm): self.arm.cancel_goal(self.whicharm) break """ self.lift_arm() def free_space_push_down(self, pos1, pos2): pose1 = list(pos1) + self.horz pose2 = list(pos2) + self.horz time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2)) self.approach_pose(pose1) traj = [ self.free_push(pose1) + [time*.1], self.free_push(pose2) + [time] ] self.arm.cart_movearm(self.whicharm, traj, self.frame, True) self.lift_arm(1.) def free_space_push_right(self, pos1, pos2): pose1 = list(pos1) + self.vert pose2 = list(pos2) + self.vert time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2)) self.approach_pose(pose1) traj = [ self.free_push(pose1) + [time*.1], self.free_push(pose2) + [time] ] self.arm.cart_movearm(self.whicharm, traj, self.frame, True) self.lift_arm(1.) def free_push(self, pose, rating=1.0): # position controlled fx,fy,fz = [300*rating]*3 # max stiffness ox,oy,oz = [10*rating]*3 fz = -5*rating # force in downard direction states = [False]*6 states[2] = True # use force control for Z return list(pose) + [fx,fy,fz,ox,oy,oz] + states def right_contact_slide_down(self, pos, rating=1.0): pose = list(pos) + self.rot_r #use force control fx = -7*rating fy = -3 * rating fz = -2* rating #keep gripper rotated ox,oy,oz = [30]*3 states =[True, True, True] + [False]*3 cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states self.do_force_control(cmd) def left_contact_slide_down(self, pos, rating=1.0): pose = list(pos) + self.rot_l #use force control fx = -7*rating fy = 2*rating fz = -2*rating #keep gripper rotated ox,oy,oz = [30]*3 states =[True, True, True] + [False]*3 cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states self.do_force_control(cmd) def right_contact_slide_right(self, pos, rating=1.0): pose = list(pos) + self.rot_r #use force control fx = -2*rating fy = -5*rating fz = -2*rating #keep gripper rotated ox,oy,oz = [30]*3 states =[True, True, True] + [False]*3 cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states self.do_force_control(cmd) def push_down(self, pos, rating=1.0): pose = list(pos) + self.horz #use force control fx = -7*rating fy = 300*rating fz = -3*rating #keep gripper rotated ox,oy,oz = [30]*3 states =[True, False, True] + [False]*3 cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states self.do_force_control(cmd) def push_right(self, pos, rating=1.0): pose = list(pos) + self.vert #use force control fx = 400*rating fy = -5 *rating fz = -3*rating #keep gripper rotated ox,oy,oz = [30]*3 states =[False, True, True] + [False]*3 cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states self.do_force_control(cmd) def push_left(self, pos, rating=1.0): pose = list(pos) + self.vert #use force control fx = 300*rating fy = 5 *rating fz = -3*rating #keep gripper rotated ox,oy,oz = [30]*3 states =[False, True, True] + [False]*3 cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states self.do_force_control(cmd) def convert_xy(self, (x,y) ): return (x*self.block_size, y*self.block_size, self.z_down)