def initialize_environment(): # Assume rosnode is already is initialized; scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) return scene
def initialize_environment(): # Initialize environment rospy.init_node('Baxter_Env') robot = RobotCommander() scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) return scene, robot
def main(): rospy.init_node('Baxter_Env') robot = RobotCommander() scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) # centre table p1 = PoseStamped() p1.header.frame_id = robot.get_planning_frame() p1.pose.position.x = 1. # position of the table in the world frame p1.pose.position.y = 0. p1.pose.position.z = 0. # left table (from baxter's perspective) p1_l = PoseStamped() p1_l.header.frame_id = robot.get_planning_frame() p1_l.pose.position.x = 0.5 # position of the table in the world frame p1_l.pose.position.y = 1. p1_l.pose.position.z = 0. p1_l.pose.orientation.x = 0.707 p1_l.pose.orientation.y = 0.707 # right table (from baxter's perspective) p1_r = PoseStamped() p1_r.header.frame_id = robot.get_planning_frame() p1_r.pose.position.x = 0.5 # position of the table in the world frame p1_r.pose.position.y = -1. p1_r.pose.position.z = 0. p1_r.pose.orientation.x = 0.707 p1_r.pose.orientation.y = 0.707 # open back shelf p2 = PoseStamped() p2.header.frame_id = robot.get_planning_frame() p2.pose.position.x = 1.2 # position of the table in the world frame p2.pose.position.y = 0.0 p2.pose.position.z = 0.75 p2.pose.orientation.x = 0.5 p2.pose.orientation.y = -0.5 p2.pose.orientation.z = -0.5 p2.pose.orientation.w = 0.5 pw = PoseStamped() pw.header.frame_id = robot.get_planning_frame() # add an object to be grasped p_ob1 = PoseStamped() p_ob1.header.frame_id = robot.get_planning_frame() p_ob1.pose.position.x = .92 p_ob1.pose.position.y = 0.3 p_ob1.pose.position.z = 0.89 # the ole duck p_ob2 = PoseStamped() p_ob2.header.frame_id = robot.get_planning_frame() p_ob2.pose.position.x = 0.87 p_ob2.pose.position.y = -0.4 p_ob2.pose.position.z = 0.24 # clean environment scene.remove_world_object("table_center") scene.remove_world_object("table_side_left") scene.remove_world_object("table_side_right") scene.remove_world_object("shelf") scene.remove_world_object("wall") scene.remove_world_object("part") scene.remove_world_object("duck") rospy.sleep(1) scene.add_box("table_center", p1, size=(.5, 1.5, 0.4)) # dimensions of the table scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4)) scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4)) scene.add_mesh( "shelf", p2, "/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/bookshelf_light.stl", size=(.025, .01, .01)) scene.add_plane("wall", pw, normal=(0, 1, 0)) part_size = (0.07, 0.05, 0.12) scene.add_box("part", p_ob1, size=part_size) scene.add_mesh( "duck", p_ob2, "/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/duck.stl", size=(.001, .001, .001)) rospy.sleep(1) print(scene.get_known_object_names()) ## ---> SET COLOURS table_color = color_norm([105, 105, 105]) # normalize colors to range [0, 1] shelf_color = color_norm([139, 69, 19]) duck_color = color_norm([255, 255, 0]) _colors = {} setColor('table_center', _colors, table_color, 1) setColor('table_side_left', _colors, table_color, 1) setColor('table_side_right', _colors, table_color, 1) setColor('shelf', _colors, shelf_color, 1) setColor('duck', _colors, duck_color, 1) sendColors(_colors, scene)
def main(): rospy.init_node(sys.argv[1]) # sometimes takes a few tries to connect to robot arm limb_init = False while not limb_init: try: limb = baxter_interface.Limb('right') limb_init = True except OSError: limb_init = False neutral_start = limb.joint_angles() min_goal_cost_threshold = 2.0 # Set up planning scene and Move Group objects scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher( 'planning_scene', PlanningScene, queue_size=0) robot = RobotCommander() group = MoveGroupCommander("right_arm") sv = StateValidity() set_environment(robot, scene) # Setup tables (geometry and location defined in moveit_functions.py, set_environment function) # set_environment(robot, scene) # Additional Move group setup # group.set_goal_joint_tolerance(0.001) # group.set_max_velocity_scaling_factor(0.7) # group.set_max_acceleration_scaling_factor(0.1) max_time = 300 group.set_planning_time(max_time) # Dictionary to save path data, and filename to save the data to in the end pathsDict = {} pathsFile = "data/path_data_example" # load data from environment files for obstacle locations and collision free goal poses with open("env/trainEnvironments.pkl", "rb") as env_f: envDict = pickle.load(env_f) with open("env/trainEnvironments_testGoals.pkl", "rb") as goal_f: goalDict = pickle.load(goal_f) # Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt sceneModifier = PlanningSceneModifier(envDict['obsData']) sceneModifier.setup_scene(scene, robot, group) robot_state = robot.get_current_state() rs_man = RobotState() rs_man.joint_state.name = robot_state.joint_state.name # Here we go test_envs = envDict['poses'].keys() #slice this if you want only some environments done = False iter_num = 0 print("Testing envs: ") print(test_envs) while(not rospy.is_shutdown() and not done): for i_env, env_name in enumerate(test_envs): print("env iteration number: " + str(i_env)) print("env name: " + str(env_name)) sceneModifier.delete_obstacles() new_pose = envDict['poses'][env_name] sceneModifier.permute_obstacles(new_pose) print("Loaded new pose and permuted obstacles\n") pathsDict[env_name] = {} pathsDict[env_name]['paths'] = [] pathsDict[env_name]['costs'] = [] pathsDict[env_name]['times'] = [] pathsDict[env_name]['total'] = 0 pathsDict[env_name]['feasible'] = 0 collision_free_goals = goalDict[env_name]['Joints'] total_paths = 0 feasible_paths = 0 i_path = 0 while (total_paths < 30): #run until either desired number of total or feasible paths has been found #do planning and save data # some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table # by only taking goals which have a straight path cost above a threshold valid_goal = False while not valid_goal: goal = collision_free_goals[np.random.randint(0, len(collision_free_goals))] optimal_path = [neutral_start.values(), goal.values()] optimal_cost = compute_cost(optimal_path) if optimal_cost > min_goal_cost_threshold: valid_goal = True print("FP: " + str(feasible_paths)) print("TP: " + str(total_paths)) total_paths += 1 i_path += 1 # Uncomment below if using a start state different than the robot current state # filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff # filler_robot_state[10:17] = moveit_scrambler(start.values()) # rs_man.joint_state.position = tuple(filler_robot_state) # group.set_start_state(rs_man) # set start group.set_start_state_to_current_state() group.clear_pose_targets() try: group.set_joint_value_target(moveit_scrambler(goal.values())) # set target except MoveItCommanderException as e: print(e) continue start_t = time.time() plan = group.plan() pos = [plan.joint_trajectory.points[i].positions for i in range(len(plan.joint_trajectory.points))] if pos != []: pos = np.asarray(pos) cost = compute_cost(pos) t = time.time() - start_t print("Time: " + str(t)) print("Cost: " + str(cost)) # Uncomment below if using max time as criteria for failure if (t > (max_time*0.99)): print("Reached max time...") continue feasible_paths += 1 pathsDict[env_name]['paths'].append(pos) pathsDict[env_name]['costs'].append(cost) pathsDict[env_name]['times'].append(t) pathsDict[env_name]['feasible'] = feasible_paths pathsDict[env_name]['total'] = total_paths # Uncomment below if you want to overwrite data on each new feasible path with open (pathsFile + "_" + env_name + ".pkl", "wb") as path_f: pickle.dump(pathsDict[env_name], path_f) print("\n") sceneModifier.delete_obstacles() iter_num += 1 print("Env: " + str(env_name)) print("Feasible Paths: " + str(feasible_paths)) print("Total Paths: " + str(total_paths)) print("\n") pathsDict[env_name]['total'] = total_paths pathsDict[env_name]['feasible'] = feasible_paths with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f: pickle.dump(pathsDict[env_name], path_f) print("Done iterating, saving all data and exiting...\n\n\n") with open(pathsFile + ".pkl", "wb") as path_f: pickle.dump(pathsDict, path_f) done = True