def mover_setup(self): limb = Limb("right") # Right arm planner self.planner = PathPlanner("right_arm") # Left arm planner self.planner_left = PathPlanner("left_arm") place_holder = {'images': [], 'camera_infos': []} #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) count = 0 print(limb.endpoint_pose()) calibration_points = [] if ROBOT == "sawyer": Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3]) Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) else: Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4]) Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Controller for right arm self.c = Controller(Kp, Ki, Kd, Kw, Limb('right')) self.orien_const = OrientationConstraint() self.orien_const.link_name = "right_gripper"; self.orien_const.header.frame_id = "base"; self.orien_const.orientation.y = -1.0; self.orien_const.absolute_x_axis_tolerance = 0.1; self.orien_const.absolute_y_axis_tolerance = 0.1; self.orien_const.absolute_z_axis_tolerance = 0.1; self.orien_const.weight = 1.0; box = PoseStamped() box.header.frame_id = "base" box.pose.position.x = self.box.pose.position.x box.pose.position.y = self.box.pose.position.y # box.pose.position.z = self.box.pose.position.z box.pose.position.z = self.conveyor_z # box.pose.position.x = 0.5 # box.pose.position.y = 0.0 # box.pose.position.z = 0.0 box.pose.orientation.x = 0.0 box.pose.orientation.y = 0.0 box.pose.orientation.z = 0.0 box.pose.orientation.w = 1.0 self.planner.add_box_obstacle((100, 100, 0.00001), "box", box) # Controller for left arm self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def __init__(self, gsl, sil, vil): self.LOG.info('init') self._sil = sil self.tc = ThrottleControl(gsl) self.sc = SteeringControl(gsl) self.tlc = TopLevelControl(gsl, sil, self.tc, self.sc) self.pp = PathPlanner(gsl, sil, vil, self.tc, self.sc)
def main(): global thetas configureLeftHand() planner = PathPlanner("right_arm") grip = Gripper('right') grip.calibrate() raw_input("gripper calibrated") grip.open() table_size = np.array([3, 1, 10]) table_pose = PoseStamped() table_pose.header.frame_id = "base" table_pose.pose.position.x = .9 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = -5 - .112 #put cup cup_pos = start_pos_xy end_pos = goal_pos_xy putCupObstacle(planner, cup_pos, "cup1") putCupObstacle(planner, end_pos, "cup2") planner.add_box_obstacle(table_size, "table", table_pose) #move gripper to behind cup position = cup_pos + np.array([-0.1, 0, -0.02]) orientation = np.array([0, np.cos(np.pi / 4), 0, np.cos(np.pi / 4)]) executePositions(planner, [position], [orientation]) raw_input("moved behind cup, press enter") #move to cup and remove cup1 as obstacle since picking up removeCup(planner, "cup1") position = cup_pos + np.array([0, 0, -0.05]) executePositions(planner, [position], [orientation]) raw_input("moved to cup, press to close") grip.close() rospy.sleep(1) raw_input("gripped") moveAndPour(planner, end_pos) raw_input("poured") executePositions(planner, [position + np.array([0, 0, 0.02])], [orientation]) grip.open() removeCup(planner, "cup2")
def main(): get_top_left_AR_tag() # print("Here are the marker rotations: " + str(marker_rotations)) # print("Here are the marker translations: " + str(marker_translations)) global top_left_x global top_left_y global top_left_z assert len(marker_translations) == 1 R = marker_rotations[0] t = marker_translations[0] top_left_x, top_left_y, top_left_z = t sawyer = Robot(PathPlanner("right_arm"), None, "right_gripper", 0.095) # Adding environment obstacles for the robot to avoid during path planning # add_block_obstacle(sawyer, position=[0.67452, -0.4455, 0.916557], name='right_wall', size=[5, 0, 5], orient=[1, 0, 0, 0]) add_block_obstacle(sawyer, position=[0.460625, 0.5444, 0.916557], name='left_wall', size=[5, 0, 5], orient=[1, 0, 0, 0]) # add_block_obstacle(sawyer, position=[, , TABLE_HEIGHT - sawyer.gripper_length], name='table', size=[3.5, 3.5, 0], orient=[0, 1, 0, 0]) # add_block_obstacle(planner, [0,0,0], 'ceiling', [10, 5, 1]) add_block_obstacle( sawyer, position=[0.81, -0.26, -0.1 + (0.5 * BLOCK_TOTAL_HEIGHT)], name='baseplate', size=[ 5 * BLOCK_SIDE_LENGTH, 6 * BLOCK_SIDE_LENGTH, 0.5 * BLOCK_TOTAL_HEIGHT ]) add_block_obstacle(sawyer, position=[0.81, -0.26, -0.1 - (BLOCK_TOTAL_HEIGHT / 2)], name='table', size=[5, 5, BLOCK_TOTAL_HEIGHT / 2]) #TODO: Finish adding as many environment constraints as possible without compromising on path planning. dest_x, dest_y = block_coord_to_sawyer((4, 5)) print('Top left:', top_left_x, top_left_y) start_x3 = top_left_x - 0.08 - (BLOCK_SIDE_LENGTH / 2.0) start_y3 = top_left_y + 0.0335 print(start_x3, start_y3) start_x3, start_y3 = start_num_to_sawyer(3) print(start_x3, start_y3) print() for i in range(8): print(start_num_to_sawyer(i)) while 1 < 2: pick_and_place_block(sawyer, start_x3, start_y3, -0.0320774, dest_x, dest_y, top_left_z + (0.5 * BLOCK_SQUARE_HEIGHT), 'block_test')
def execute_rendezvous( self, master_coords, drone_coords ): # calculates rendezvous path and sets nav goal for robots at the provided poses if self.executing_rendezvous or not self.map_initialized: return False # init and run the path planner path_planner = PathPlanner(self.occupancy_grid, self.map_width, self.map_height, self.map_resolution, self.map_origin) path_planner.set_start(master_coords) path_planner.set_goal(drone_coords) rendezvous_path = path_planner.a_star() # abort if no path found if len(rendezvous_path) == 0: return False # parse the result and start nav to goal for both robots grid_goal = rendezvous_path[int(math.floor(len(rendezvous_path) / 2))] world_goal = path_planner.grid_to_world(grid_goal) print 'executing rendezvous at ' + str(world_goal) self.master_node.start_navigation( world_goal[0], world_goal[1], 0, (master_coords[0], master_coords[1], 0)) self.drone_node.start_navigation(world_goal[0], world_goal[1], 0, (drone_coords[0], drone_coords[1], 0)) self.executing_rendezvous = True return True
def configureLeftHand(): planner = PathPlanner("left_arm") position = np.array([0.516, 0.485, 0.362]) orientation = [0.647, 0.655, -0.281, 0.271] executePositions(planner, [position], [orientation])
def grasp(key, x, y, z, x_threshhold=0.02, y_threshhold=0.02, hoverDist=0.020, zoffset=.92, orien_const=[], or_x=0.0, or_y=-1.0, or_z=0.0, or_w=0.0): #while not rospy.is_shutdown(): try: left_arm_planner = PathPlanner("left_arm") right_arm_planner = PathPlanner("right_arm") goal = PoseStamped() goal.header.frame_id = "base" #x, y, and z position goal.pose.position.x = x + x_threshhold goal.pose.position.y = y + y_threshhold goal.pose.position.z = z - zoffset + hoverDist #Orientation as a quaternion goal.pose.orientation.x = or_x goal.pose.orientation.y = or_y goal.pose.orientation.z = or_z goal.pose.orientation.w = or_w if key == 'left_arm': plan = left_arm_planner.plan_to_pose(goal, orien_const) openLeftGripper() left_arm_planner.execute_plan(plan) closeLeftGripper() else: plan = right_arm_planner.plan_to_pose(goal, orien_const) openRightGripper() right_arm_planner.execute_plan(plan) closeRightGripper() except Exception as e: print e traceback.print_exc()
def on_start(self, goal, frame): self.goal = goal filtered_image = self.obstacleDetector.applyFilter(frame) planner = PathPlanner() self.path, expanded, found_goal = planner.planPath( filtered_image, self.marbleStateManager.get_position(), goal) print("Got a path of length: " + str(len(self.path)))
def __init__(self): self.path_planner = PathPlanner() # will need a map: map_maker = MapMaker() map_maker.set_map() map_maker.calibrate_map() lowres_map = map_maker.get_lowres_map() self.path_planner.set_map(lowres_map)
def __init__(self): if not self.load_parameters(): sys.exit(1) self._planner = PathPlanner('{}_arm'.format(self._arm)) rospy.on_shutdown(self.shutdown) if not self.register_callbacks(): sys.exit(1)
def main(): # rospy.spin() left = baxter_interface.Limb('left') lj = left.joint_names() left_gripper = robot_gripper.Gripper('left') right = baxter_interface.Limb('right') rj = right.joint_names() right_gripper = robot_gripper.Gripper('right') print(right_gripper.force()) left.set_joint_position_speed(0.8) right.set_joint_position_speed(0.8) print("Initializing planner") planner_right = PathPlanner("right_arm") planner_left = PathPlanner("left_arm") print("Calling function") # move_at_angle(left, right, lj, rj, planner_left, planner_right, 0.4) return_to_neutral(left, right, lj, rj, planner_left, planner_right)
def setUp(self): self.path_planner = PathPlanner() # will need a map: map_maker = MapMaker() map_maker.set_map() map_maker.calibrate_map() lowres_map = map_maker.get_lowres_map() self.path_planner.set_map(lowres_map) self.path_planner.set_start_node() self.path_planner.set_end_node()
def __init__(self): print("Starting init") self.left_arm = baxter_interface.Limb('left') print("Left arm obtrained") self.lj = self.left_arm.joint_names() self.left_gripper = robot_gripper.Gripper('left') self.left_joint_neutral = self.left_arm.joint_angles() self.right_arm = baxter_interface.Limb('right') self.rj = self.right_arm.joint_names() self.right_gripper = robot_gripper.Gripper('right') self.right_joint_neutral = self.right_arm.joint_angles() self.planner_left = PathPlanner('left_arm') self.planner_right = PathPlanner('right_arm') self.orien_const_left_vert = OrientationConstraint() self.orien_const_left_vert.link_name = "left_gripper" self.orien_const_left_vert.header.frame_id = "base" self.orien_const_left_vert.orientation.y = -1.0 self.orien_const_left_vert.absolute_x_axis_tolerance = 0.1 self.orien_const_left_vert.absolute_y_axis_tolerance = 0.1 self.orien_const_left_vert.absolute_z_axis_tolerance = 0.1 self.orien_const_left_vert.weight = 1.0 self.orien_const_right_vert = copy.deepcopy(self.orien_const_left_vert) self.orien_const_right_vert.link_name = "right_gripper" self.previous_deviation = (None, None) self.setting_to_neutral = False self.tilt_after_neutral = True self.pub = rospy.Publisher("/board_controller_log", std_msgs.msg.String, queue_size=10) rospy.Subscriber("/physics_inference", std_msgs.msg.String, self.perform_tilt) rospy.Subscriber("/control/neutral_set", std_msgs.msg.Bool, self.neutral_listener) rospy.Subscriber("control/tilt_set", std_msgs.msg.Bool, self.tilt_listener)
def __init__(self, sub_topic, pub_topic): self.planner = None self.limb = None self.executed = False self.planner = PathPlanner("right_arm") self.limb = Limb("right") # self.orien_const = OrientationConstraint() # self.orien_const.link_name = "right_gripper" # self.orien_const.header.frame_id = "base" # self.orien_const.orientation.z = 0 # self.orien_const.orientation.y = 1 # self.orien_const.orientation.w = 0 # self.orien_const.absolute_x_axis_tolerance = 0.1 # self.orien_const.absolute_y_axis_tolerance = 0.1 # self.orien_const.absolute_z_axis_tolerance = 0.1 # self.orien_const.weight = 1.0 size = [1, 2, 2] obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = "base" obstacle_pose.pose.position.x = -1 obstacle_pose.pose.position.y = 0 obstacle_pose.pose.position.z = 0 obstacle_pose.pose.orientation.x = 0 obstacle_pose.pose.orientation.y = 0 obstacle_pose.pose.orientation.z = 0 obstacle_pose.pose.orientation.w = 1 self.planner.add_box_obstacle(size, "wall", obstacle_pose) size = [.75, 1, 1] obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = "base" obstacle_pose.pose.position.x = 1 obstacle_pose.pose.position.y = 0 obstacle_pose.pose.position.z = -.4 obstacle_pose.pose.orientation.x = 0 obstacle_pose.pose.orientation.y = 0 obstacle_pose.pose.orientation.z = 0 obstacle_pose.pose.orientation.w = 1 self.planner.add_box_obstacle(size, "table", obstacle_pose) self.pub = rospy.Publisher(pub_topic, Bool) self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback)
def __init__(self, map_name='campus'): self.map = MapLoader.load(path='resources/campus.json') self.pt = PositionTracker(osmap=self.map) self.pf = PositionFetcher(self.pt) self.planner = PathPlanner(self.pt, on_wp_reached=self.remove_waypoint) self.vis = Visualizer(map_name=map_name, pt=self.pt, add_wp=self.add_waypoint, osmap=self.map) self.waypoints_lock = Lock() self.waypoints_changed = True self.waypoints: Optional[List[Waypoint]] = None self.set_waypoints([])
def __init__(self, dt, target_speed, vehicle, param_dict): super(RandomBehaviorPlanner, self).__init__(vehicle, dt, param_dict) self.dt = dt self.target_speed = target_speed self.path_planner = PathPlanner(self.vehicle, {'dt': dt, 'target_speed': target_speed}) self.p_l = param_dict['p_l'] self.p_r = param_dict['p_r'] self.switcher_step = 0 # cycles through 0, 1, ..., (Tds - 1) each timestep self.chg_hazard_l = False # there is a hazard on the left self.hazard_c = False # there is a hazard ahead self.chg_hazard_r = False # there is a hazard on the right
def __init__(self): rospy.init_node('waypoint_updater') rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) # TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1) # TODO: Add other member variables you need below self.current_pose = None self.base_waypoints = None self.planner = PathPlanner(LOOKAHEAD_WPS)
def play_chords(note_pos): planner = PathPlanner("left_arm") new_note_pos = [] num_waypoints_per_note = 4 # add waypoints above the note to ensure the note is struck properly for note in note_pos: waypoint1 = Vector3() waypoint1.x = note.x waypoint1.y = note.y waypoint1.z = note.z + waypoint_offset waypoint2 = Vector3() waypoint2.x = note.x waypoint2.y = note.y waypoint2.z = note.z + waypoint_offset / 2.0 new_note_pos += [waypoint1, waypoint2, note, waypoint1] for i, pos in enumerate(new_note_pos): print(i, pos) # Loop until that position is reached while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = pos.x goal_1.pose.position.y = pos.y goal_1.pose.position.z = pos.z # #Orientation as a quaternion, facing straight down goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_1, list()) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def __init__(self): rospy.init_node('waypoint_updater') rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) # TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb) rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_lights_cb) self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1) self.speed_limit = rospy.get_param('/waypoint_loader/velocity') * 1000 / 3600. # TODO: Add other member variables you need below self.light_wp = None self.lights = None self.current_pose = None self.base_waypoints = None self.planner = PathPlanner(LOOKAHEAD_WPS) self.planner.set_speed_limit(self.speed_limit)
def __init__(self, dt, target_speed, vehicle, param_dict, CAV_agents_dict): super(CAVBehaviorPlanner, self).__init__(vehicle, dt, param_dict) self.dt = dt self.target_speed = target_speed self.path_planner = PathPlanner(self.vehicle, {'dt': dt, 'target_speed': target_speed}) self.F = param_dict['F'] self.w = param_dict['w'] self.theta_CL = param_dict['theta_CL'] self.theta_CR = param_dict['theta_CR'] self.eps = param_dict['eps'] self.chg_hazard_l = False self.hazard_c = False self.chg_hazard_r = False self.switcher_step = 0 # cycles through 0, 1, ..., (Tds - 1) each timestep self.Qv_l = 0.9 * self.target_speed self.Qv_c = 0.9 * self.target_speed self.Qv_r = 0.9 * self.target_speed self.change_buf = Queue(maxsize=self.F) while not self.change_buf.full(): self.change_buf.put(False) self.Qf = 0 self.rCL = 0 self.rCR = 0 self.neighbor_left = [] self.neighbor_current = [] self.neighbor_right = [] self.behavior = Behavior.KEEP_LANE self.closeneighbor_left = [] self.closeneighbor_current = [] self.closeneighbor_right = [] self.CAV_agents_dict = CAV_agents_dict
def generate_paths(self, cost_map): problem_valid = False # Choose a goal position num_alternatives = len(self.possible_goals) goal_position = self.possible_goals[random.randint(0, num_alternatives-1)] while not problem_valid: # Trying to generate a new problem start_position = (random.randint(0, self.height - 1), random.randint(0, self.width - 1)) # If the start happen to be within an obstacle, we discard them and # try new samples if cost_map.is_occupied(start_position[0], start_position[1]): continue if start_position == goal_position: continue try: path_planner = PathPlanner(cost_map) dijkstra_path, cost = path_planner.dijkstra(start_position, goal_position) greedy_path, cost = path_planner.greedy(start_position, goal_position) a_star_path, cost = path_planner.a_star(start_position, goal_position) problem_valid = True except AttributeError: # In case there is no valid path continue # print(start_position, goal_position) # plt.matshow(cost_map.grid) # plt.plot(start_position[1], start_position[0], 'g*', markersize=8) # plt.plot(goal_position[1], goal_position[0], 'rx', markersize=8) # title = str(start_position) + ", " + str(goal_position) # plt.title(title) # plt.show() return [dijkstra_path, greedy_path, a_star_path]
def __init__(self): #self.listener = KinectSubscriber() self.map_maker = MapMaker() #self.map_maker.set_map() #self.map_data = self.map_maker.get_lowres_map() self.path_planner = PathPlanner()
def main(): # print("Here are the marker rotations: " + str(marker_rotations)) # print("Here are the marker translations: " + str(marker_translations)) #assert len(marker_translations) == 1 #R = marker_rotations[0] #t = marker_translations[0] global top_left_x global top_left_y global top_left_z # get_top_left_AR_tag() top_left_x, top_left_y, top_left_z = 0.969, 0.143, -0.214 # assert len(marker_translations) == 1 # top_left_x, top_left_y, top_left_z = marker_translations[0] sawyer = Robot(PathPlanner("right_arm"), None, "right_gripper", 0.095) # Adding environment obstacles for the robot to avoid during path planning # add_block_obstacle(sawyer, position=[0.67452, -0.4455, 0.916557], name='right_wall', size=[5, 0, 5], orient=[1, 0, 0, 0]) # add_block_obstacle(sawyer, position=[0.460625, 0.5444, 0.916557], name='left_wall', size=[5, 0, 5], orient=[1, 0, 0, 0]) # add_block_obstacle(sawyer, position=[, , TABLE_HEIGHT - sawyer.gripper_length], name='table', size=[3.5, 3.5, 0], orient=[0, 1, 0, 0]) # add_block_obstacle(planner, [0,0,0], 'ceiling', [10, 5, 1]) ''' add_block_obstacle(sawyer, position=[0.81, -0.26, -0.1 + (0.5 * BLOCK_TOTAL_HEIGHT)], name='baseplate', size=[5 * BLOCK_SIDE_LENGTH, 6 * BLOCK_SIDE_LENGTH , 0.5 * BLOCK_TOTAL_HEIGHT]) add_block_obstacle(sawyer, position=[0.81, -0.26, -0.1 - (BLOCK_TOTAL_HEIGHT / 2)], name='table', size=[5, 5, BLOCK_TOTAL_HEIGHT / 2]) ''' add_block_obstacle(sawyer, position=[top_left_x, top_left_y, top_left_z - (BLOCK_TOTAL_HEIGHT / 2)], name='table', size=[5, 5, BLOCK_TOTAL_HEIGHT / 2]) import random # BASE_PLATE_HEIGHT = 0.5 * BLOCK_SQUARE_HEIGHT # dest_x, dest_y = block_coord_to_sawyer((5, 4)) # start_z_offset = 0.08 # for layer in range(2): # new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT + layer * BLOCK_SQUARE_HEIGHT # add_block_obstacle(sawyer, [dest_x, dest_y, new_dest_z - (0.5 * BLOCK_SQUARE_HEIGHT)], 'ehllo' + str(random.randint(1, 1000)), [BLOCK_SIDE_LENGTH, BLOCK_SIDE_LENGTH, BLOCK_SQUARE_HEIGHT]) # #TODO: Finish adding as many environment constraints as possible without compromising on path planning. # return dest_x, dest_y = block_coord_to_sawyer((5, 4)) # add_block_obstacle(sawyer, position=[dest_x, dest_y, top_left_z], name='table', size=[5, 5, 0], orient=[1, 0, 0, 0]) # add_block_obstacle(sawyer, position=[dest_x, dest_y, top_left_z - 0.006 + 0.25 * BLOCK_SQUARE_HEIGHT], name='baseplate', size=[5 * BLOCK_SIDE_LENGTH , 6 * BLOCK_SIDE_LENGTH, .5 * BLOCK_SQUARE_HEIGHT], orient=[1, 0, 0, 0]) # print('Top left:', top_left_x, top_left_y) #start_x3 = top_left_x - 0.074 - (BLOCK_SIDE_LENGTH / 2.0) #start_y3 = top_left_y + 0.0425 + (BLOCK_SIDE_LENGTH / 2.0) #print(start_x3, start_y3) start_x3, start_y3 = start_num_to_sawyer(3) print(start_x3, start_y3) # print() # for i in range(8): # print(start_num_to_sawyer(i)) #while 1 < 2: # pick_and_place_block(sawyer, start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT, dest_x, dest_y, top_left_z + (0.5 * BLOCK_SQUARE_HEIGHT), 'block_test') #pick_and_place_block(sawyer, start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT, dest_x, dest_y, top_left_z + BLOCK_SQUARE_HEIGHT, 'block_test') #pick_and_place_block(sawyer, top_left_x, top_left_y, top_left_z, dest_x, dest_y, top_left_z + BLOCK_SQUARE_HEIGHT, 'block_test') start_z_offset = 0.08 BASE_PLATE_HEIGHT = 0.5 * BLOCK_SQUARE_HEIGHT print(top_left_x, top_left_y, top_left_z) # import random # for layer in range(3): # # dest_x, dest_y = block_coord_to_sawyer((5 + layer * 2, 4)) # # new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT # new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT + layer * BLOCK_SQUARE_HEIGHT # pick_and_place_block(sawyer, # start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset, # dest_x, dest_y, new_dest_z, # 'block_' + str(random.randint(1, 1000))) import numpy as np workspace_src_path = os.path.abspath(os.path.join(planningPath, '..')) creator_cv_path = os.path.join(workspace_src_path, 'creator_cv') creator_cv_src_path = os.path.join(creator_cv_path, 'src') creator_cv_blocks_path = os.path.join(creator_cv_src_path, 'blocks') file_name = 'test.npy' blocks = np.load(creator_cv_blocks_path + '/' + file_name) for layer, layer_blocks = enumerate(blocks): for layer_block in layer_blocks: dest_x, dest_y = block_coord_to_sawyer(layer_block) # new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT + layer * BLOCK_SQUARE_HEIGHT pick_and_place_block(sawyer, start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset, dest_x, dest_y, new_dest_z, 'block_' + str(random.randint(1, 10000)))
def main(): """ Main Script """ ############# offset = -0.3 ############# # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("left_arm") ## ## Init Gripper ## rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled left = baxter_interface.Gripper('left', CHECK_VERSION) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; tf_listener = tf.TransformListener(rospy.Duration(1)) id_num = raw_input("Input Grasp ID: ") tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0)) t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time()) matrix = quaternion_matrix(q) new_matrix = matrix # new_matrix = np.eye(4) # new_matrix[:, 0] = matrix[:, 2] # new_matrix[:, 1] = -matrix[:, 1] # new_matrix[:, 2] = matrix[:, 0] t_pre = t + new_matrix[0:3, 2] * offset quaternion_from_matrix q = quaternion_from_matrix(new_matrix) print("try to move to object...........Move it!") ########## id_num = 0 ########## while not rospy.is_shutdown(): i_pregrasp = 0 while not rospy.is_shutdown(): ########################################### # if i_pregrasp > 5: # tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0)) # t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time()) # if id_num > rospy.get_param('good_grasps'): # print("Fail!") # return ############################################ i_pregrasp += 1 try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = t_pre[0] goal_1.pose.position.y = t_pre[1] goal_1.pose.position.z = t_pre[2] #Orientation as a quaternion goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_1, list()) print(plan) if not plan: raise Exception("Execution failed") ## in Rviz rospy.sleep(6) raw_input("Press <Enter> to PRE grasp pose: ") print("PRE grasp pose %d" % i_pregrasp) # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break i_grasp = 0 while not rospy.is_shutdown(): i_grasp += 1 try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = t[0] goal_2.pose.position.y = t[1] goal_2.pose.position.z = t[2] #Orientation as a quaternion goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_2, list()) if not plan: raise Exception("Execution failed") ## in Rviz rospy.sleep(6) raw_input("Press <Enter> to GRASP pose : ") print("GRASP pose %d" % i_grasp) # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break ## ## grasp ## # open_gripper(left) # close_gripper(left) i_leave = 1 while not rospy.is_shutdown(): i_leave += 1 try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position # [0.551, 0.690, -0.049] goal_3.pose.position.x = 0.551 goal_3.pose.position.y = 0.690 goal_3.pose.position.z = -0.049 #Orientation as a quaternion goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_3, list()) if not plan: raise Exception("Execution failed") rospy.sleep(6) raw_input("Press <Enter> to LEAVE pose: ") print("LEAVE pose %d" % i_leave) # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break ## ## grasp ## open_gripper(left) raw_input("Press <Enter> to continue")
def main(): global prev_msg global sec_pre plandraw = PathPlanner('right_arm') # plandraw.grip?per_op plandraw.start_position() ## BOX box_size = np.array([2.4, 2.4, 0.1]) box_pose = PoseStamped() box_pose.header.stamp = rospy.Time.now() box_pose.header.frame_id = "base" box_pose.pose.position.x = 0 box_pose.pose.position.y = 0 box_pose.pose.position.z = -0.4 box_pose.pose.orientation.x = 0.00 box_pose.pose.orientation.y = 0.00 box_pose.pose.orientation.z = 0.00 box_pose.pose.orientation.w = 1.00 plandraw.add_box_obstacle(box_size, "box1", box_pose) box_size2 = np.array([2.4, 2.4, 0.1]) box_pose2 = PoseStamped() box_pose2.header.stamp = rospy.Time.now() box_pose2.header.frame_id = "base" box_pose2.pose.position.x = 0 box_pose2.pose.position.y = 0 box_pose2.pose.position.z = 1 box_pose2.pose.orientation.x = 0.00 box_pose2.pose.orientation.y = 0.00 box_pose2.pose.orientation.z = 0.00 box_pose2.pose.orientation.w = 1.00 plandraw.add_box_obstacle(box_size2, "box2", box_pose2) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper_tip" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 def set_use_pen(pen_id, goal_1): if pen_id == 0: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -0.9848078 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = -0.1736482 if pen_id == 1: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 if pen_id == 2: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -0.9848078 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.1736482 waypoints = [] while not rospy.is_shutdown(): #raw_input("~~~~~~~~~~~~!!!!!!!!!!!!") while not rospy.is_shutdown(): try: while len(queue): print(len(queue)) cur = queue.popleft() x, y, z = cur.position_x, cur.position_y, cur.position_z x -= 0.085 # ada different coordinate z += 0.03 if cur.status_type != "edge_grad": # ti bi !!!!! luo bi !!!! if cur.status_type == "starting": print("start") # waypoints = [] goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z + 0.12 #Orientation as a quaternion # [0.766, -0.623, 0.139, -0.082] # [-0.077408, 0.99027, -0.024714, ] set_use_pen(cur.pen_type, goal_1) waypoints.append(copy.deepcopy(goal_1.pose)) goal_1.pose.position.z -= 0.12 waypoints.append(copy.deepcopy(goal_1.pose)) # plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints) # if not plandraw.execute_plan(plan): # raise Exception("Starting execution failed") # else: # queue.pop(0) elif cur.status_type == "next_point": print("next") goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion # goal_1.pose.orientation.x = 0.459962 # goal_1.pose.orientation.y = -0.7666033 # goal_1.pose.orientation.z = 0.0 # goal_1.pose.orientation.w = -0.4480562 set_use_pen(cur.pen_type, goal_1) # waypoints = [] waypoints.append(copy.deepcopy(goal_1.pose)) # plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints) # if not plandraw.execute_plan(plan): # raise Exception("Execution failed, point is ", cur) # else: # queue.pop(0) elif cur.status_type == "ending": print("ppppppp ", cur) # mmm = plandraw.get_cur_pos().pose goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z + 0.12 #Orientation as a quaternion set_use_pen(1, goal_1) # waypoints = [] waypoints.append(copy.deepcopy(goal_1.pose)) plan = plandraw.plan_to_pose(goal_1, [], waypoints) waypoints = [] # queue.pop(0) if not plandraw.execute_plan(plan): raise Exception("Execution failed") print("ti bi") # rospy.sleep(5) #raw_input("Press <Enter> to move next!!!") except Exception as e: print e else: #print("lllllllllllllllllllll") break
def main(list_of_class_objs, basket_coordinates, planner_left): """ Main Script input:first argument: a list of classX_objs. Ex: [class1_objs, class2_objs] where class1_objs contains the same kind of fruits second argument: a list of baskets coordinates """ # Make sure that you've looked at and understand path_planner.py before starting planner_left = PathPlanner("left_arm") home_coordinates = [0.544, 0.306, 0.3] home = PoseStamped() home.header.frame_id = "base" #home x,y,z position home_x = home_coordinates[0] home_y = home_coordinates[1] home_z = home_coordinates[2] home.pose.position.x = home_x home.pose.position.y = home_y home.pose.position.z = home_z #Orientation as a quaternion home.pose.orientation.x = 1.0 home.pose.orientation.y = 0.0 home.pose.orientation.z = 0.0 home.pose.orientation.w = 0.0 intermediate_obstacle = PoseStamped() intermediate_obstacle.header.frame_id = "base" intermediate_obstacle.pose.position.x = 0 intermediate_obstacle.pose.position.y = 0 intermediate_obstacle.pose.position.z = 0.764 intermediate_obstacle.pose.orientation.x = 1 intermediate_obstacle.pose.orientation.y = 0 intermediate_obstacle.pose.orientation.z = 0 intermediate_obstacle.pose.orientation.w = 0 intermediate_size = [1, 1, 0.2] left_gripper = robot_gripper.Gripper('left') print('Calibrating...') left_gripper.calibrate() while not rospy.is_shutdown(): try: #GO HOME plan = planner_left.plan_to_pose(home, list()) raw_input("Press <Enter> to move the left arm to home position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break for i, classi_objs in enumerate(list_of_class_objs): #x, y,z, orientation of class(basket) print("processing class: " + str(i)) classi_position = basket_coordinates[i] classi = PoseStamped() classi.header.frame_id = "base" classi_x = classi_position[0] classi_y = classi_position[1] classi_z = classi_position[2] classi.pose.position.x = classi_x classi.pose.position.y = classi_y classi.pose.position.z = classi_z +.1 classi.pose.orientation.x = 1.0 classi.pose.orientation.y = 0.0 classi.pose.orientation.z = 0.0 classi.pose.orientation.w = 0.0 for fruit in classi_objs: gripper_yaw = fruit[3] fruit_x = fruit[0] fruit_y = fruit[1] fruit_z = fruit[2] gripper_orientation = euler_to_quaternion(gripper_yaw) orien_const = OrientationConstraint() orien_const.link_name = "left_gripper"; orien_const.header.frame_id = "base"; gripper_orientation_x = gripper_orientation[0] gripper_orientation_y = gripper_orientation[1] gripper_orientation_z = gripper_orientation[2] gripper_orientation_w = gripper_orientation[3] orien_const.orientation.x = gripper_orientation_x orien_const.orientation.y = gripper_orientation_y orien_const.orientation.z = gripper_orientation_z orien_const.orientation.w = gripper_orientation_w orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 print('Opening...') left_gripper.open() rospy.sleep(1.0) while not rospy.is_shutdown(): try: planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose) #intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper intermidiate_to_fruit = PoseStamped() intermidiate_to_fruit.header.frame_id = "base" #x,y,z position intermidiate_to_fruit.pose.position.x = fruit_x intermidiate_to_fruit.pose.position.y = fruit_y intermidiate_to_fruit.pose.position.z = home_z - .1 print("Trying to reach intermeidiate_to_fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z)) intermidiate_to_fruit.pose.orientation.x = gripper_orientation_x intermidiate_to_fruit.pose.orientation.y = gripper_orientation_y intermidiate_to_fruit.pose.orientation.z = gripper_orientation_z intermidiate_to_fruit.pose.orientation.w = gripper_orientation_w plan = planner_left.plan_to_pose(intermidiate_to_fruit, list()) raw_input("Press <Enter> to move the left arm to intermidiate_to_fruit position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") planner_left.remove_obstacle("intermediate") except Exception as e: print e else: break while not rospy.is_shutdown(): try: #go down to the actual height of the fruit and close gripper fruitobs = generate_obstacle(fruit_x, fruit_y) fruit = PoseStamped() fruit.header.frame_id = "base" #x,y,z position fruit.pose.position.x = fruit_x fruit.pose.position.y = fruit_y fruit.pose.position.z = fruit_z print("Trying to reach fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z)) #Orientation as a quaternion fruit.pose.orientation.x = gripper_orientation_x fruit.pose.orientation.y = gripper_orientation_y fruit.pose.orientation.z = gripper_orientation_z fruit.pose.orientation.w = gripper_orientation_w plan = planner_left.plan_to_pose(fruit, [orien_const]) raw_input("Press <Enter> to move the left arm to fruit position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") fruitobs() except Exception as e: print e else: break #close the gripper print('Closing...') left_gripper.close() rospy.sleep(1.0) while not rospy.is_shutdown(): try: fruitobs = generate_obstacle(fruit_x, fruit_y) #intermidiate_to_basket stage1: Lift up to a height higher than the height of the basket firt_intermidiate_to_class_i = PoseStamped() firt_intermidiate_to_class_i.header.frame_id = "base" #x, y, and z position firt_intermidiate_to_class_i.pose.position.x = fruit_x firt_intermidiate_to_class_i.pose.position.y = fruit_y firt_intermidiate_to_class_i.pose.position.z = classi_z + 0.25 print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2)) #Orientation as a quaternion firt_intermidiate_to_class_i.pose.orientation.x = 1.0 firt_intermidiate_to_class_i.pose.orientation.y = 0.0 firt_intermidiate_to_class_i.pose.orientation.z = 0.0 firt_intermidiate_to_class_i.pose.orientation.w = 0.0 plan = planner_left.plan_to_pose(firt_intermidiate_to_class_i, list()) raw_input("Press <Enter> to move the left arm to first_intermidiate_to_class_" + str(i) + "position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") fruitobs() except Exception as e: print e else: break while not rospy.is_shutdown(): try: planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose) #intermidiate_to_basket stage2: Move to the top of the basket intermidiate_to_class_i = PoseStamped() intermidiate_to_class_i.header.frame_id = "base" #x, y, and z position intermidiate_to_class_i.pose.position.x = classi_x intermidiate_to_class_i.pose.position.y = classi_y intermidiate_to_class_i.pose.position.z = classi_z + 0.2 print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2)) #Orientation as a quaternion intermidiate_to_class_i.pose.orientation.x = 1.0 intermidiate_to_class_i.pose.orientation.y = 0.0 intermidiate_to_class_i.pose.orientation.z = 0.0 intermidiate_to_class_i.pose.orientation.w = 0.0 plan = planner_left.plan_to_pose(intermidiate_to_class_i, list()) raw_input("Press <Enter> to move the left arm to second_intermidiate_to_class_" + str(i) + "position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") planner_left.remove_obstacle("intermediate") except Exception as e: print e else: break while not rospy.is_shutdown(): try: #basket stage: put the fruit in the basket classi_obs = generate_obstacle(classi_x, class_y) plan = planner_left.plan_to_pose(classi, list()) raw_input("Press <Enter> to move the left arm to sclass_" + str(i) + "position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") classi_obs() except Exception as e: print e else: break #Open the gripper print('Opening...') left_gripper.open() rospy.sleep(1.0) while not rospy.is_shutdown(): try: #intermidiate to home stage: lifting up to the home position height intermidiate_to_home_1 = PoseStamped() intermidiate_to_home_1.header.frame_id = "base" #x, y, and z position intermidiate_to_home_1.pose.position.x = classi_x intermidiate_to_home_1.pose.position.y = classi_y intermidiate_to_home_1.pose.position.z = home_z #Orientation as a quaternion intermidiate_to_home_1.pose.orientation.x = 1.0 intermidiate_to_home_1.pose.orientation.y = 0.0 intermidiate_to_home_1.pose.orientation.z = 0.0 intermidiate_to_home_1.pose.orientation.w = 0.0 plan = planner_left.plan_to_pose(intermidiate_to_home_1, list()) raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break planner_left.shutdown()
def main(): """ Main Script """ use_orien_const = False input = raw_input("use orientation constraint? y/n\n") if input == 'y' or input == 'yes': use_orien_const = True print("using orientation constraint") elif input == 'n' or input == 'no': use_orien_const = False print("not using orientation constraint") else: print("input error, not using orientation constraint as default") add_box = False input = raw_input("add_box? y/n\n") if input == 'y' or input == 'yes': add_box = True print("add_box") elif input == 'n' or input == 'no': add_box = False print("not add_box") else: print("input error, not add_box as default") open_loop_contro = False input = raw_input("using open loop controller? y/n\n") if input == 'y' or input == 'yes': open_loop_contro = True print("using open loop controller") elif input == 'n' or input == 'no': open_loop_contro = False print("not using open loop controller") else: print("input error, not using open loop controller as default") # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned # joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'] my_controller = Controller(Kp, Kd, Ki, Kw, Limb("right")) ## ## Add the obstacle to the planning scene here ## if add_box: name = "obstacle_1" size = np.array([0.4, 1.2, 0.1]) pose = PoseStamped() pose.header.frame_id = "base" #x, y, and z position pose.pose.position.x = 0.5 pose.pose.position.y = 0.0 pose.pose.position.z = 0.0 #Orientation as a quaternion pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 planner.add_box_obstacle(size, name, pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.4 goal_1.pose.position.y = -0.3 goal_1.pose.position.z = 0.2 #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 if not use_orien_const: plan = planner.plan_to_pose(goal_1, list()) else: plan = planner.plan_to_pose(goal_1, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose 1: ") if open_loop_contro: if not my_controller.execute_path(plan): raise Exception("Execution failed") else: if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.6 goal_2.pose.position.y = -0.3 goal_2.pose.position.z = 0.0 #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 # plan = planner.plan_to_pose(goal_2, list()) if not use_orien_const: plan = planner.plan_to_pose(goal_2, list()) else: plan = planner.plan_to_pose(goal_2, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose 2: ") if open_loop_contro: if not my_controller.execute_path(plan): raise Exception("Execution failed") else: if not planner.execute_plan(plan): raise Exception("Execution failed") # if not my_controller.execute_path(plan): # raise Exception("Execution failed") # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.6 goal_3.pose.position.y = -0.1 goal_3.pose.position.z = 0.1 #Orientation as a quaternion goal_3.pose.orientation.x = 0.0 goal_3.pose.orientation.y = -1.0 goal_3.pose.orientation.z = 0.0 goal_3.pose.orientation.w = 0.0 # plan = planner.plan_to_pose(goal_3, list()) if not use_orien_const: plan = planner.plan_to_pose(goal_3, list()) else: plan = planner.plan_to_pose(goal_3, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose 3: ") if open_loop_contro: if not my_controller.execute_path(plan): raise Exception("Execution failed") else: if not planner.execute_plan(plan): raise Exception("Execution failed") # if not my_controller.execute_path(plan): # raise Exception("Execution failed") # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") # K values for Part 5 Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3 ]) # Borrowed from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Borrowed from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned # Initialize the controller for Part 5 # controller = Controller( . . . ) #-----------------------------------------------------# ## Add any obstacles to the planning scene here #-----------------------------------------------------# size = np.array([0.4, 1.2, 0.1]) pose = PoseStamped() pose.header.frame_id = "base" pose.pose.position.x = 0.5 pose.pose.position.y = 0.0 pose.pose.position.z = 0.0 pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.z = 1.0 # planner.add_box_obstacle(size, "box", pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; def move_to_goal(x, y, z, orien_const=[], or_x=0.0, or_y=-1.0, or_z=0.0, or_w=0.0): while not rospy.is_shutdown(): try: goal = PoseStamped() goal.header.frame_id = "base" #x, y, and z position goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = z #Orientation as a quaternion goal.pose.orientation.x = or_x goal.pose.orientation.y = or_y goal.pose.orientation.z = or_z goal.pose.orientation.w = or_w plan = planner.plan_to_pose(goal, orien_const) # raw_input("Press <Enter> to move the right arm to goal pose: ") rospy.sleep(1) # Might have to edit this for part 5 if not planner.execute_plan(plan): raise Exception("Execution failed") else: break except Exception as e: print e traceback.print_exc() # else: # break while not rospy.is_shutdown(): right_gripper = robot_gripper.Gripper('right') right_gripper.set_moving_force(80.0) right_gripper.set_holding_force(80.0) # right_gripper.calibrate() # Set your goal positions here print("starting") # move_to_goal(0.85, -0.3001, 0.1) # rospy.sleep(1.) move_to_goal(0.85, -0.2995, 0.1) print("opening") right_gripper.open() rospy.sleep(1.) print("executing") move_to_goal(0.85, -0.2995, -0.030) #-0.41 print("closings") # right_gripper.close() print("MISSED: ", right_gripper.missed()) print("FORCEEE: ", right_gripper.force()) print("Done") # move_to_goal(0.4225 + 0.1, -0.1265, 0.7725 - 0.92) # right_gripper.close() # move_to_goal(0.4225 + 0.1 + 0.05, -0.1265, 0.7725 - 0.92) # right_gripper.open() break
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting plannerR = PathPlanner("right_arm") plannerL = PathPlanner("left_arm") right_gripper = robot_gripper.Gripper('right') left_gripper = robot_gripper.Gripper('left') # Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students # Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students # Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned # Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned # right_arm = Limb("right") # controller = Controller(Kp, Kd, Ki, Kw, right_arm) ## ## Add the obstacle to the planning scene here ## obstacle1 = PoseStamped() obstacle1.header.frame_id = "base" #x, y, and z position obstacle1.pose.position.x = 0.4 obstacle1.pose.position.y = 0 obstacle1.pose.position.z = -0.29 #Orientation as a quaternion obstacle1.pose.orientation.x = 0.0 obstacle1.pose.orientation.y = 0.0 obstacle1.pose.orientation.z = 0.0 obstacle1.pose.orientation.w = 1 plannerR.add_box_obstacle(np.array([1.2, 1.2, .005]), "tableBox", obstacle1) # obstacle2 = PoseStamped() # obstacle2.header.frame_id = "wall" # #x, y, and z position # obstacle2.pose.position.x = 0.466 # obstacle2.pose.position.y = -0.670 # obstacle2.pose.position.z = -0.005 # #Orientation as a quaternion # obstacle2.pose.orientation.x = 0.694 # obstacle2.pose.orientation.y = -0.669 # obstacle2.pose.orientation.z = 0.251 # obstacle2.pose.orientation.w = -0.092 # planner.add_box_obstacle(np.array([0.01, 2, 2]), "wall", obstacle2) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.440 goal_3.pose.position.y = -0.012 goal_3.pose.position.z = 0.549 #Orientation as a quaternion goal_3.pose.orientation.x = -0.314 goal_3.pose.orientation.y = -0.389 goal_3.pose.orientation.z = 0.432 goal_3.pose.orientation.w = 0.749 #plan = plannerR.plan_to_pose(goal_3, list()) raw_input( "Press <Enter> to move the right arm to goal pose 3: ") if not plannerR.execute_plan( plannerR.plan_to_pose(goal_3, list())): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.448 goal_2.pose.position.y = -0.047 goal_2.pose.position.z = -0.245 #Orientation as a quaternion goal_2.pose.orientation.x = 0 goal_2.pose.orientation.y = 1 goal_2.pose.orientation.z = 0 goal_2.pose.orientation.w = 0 #plan = plannerR.plan_to_pose(goal_2, list()) raw_input( "Press <Enter> to move the right arm to goal pose 2: ") if not plannerR.execute_plan( plannerR.plan_to_pose(goal_2, list())): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.448 goal_1.pose.position.y = -0.047 goal_1.pose.position.z = -0.245 #Orientation as a quaternion goal_1.pose.orientation.x = 1 goal_1.pose.orientation.y = 0 goal_1.pose.orientation.z = 0 goal_1.pose.orientation.w = 0 # Might have to edit this . . . #plan = plannerR.plan_to_pose(goal_1, list()) # put orien_const here raw_input( "Press <Enter> to move the right arm to goal pose 1: ") if not plannerR.execute_plan( plannerR.plan_to_pose(goal_1, list())): raise Exception("Execution failed") except Exception as e: print e else: break
raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break planner_left.shutdown() if __name__ == '__main__': rospy.init_node('moveit_node') planner_left = PathPlanner("left_arm") table_pose = PoseStamped() table_pose.header.frame_id = "base" table_pose.pose.position.x = 0 table_pose.pose.position.y = 0 table_pose.pose.position.z = 0 table_pose.pose.orientation.x = 1 table_pose.pose.orientation.y = 0 table_pose.pose.orientation.z = 0 table_pose.pose.orientation.w = 0 table_size = [1, 1, 0.764]