def execute(self, userdata): pose = userdata.pose_in side = get_pickup_side(pose['name']) rotate = rotate_only(side=side, turned=False) go([rotate]) return 'done'
def execute(self, userdata): # side = position['side'] rotate = rotate_only('left', turned=False) arm_up = [ rotate[0], joints['arm_joint_2']['straight'], joints['arm_joint_3']['straight'], joints['arm_joint_4']['straight'], rotate[4] ] tuck = [ rotate[0], joints['arm_joint_2']['min'], joints['arm_joint_3']['max'], joints['arm_joint_4']['min'], rotate[4] ] # print arm_up, tuck #return 'success' if go([rotate, arm_up, tuck]): self.counter = 0 return 'success' else: if self.counter < MAX_TRIES: self.counter += 1 return 'not_reached' else: self.counter = 0 return 'failed'
def execute(self, userdata): pose = userdata.pose_in side = get_pickup_side(pose['name']) rotate = rotate_only(side=side, turned=False) go([rotate]) return 'done'
def execute(self, userdata): pose = userdata.pose_in position = rospy.get_param(pose) side = position['side'] rotate = rotate_only(side, turned=True) pre_kinect[0] = rotate[0] userdata.pose_out = pose if go([rotate, pre_kinect]): return 'success' else: return 'failed'
def execute(self, userdata): pose = userdata.pose_in position = rospy.get_param(pose) side = position['side'] rotate = rotate_only(side, turned=True) pre_kinect[0] = rotate[0] userdata.pose_out = pose if go([rotate, pre_kinect]): return 'success' else: return 'failed'
def execute(self, userdata): side = 'front' rotate = rotate_only(side) conf = [x for x in pre_grip] conf[0] = rotate[0] + 0.2 conf[3] -= 0.3 conf[1] = joints['arm_joint_2']['straight'] # gripperClose(False) # rotate, go([conf], blocking=self.blocking) return 'success'
def execute(self, userdata): side = 'front' rotate = rotate_only(side) conf = [x for x in pre_grip] conf[0] = rotate[0] + 0.2 conf[3] -= 0.3 conf[1] = joints['arm_joint_2']['straight'] # gripperClose(False) # rotate, go([conf], blocking=self.blocking) return 'success'
def execute(self, userdata): pose = userdata.pose_in # position = rospy.get_param(pose) # print 'go', pose side = get_pickup_side(pose['name']) #print 'go' rotate = rotate_only(side) pre_grip[0] = rotate[0] # gripperClose(False) # rotate, go([pre_grip], blocking=self.blocking) return 'success'
def execute(self, userdata): # locations = rosparam.get('locations') # pose = locations[0] pose = userdata.pose_in side = get_pickup_side(pose['name']) rotate = rotate_only(side) #pre_grip[0] = rotate[0] #up = [x for x in cam_test] #up[0] = configuration[0] if go([rotate]): return 'success' else: return 'failed'
def execute(self, userdata): # locations = rosparam.get('locations') # pose = locations[0] pose = userdata.pose_in side = get_pickup_side(pose['name']) rotate = rotate_only(side) #pre_grip[0] = rotate[0] #up = [x for x in cam_test] #up[0] = configuration[0] if go([rotate]): return 'success' else: return 'failed'
def execute(self, userdata): pose = userdata.pose_in # position = rospy.get_param(pose) # print 'go', pose side = get_pickup_side(pose['name']) #print 'go' rotate = rotate_only(side) pre_grip[0] = rotate[0] # gripperClose(False) # rotate, go([pre_grip], blocking=self.blocking) return 'success'
def execute(self, userdata): confs = [] if not self.no_rotate: pose = userdata.pose_in side = get_pickup_side(pose['name']) # print joints tucked = rotate_only(side='left') #tucked[0] = configuration[0] #tucked[1] = joints['arm_joint_2']['min'] #tucked[2] = joints['arm_joint_3']['max'] #tucked[3] = joints['arm_joint_4']['min'] #tucked[4] = configuration[4] confs.append(tucked) tucked_for_drive = rotate_only(side='left') # tucked[0] = conf[0] tucked_for_drive[1] = joints['arm_joint_2']['min'] tucked_for_drive[2] = joints['arm_joint_3']['max'] tucked_for_drive[3] = joints['arm_joint_4']['min'] tucked_for_drive[4] = joints['arm_joint_5']['straight'] confs.append(tucked_for_drive) go(confs, blocking=self.blocking) return 'done'
def execute(self, userdata): confs = [] if not self.no_rotate: pose = userdata.pose_in side = get_pickup_side(pose['name']) # print joints tucked = rotate_only(side='left') #tucked[0] = configuration[0] #tucked[1] = joints['arm_joint_2']['min'] #tucked[2] = joints['arm_joint_3']['max'] #tucked[3] = joints['arm_joint_4']['min'] #tucked[4] = configuration[4] confs.append(tucked) tucked_for_drive = rotate_only(side='left') # tucked[0] = conf[0] tucked_for_drive[1] = joints['arm_joint_2']['min'] tucked_for_drive[2] = joints['arm_joint_3']['max'] tucked_for_drive[3] = joints['arm_joint_4']['min'] tucked_for_drive[4] = joints['arm_joint_5']['straight'] confs.append(tucked_for_drive) go(confs, blocking=self.blocking) return 'done'
def execute(self, userdata): rospy.sleep(4) gripper_close(True) tucked = [x for x in configuration] # tucked[0] = configuration[0] tucked[1] = joints['arm_joint_2']['min'] tucked[2] = joints['arm_joint_3']['max'] tucked[3] = joints['arm_joint_4']['min'] # tucked[4] = configuration[4] tucked_for_drive = rotate_only(side='left') #tucked[0] = conf[0] tucked_for_drive[1] = joints['arm_joint_2']['min'] tucked_for_drive[2] = joints['arm_joint_3']['max'] tucked_for_drive[3] = joints['arm_joint_4']['min'] tucked_for_drive[4] = joints['arm_joint_5']['straight'] go([tucked, tucked_for_drive]) return 'end'
def execute(self, userdata): rospy.sleep(4) gripper_close(True) tucked = [x for x in configuration] # tucked[0] = configuration[0] tucked[1] = joints['arm_joint_2']['min'] tucked[2] = joints['arm_joint_3']['max'] tucked[3] = joints['arm_joint_4']['min'] # tucked[4] = configuration[4] tucked_for_drive = rotate_only(side='left') #tucked[0] = conf[0] tucked_for_drive[1] = joints['arm_joint_2']['min'] tucked_for_drive[2] = joints['arm_joint_3']['max'] tucked_for_drive[3] = joints['arm_joint_4']['min'] tucked_for_drive[4] = joints['arm_joint_5']['straight'] go([tucked, tucked_for_drive]) return 'end'
def execute(self, userdata): pose = userdata.pose_in # position = rospy.get_param(pose) userdata.pose_out = pose side = get_pickup_side(pose['name']) rotate = rotate_only(side) # pre_grip[0] = rotate[0] gripper_close(False) new_grip_point = [x for x in grip_point] new_grip_point[0] += 0.05 new_grip_point[2] += 0.015 conf = np.array(call_ik_solver(new_grip_point, side=side)) conf[4] = get_angle(conf[4], math.pi / 2) if go([rotate, conf]): return 'success' else: return 'failed'
def execute(self, userdata): pose = userdata.pose_in # position = rospy.get_param(pose) userdata.pose_out = pose side = get_pickup_side(pose['name']) rotate = rotate_only(side) # pre_grip[0] = rotate[0] gripper_close(False) new_grip_point = [x for x in grip_point] new_grip_point[0] += 0.05 new_grip_point[2] += 0.015 conf = np.array(call_ik_solver(new_grip_point, side=side)) conf[4] = get_angle(conf[4], math.pi / 2) if go([rotate, conf]): return 'success' else: return 'failed'
def execute(self, userdata): # side = position['side'] rotate = rotate_only('left', turned=False) arm_up = [rotate[0], joints['arm_joint_2']['straight'], joints['arm_joint_3']['straight'], joints['arm_joint_4']['straight'], rotate[4]] tuck = [rotate[0], joints['arm_joint_2']['min'], joints['arm_joint_3']['max'], joints['arm_joint_4']['min'], rotate[4]] # print arm_up, tuck #return 'success' if go([rotate, arm_up, tuck]): self.counter = 0 return 'success' else: if self.counter < MAX_TRIES: self.counter += 1 return 'not_reached' else: self.counter = 0 return 'failed'
def execute(self, userdata): obj = userdata.object_in # offset detected by object detection offset = userdata.point_in pose = userdata.pose_in # to which side side = get_pickup_side(pose['name']) quat = [offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w] r, p, y = tf.transformations.euler_from_quaternion(quat) new_grip_point = [x for x in grip_point] new_grip_point[0] += offset.position.x new_grip_point[1] -= offset.position.y new_grip_point[2] += get_pick_height_for_obj(obj['label']) publish_status('I am grasping object ' + str(obj['name']) + '.') ## TODO check HEIGHT FOR HIGH OBJECTS if NO_ARM: return 'success' conf = None try: conf = call_ik_solver(new_grip_point, side=side) except: print "got no config from IK Solver" if len(conf) == 0: return 'too_far' else: conf = np.array(conf) grip_angle = get_angle(conf[4], y) straight_angle = conf[4] conf[4] = grip_angle height = new_grip_point[2] new_grip_point[2] += PRE_GRIP_HEIGHT confs = [] rotate = rotate_only(side=side) confs.append(rotate) first = None gripX = new_grip_point[0] gripY = new_grip_point[1] while new_grip_point[2] > height: new_grip_point[2] -= STEP_SIZE new_grip_point[2] = max(height, new_grip_point[2]) new_grip_point[0] = gripX new_grip_point[1] = gripY conf2 = [] while len(conf2) == 0: try: print 'get conf', new_grip_point conf2 = call_ik_solver(new_grip_point, side=side) print conf2 except: #new_grip_point[0] -= 0.01 #get closer to solve pre-grip print 'got not config' return 'too_far' if len(conf2) > 0: conf2 = np.array(conf2) print conf2 new_grip_point[0] -= 0.005 #get closer to solve pre-grip conf2[4] = conf[4] if first is None: first = [x for x in conf2] confs.append(conf2) gripper_close(False) confs.append(conf) if not go(confs): return 'failed' if userdata.object_in['label'] == 'M20_100_h': obj = userdata.object_in ### TODO set correct rotation obj['rotation'] = -y - (grip_angle - straight_angle) userdata.object_out = obj print "rotation is", obj['rotation'], y, grip_angle - straight_angle gripper_close(True, heavy=True) else: gripper_close(True) if not go([first]): return 'failed_after_grip' return 'success'
def execute(self, userdata): obj = userdata.object_in # offset detected by object detection offset = userdata.point_in pose = userdata.pose_in # to which side side = get_pickup_side(pose['name']) quat = [ offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w ] r, p, y = tf.transformations.euler_from_quaternion(quat) new_grip_point = [x for x in grip_point] new_grip_point[0] += offset.position.x new_grip_point[1] -= offset.position.y new_grip_point[2] += get_pick_height_for_obj(obj['label']) publish_status('I am grasping object ' + str(obj['name']) + '.') ## TODO check HEIGHT FOR HIGH OBJECTS if NO_ARM: return 'success' conf = None try: conf = call_ik_solver(new_grip_point, side=side) except: print "got no config from IK Solver" if len(conf) == 0: return 'too_far' else: conf = np.array(conf) grip_angle = get_angle(conf[4], y) straight_angle = conf[4] conf[4] = grip_angle height = new_grip_point[2] new_grip_point[2] += PRE_GRIP_HEIGHT confs = [] rotate = rotate_only(side=side) confs.append(rotate) first = None gripX = new_grip_point[0] gripY = new_grip_point[1] while new_grip_point[2] > height: new_grip_point[2] -= STEP_SIZE new_grip_point[2] = max(height, new_grip_point[2]) new_grip_point[0] = gripX new_grip_point[1] = gripY conf2 = [] while len(conf2) == 0: try: print 'get conf', new_grip_point conf2 = call_ik_solver(new_grip_point, side=side) print conf2 except: #new_grip_point[0] -= 0.01 #get closer to solve pre-grip print 'got not config' return 'too_far' if len(conf2) > 0: conf2 = np.array(conf2) print conf2 new_grip_point[0] -= 0.005 #get closer to solve pre-grip conf2[4] = conf[4] if first is None: first = [x for x in conf2] confs.append(conf2) gripper_close(False) confs.append(conf) if not go(confs): return 'failed' if userdata.object_in['label'] == 'M20_100_h': obj = userdata.object_in ### TODO set correct rotation obj['rotation'] = -y - (grip_angle - straight_angle) userdata.object_out = obj print "rotation is", obj[ 'rotation'], y, grip_angle - straight_angle gripper_close(True, heavy=True) else: gripper_close(True) if not go([first]): return 'failed_after_grip' return 'success'
def execute(self, userdata): # offset detected by object detection offset = userdata.point_in pose = userdata.pose_in obj = userdata.object_in side = get_pickup_side(pose['name']) # to which side publish_status('I am placing the object <br> on the service area.') quat = [offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w] r, p, y = tf.transformations.euler_from_quaternion(quat) new_grip_point = [x for x in grip_point] new_grip_point[0] += offset.position.x new_grip_point[1] -= offset.position.y new_grip_point[2] += get_place_height_for_obj(obj['label']) if NO_ARM: return 'success' conf = None try: conf = call_ik_solver(new_grip_point, side=side) except: print "got no config from IK Solver" if len(conf) == 0: return 'too_far' else: conf = np.array(conf) straight = conf[4] grip_angle = get_angle(conf[4], y) conf[4] = grip_angle new_grip_point[2] += PRE_PLACE_HEIGHT rotate = rotate_only(side=side) gripX = new_grip_point[0] gripY = new_grip_point[1] new_grip_point[0] = gripX new_grip_point[1] = gripY conf2 = [] while len(conf2) == 0: try: print 'get conf', new_grip_point conf2 = call_ik_solver(new_grip_point, side=side) print conf2 except: # new_grip_point[0] -= 0.01 #get closer to solve pre-grip print 'got not config' return 'too_far' if len(conf2) > 0: conf2 = np.array(conf2) new_grip_point[0] -= 0.005 #get closer to solve pre-grip conf2[4] = conf[4] go([rotate]) print obj if obj['label'] == 'M20_100_h' and obj['type'] == 'PPT': m20_rot = obj['rotation'] yaw = self.get_rotation(y, straight, m20_rot) if yaw is None: print "rotate screw!" yaw = self.get_rotation(y, straight, math.pi - abs(m20_rot)) temp_y = get_angle(straight, y - math.pi / 2.) scnd_tmp = temp_y + math.pi if scnd_tmp > joints['arm_joint_5']['max']: scnd_tmp -= 2. * math.pi conf[4] = temp_y conf2[4] = temp_y go([conf2, conf]) gripper_close(False) go([conf2]) conf[4] = scnd_tmp conf2[4] = scnd_tmp go([conf2, conf]) gripper_close(True, heavy=True) go([conf2]) conf[4] = yaw conf2[4] = yaw if not go([conf2, conf]): return 'failed' gripper_close(False) #rotate?? if False: rotate_wiggle = [x for x in conf] rotate_wiggle[4] -= 0.1 rotate_wiggle2 = [x for x in conf] rotate_wiggle2[4] += 0.1 if not go([rotate_wiggle, rotate_wiggle2, conf]): return 'failed' if not go([conf2]): return 'failed_after_place' return 'success'
def execute(self, userdata): # offset detected by object detection offset = userdata.point_in pose = userdata.pose_in obj = userdata.object_in side = get_pickup_side(pose['name']) # to which side publish_status('I am placing the object <br> on the service area.') quat = [ offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w ] r, p, y = tf.transformations.euler_from_quaternion(quat) new_grip_point = [x for x in grip_point] new_grip_point[0] += offset.position.x new_grip_point[1] -= offset.position.y new_grip_point[2] += get_place_height_for_obj(obj['label']) if NO_ARM: return 'success' conf = None try: conf = call_ik_solver(new_grip_point, side=side) except: print "got no config from IK Solver" if len(conf) == 0: return 'too_far' else: conf = np.array(conf) straight = conf[4] grip_angle = get_angle(conf[4], y) conf[4] = grip_angle new_grip_point[2] += PRE_PLACE_HEIGHT rotate = rotate_only(side=side) gripX = new_grip_point[0] gripY = new_grip_point[1] new_grip_point[0] = gripX new_grip_point[1] = gripY conf2 = [] while len(conf2) == 0: try: print 'get conf', new_grip_point conf2 = call_ik_solver(new_grip_point, side=side) print conf2 except: # new_grip_point[0] -= 0.01 #get closer to solve pre-grip print 'got not config' return 'too_far' if len(conf2) > 0: conf2 = np.array(conf2) new_grip_point[0] -= 0.005 #get closer to solve pre-grip conf2[4] = conf[4] go([rotate]) print obj if obj['label'] == 'M20_100_h' and obj['type'] == 'PPT': m20_rot = obj['rotation'] yaw = self.get_rotation(y, straight, m20_rot) if yaw is None: print "rotate screw!" yaw = self.get_rotation(y, straight, math.pi - abs(m20_rot)) temp_y = get_angle(straight, y - math.pi / 2.) scnd_tmp = temp_y + math.pi if scnd_tmp > joints['arm_joint_5']['max']: scnd_tmp -= 2. * math.pi conf[4] = temp_y conf2[4] = temp_y go([conf2, conf]) gripper_close(False) go([conf2]) conf[4] = scnd_tmp conf2[4] = scnd_tmp go([conf2, conf]) gripper_close(True, heavy=True) go([conf2]) conf[4] = yaw conf2[4] = yaw if not go([conf2, conf]): return 'failed' gripper_close(False) #rotate?? if False: rotate_wiggle = [x for x in conf] rotate_wiggle[4] -= 0.1 rotate_wiggle2 = [x for x in conf] rotate_wiggle2[4] += 0.1 if not go([rotate_wiggle, rotate_wiggle2, conf]): return 'failed' if not go([conf2]): return 'failed_after_place' return 'success'