def parallel_connect_worker(batches, interp_fn, distance_fn, sim_context_cls, sim_config): sim_context = sim_context_cls(sim_config, setup=False) sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False}) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() _ = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() # Disabled collisions during planning with certain exclusions in place. # collision_exclusions = sim_context.get_collision_exclusions() collision_exclusions = {} with DisabledCollisionsContext(sim, **collision_exclusions): connections = [] for batch in batches: q_sample = batch[0] neighbors = batch[1] for q_near in neighbors: local_path = interp_fn(np.array(q_near), np.array(q_sample), steps=10) valid = subdivision_evaluate(svc.validate, local_path) if valid: connections.append( [q_near, q_sample, distance_fn(local_path)]) return connections
def _compute_probabilities(self, K_noisy_trajectories): S = np.zeros((self.K, self.N)) P = np.zeros((self.K, self.N)) exp_values = np.zeros((self.K, self.N)) with DisabledCollisionsContext(self.sim): for k in range(self.K): for i in range(self.N): S[k][i] = self._state_cost(K_noisy_trajectories[k][i]) + \ self._control_cost(K_noisy_trajectories[k])/self.N # S[k][i] = self._control_cost(K_noisy_trajectories[k]) / self.N S_max_for_each_i = np.max(S, axis=0) S_min_for_each_i = np.min(S, axis=0) for k in range(self.K): for i in range(self.N): exp_values[k][i] = np.exp( (self.h * -1 * (S[k][i] - S_min_for_each_i[i])) / (S_max_for_each_i[i] - S_min_for_each_i[i])) sum_exp_values_for_each_i = np.sum(exp_values, axis=0) for k in range(self.K): for i in range(self.N): P[k][i] = exp_values[k][i] / sum_exp_values_for_each_i[i] return P
def parallel_projection_worker(num_samples, tsr, sim_context_cls, sim_config): sim_context = sim_context_cls(sim_config, setup=False) sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False}) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() scs = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): while len(valid_samples) < num_samples: sample = scs.sample() if svc.validate(sample): q_constrained = project_config(sawyer_robot, np.array( sample), np.array(sample), tsr, .1, .01) normalized_q_constrained = [] if q_constrained is not None: for value in q_constrained: normalized_q_constrained.append( wrap_to_interval(value)) else: continue if svc.validate(normalized_q_constrained): valid_samples.append(normalized_q_constrained) return valid_samples
def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() logger = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) with DisabledCollisionsContext(sim, [], []): ####### # LazyPRM # ####### # Use parametric linear interpolation with 10 steps between points. interp = partial(parametric_lerp, steps=10) # See params for PRM specific parameters prm = LazyPRM(state_space, svc, interp, params={ 'n_samples': 4000, 'k': 8, 'ball_radius': 2.5 }) logger.info("Planning....") plan = prm.plan( np.array([0, 0, 0, 0, 0, 0, 0]), np.array([ 1.5262755737449423, -0.1698540226273928, 2.7788151824762055, 2.4546623466066135, 0.7146948867821279, 2.7671787952787184, 2.606128412644311 ])) # get_path() reuses the interp function to get the path between vertices of a successful plan path = prm.get_path(plan) if len(path) == 0: logger.info("Planning failed....") sys.exit(1) logger.info("Plan found....") # splinging uses numpy so needs to be converted path = [np.array(p) for p in path] # Create a MinJerk spline trajectory using JointTrajectoryCurve and execute jtc = JointTrajectoryCurve() traj = jtc.generate_trajectory(path, move_time=2) sawyer_robot.execute_trajectory(traj) key = input("Press any key to excute plan.") try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def _compute_trajectory_cost(self): cost = 0 with DisabledCollisionsContext(self.sim): for i in range(self.N): cost += self._state_cost(self.trajectory[i]) state_cost = cost cost += self._control_cost(self.trajectory) if self.debug: print("State Cost = ", state_cost) print("Control Cost = ", cost - state_cost) print("Total Cost = ", cost) print("") return cost
def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() _ = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) ############ # PLANNING # ############ valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): while True: sample = state_space.sample() if svc.validate(sample): valid_samples.append(sample) if len(valid_samples) >= 1: break # Generate local plan between two points and execute local plan. steps = 100 move_time = 5 start_pos = [0] * 7 print(np.array(sawyer_robot.get_current_joint_states()[0:7])) print(np.array(valid_samples[0])) qt, qdt, qddt = interpolate_5poly(np.array(start_pos[0:7]), np.array(valid_samples[0]), steps) traj = list( zip([move_time * n / steps for n in range(0, steps)], [list(q) for q in qt])) print(traj) sawyer_robot.execute_trajectory(traj) # Loop until someone shuts us down try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def parallel_projection_worker(num_samples, sim_context_cls, sim_config): sim_context = sim_context_cls(sim_config) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() scs = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] valid_samples = [] # Utilizes RPY convention T0_w = xyzrpy2trans([.7, 0, 0, 0, 0, 0], degrees=False) # Utilizes RPY convention Tw_e = xyzrpy2trans([-.2, 0, 1.0, np.pi / 2, np.pi, 0], degrees=False) # Utilizes RPY convention Bw = bounds_matrix( [(0, 100), (-100, 100), (-100, 100) ], # allow some tolerance in the z and y and only positve in x [(-.07, .07), (-.07, .07), (-.07, .07) ]) # any rotation about z, with limited rotation about x, and y. tsr = TSR(T0_w=T0_w, Tw_e=Tw_e, Bw=Bw, manipindex=0, bodyandlink=16) # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): while len(valid_samples) < num_samples: sample = scs.sample() if svc.validate(sample): q_constrained = project_config(sawyer_robot, np.array(sample), np.array(sample), tsr, .1, .01) normalized_q_constrained = [] if q_constrained is not None: for value in q_constrained: normalized_q_constrained.append( wrap_to_interval(value)) else: continue if svc.validate(normalized_q_constrained): valid_samples.append(normalized_q_constrained) return valid_samples
def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() _ = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() _ = sim_context.get_sim_objects(['Ground'])[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): while True: sample = state_space.sample() if svc.validate(sample): valid_samples.append(sample) if len(valid_samples) >= 1: break # Generate local plan between two points and execute local plan. start_pos = [0] * 7 sawyer_robot.move_to_joint_pos(start_pos) time.sleep(.1) # This interpolate_5poly will be replaced with the sequence of points generated by a planner. qt, _, _ = interpolate_5poly(np.array(start_pos[0:7]), np.array(valid_samples[0]), 50) jtc = JointTrajectoryCurve() traj = jtc.generate_trajectory(qt, move_time=2) sawyer_robot.execute_trajectory(traj) # Loop until someone shuts us down try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def main(): config = {} config["sim_objects"] = [{ "object_name": "Ground", "model_file_or_sim_id": "plane.urdf", "position": [0, 0, 0] }, { "object_name": "Table", "model_file_or_sim_id": ASSETS_PATH + 'table.sdf', "position": [0.0, 0, 0], "orientation": [0, 0, 1.5708] }] sim_context = SawyerSimContext(config) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() scs = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() _ = sawyer_robot.get_simulator_id() table_obj = sim_context.get_sim_objects(names="Table")[0] sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0]) time.sleep(1) n_samples = 1000 valid_samples = [] starttime = timeit.default_timer() # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, [], []): print("Sampling start time is :", starttime) for i in range(0, n_samples): sample = scs.sample() if svc.validate(sample): print(sample) valid_samples.append(sample) print("The time difference is :", timeit.default_timer() - starttime) print("{} valid of {}".format(len(valid_samples), n_samples)) p.disconnect()
def parallel_sample_worker(num_samples, sim_context_cls, sim_config): sim_context = sim_context_cls(sim_config, setup=False) sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False}) sim = sim_context.get_sim_instance() _ = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() # Disabled collisions during planning with certain exclusions in place. # collision_exclusions = sim_context.get_collision_exclusions() collision_exclusions = {} with DisabledCollisionsContext(sim, **collision_exclusions): valid_samples = [] count = 0 while count < num_samples: q_rand = np.array(state_space.sample()) if svc.validate(q_rand): valid_samples.append(q_rand) count += 1 return valid_samples
final_path = [] initial_start_point = planning_G.nodes[list(planning_G.nodes)[0]]['point'] for edge in planning_G.edges(): valid_samples = [] start_point = planning_G.nodes[edge[0]]['point'] end_point = planning_G.nodes[edge[1]]['point'] print(start_point, end_point) state_space = planning_G.get_edge_data(*edge)['planning_space'] #################################### # SIMULATION AND PLANNING CONTEXTS # #################################### with DisabledCollisionsContext(sim, [], []): ####### # PRM # ####### # Use parametric linear interpolation with 10 steps between points. interp = partial(parametric_lerp, steps=10) # See params for PRM specific parameters prm = PRM(state_space, svc, interp_fn, params={ 'n_samples': 250, 'k': 6, 'ball_radius': .75 }) logger.info("Planning....")
def main(): ################################ # Environment Checks and Flags # ################################ if os.environ.get('ROS_DISTRO'): rospy.init_node("CAIRO_Sawyer_Simulator") use_ros = True else: use_ros = False ######################################################## # Create the Simulator and pass it a Logger (optional) # ######################################################## logger = Logger() sim = Simulator(logger=logger, use_ros=use_ros, use_gui=True, use_real_time=True) # Initialize the Simulator ##################################### # Create a Robot, or two, or three. # ##################################### sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1) ############################################# # Create sim environment objects and assets # ############################################# ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0]) sawyer_id = sawyer_robot.get_simulator_id() sawyer_robot.move_to_joint_pos([ 0.006427734375, -0.4784267578125, -2.6830537109375, -1.5901376953125, 0.1734560546875, 1.1468447265625, 1.310236328125 ]) time.sleep(3) # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # PLANNING # ############ # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ######################### # STATE SPACE SELECTION # ######################### # This inherently uses UniformSampler but a different sampling class could be injected. state_space = SawyerConfigurationSpace() ############################## # STATE VALIDITY FORMULATION # ############################## # Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision. excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx), (get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)] link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs) self_collision_fn = partial(self_collision_test, robot=sawyer_robot, link_pairs=link_pairs) # Create constraint checks def constraint_test(q): upright_orientation = [ 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838 ] axis = 'z' threshold = 15 world_pose, _ = sawyer_robot.solve_forward_kinematics(q) return orientation(upright_orientation, world_pose[1], threshold, axis) # In this case, we only have a self_col_fn. svc = StateValidityChecker(self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test]) ####### # PRM # ####### # Use parametric linear interpolation with 5 steps between points. interp = partial(parametric_lerp, steps=5) # See params for PRM specific parameters prm = PRM(state_space, svc, interp, params={ 'n_samples': 1000, 'k': 10, 'ball_radius': 3.0 }) logger.info("Planning....") plan = prm.plan( np.array([ 0.006427734375, -0.4784267578125, -2.6830537109375, -1.5901376953125, 0.1734560546875, 1.1468447265625, 1.310236328125 ]), np.array([ -0.9232412109375, 0.2353603515625, -2.51373828125, -0.6898984375, 0.33058203125, 1.0955361328125, 1.14510546875 ])) logger.info("Plan found....") print(len(plan)) print(plan) # get_path() reuses the interp function to get the path between vertices of a successful plan path = prm.get_path(plan) if len(path) == 0: logger.info("Planning failed....") sys.exit(1) ########## # SPLINE # ########## # splinging uses numpy so needs to be converted path = [np.array(p) for p in path] # Create a MinJerk spline trajectory using JointTrajectoryCurve and execute jtc = JointTrajectoryCurve() traj = jtc.generate_trajectory(path, move_time=5) print("Executing splined trajectory...") sawyer_robot.execute_trajectory(traj) try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)
def parallel_sample_worker(num_samples): ######################################################## # Create the Simulator and pass it a Logger (optional) # ######################################################## logger = Logger() if not Simulator.is_instantiated(): logger.warn("Simulator not instantiated, doing so now...") sim = Simulator(logger=logger, use_ros=False, use_gui=False, use_real_time=True) # Initialize the Simulator else: sim = Simulator.get_instance() ##################################### # Create a Robot, or two, or three. # ##################################### sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1) ############################################# # Create sim environment objects and assets # ############################################# ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0]) sawyer_id = sawyer_robot.get_simulator_id() # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # SAMPLING # ############ valid_samples = [] # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ######################### # STATE SPACE SELECTION # ######################### # This inherently uses UniformSampler but a different sampling class could be injected. state_space = SawyerConfigurationSpace() ############################## # STATE VALIDITY FORMULATION # ############################## # Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision. excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx), (get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)] link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs) self_collision_fn = partial( self_collision_test, robot=sawyer_robot, link_pairs=link_pairs) # Create constraint checks # def constraint_test(q): # upright_orientation = [ # 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838] # axis = 'z' # threshold = 15 # world_pose, _ = sawyer_robot.solve_forward_kinematics(q) # return orientation(upright_orientation, world_pose[1], threshold, axis) # In this case, we only have a self_col_fn. svc = StateValidityChecker( self_col_func=self_collision_fn, col_func=None, validity_funcs=None) # svc = StateValidityChecker( # self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test]) count = 0 while count < num_samples: q_rand = np.array(state_space.sample()) if svc.validate(q_rand): valid_samples.append(q_rand) count += 1 return valid_samples
############################################# # Create sim environment objects and assets # ############################################# ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0]) # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # PLANNING # ############ # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ######################### # STATE SPACE SELECTION # ######################### # This inherently uses UniformSampler but a different sampling class could be injected. state_space = SawyerConfigurationSpace() ############################## # STATE VALIDITY FORMULATION # ############################## # Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision. excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx), (get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)] link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs) self_collision_fn = partial( self_collision_test, robot=sawyer_robot, link_pairs=link_pairs)
def main(): sim_context = SawyerSimContext() sim = sim_context.get_sim_instance() logger = sim_context.get_logger() state_space = sim_context.get_state_space() svc = sim_context.get_state_validity() sawyer_robot = sim_context.get_robot() sawyer_id = sawyer_robot.get_simulator_id() ground_plane = sim_context.get_sim_objects(['Ground'])[0] # Exclude the ground plane and the pedestal feet from disabled collisions. excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx # The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions. excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)] ############ # PLANNING # ############ # Disabled collisions during planning with certain eclusions in place. with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs): ####### # PRM # ####### # Use parametric linear interpolation with 10 steps between points. interp = partial(parametric_lerp, steps=10) # See params for PRM specific parameters prm = PRM(state_space, svc, interp, params={ 'n_samples': 3000, 'k': 6, 'ball_radius': 2.0 }) logger.info("Planning....") plan = prm.plan( np.array([0, 0, 0, 0, 0, 0, 0]), np.array([ 1.5262755737449423, -0.1698540226273928, 2.7788151824762055, 2.4546623466066135, 0.7146948867821279, 2.7671787952787184, 2.606128412644311 ])) # get_path() reuses the interp function to get the path between vertices of a successful plan path = prm.get_path(plan) if len(path) == 0: logger.info("Planning failed....") sys.exit(1) logger.info("Plan found....") print(len(path)) ########## # SPLINE # ########## # splinging uses numpy so needs to be converted path = [np.array(p) for p in path] # Create a MinJerk spline trajectory using JointTrajectoryCurve and execute jtc = JointTrajectoryCurve() traj = jtc.generate_trajectory(path, move_time=10) sawyer_robot.execute_trajectory(traj) try: while True: sim.step() except KeyboardInterrupt: p.disconnect() sys.exit(0)