def visualize(self) -> None: """Draws a visualization of the path in the scene. The visualization can be removed with :py:meth:`ConfigurationPath.clear_visualization`. """ if len(self._path_points) <= 0: raise RuntimeError("Can't visualise a path with no points.") tip = self._arm.get_tip() self._drawing_handle = sim.simAddDrawingObject( objectType=sim.sim_drawing_lines, size=3, duplicateTolerance=0, parentObjectHandle=-1, maxItemCount=99999, ambient_diffuse=[1, 0, 1]) sim.simAddDrawingObjectItem(self._drawing_handle, None) init_angles = self._arm.get_joint_positions() self._arm.set_joint_positions( self._path_points[0: len(self._arm.joints)], allow_force_mode=False) prev_point = tip.get_position() for i in range(len(self._arm.joints), len(self._path_points), len(self._arm.joints)): points = self._path_points[i:i + len(self._arm.joints)] self._arm.set_joint_positions(points, allow_force_mode=False) p = tip.get_position() sim.simAddDrawingObjectItem(self._drawing_handle, prev_point + p) prev_point = p # Set the arm back to the initial config self._arm.set_joint_positions(init_angles, allow_force_mode=False)
def visualize(self) -> None: """Draws a visualization of the path in the scene. The visualization can be removed with :py:meth:`ConfigurationPath.clear_visualization`. """ if len(self._path_points) <= 0: raise RuntimeError("Can't visualise a path with no points.") tip = self._mobile self._drawing_handle = sim.simAddDrawingObject( objectType=sim.sim_drawing_lines, size=3, duplicateTolerance=0, parentObjectHandle=-1, maxItemCount=99999, ambient_diffuse=[1, 0, 1]) sim.simAddDrawingObjectItem(self._drawing_handle, None) init_pose = self._mobile.get_2d_pose() self._mobile.set_2d_pose(self._path_points[0][:3]) prev_point = list(tip.get_position()) for i in range(len(self._path_points)): points = self._path_points[i] self._mobile.set_2d_pose(points[:3]) p = list(tip.get_position()) sim.simAddDrawingObjectItem(self._drawing_handle, prev_point + p) prev_point = p # Set the arm back to the initial config self._mobile.set_2d_pose(init_pose[:3])
def clear_visualization(self) -> None: """Clears/removes a visualization of the path in the scene. """ if self._drawing_handle is not None: sim.simAddDrawingObjectItem(self._drawing_handle, None)
def __init__(self, escene_name='proximity_sensor.ttt', start=[100, 100], goal=[180, 500], rand_area=[100, 450], path_resolution=5.0, margin=0., margin_to_goal=0.5, action_max=[.5, 1.], action_min=[0., 0.], _load_path=True, path_name="PathNodes", type_of_planning="PID", type_replay_buffer="random", max_laser_range=1.0, headless=False): SCENE_FILE = join(dirname(abspath(__file__)), escene_name) self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=headless) self.pr.start() self.agent = Pioneer("Pioneer_p3dx", type_of_planning=type_of_planning, type_replay_buffer=type_replay_buffer) self.agent.set_control_loop_enabled(False) self.initial_joint_positions = self.agent.get_joint_positions() self.initial_position = self.agent.get_position() print(f"Agent initial position: {self.initial_position}") self.vision_map = VisionSensor("VisionMap") self.vision_map.handle_explicitly() self.vision_map_handle = self.vision_map.get_handle() self.floor = Shape("Floor_respondable") self.perspective_angle = self.vision_map.get_perspective_angle # degrees self.resolution = self.vision_map.get_resolution # list [resx, resy] self.margin = margin self.margin_to_goal = margin_to_goal self.rand_area = rand_area self.start = start self.goal = goal self.type_of_planning = type_of_planning self.max_laser_range = max_laser_range self.max_angle_orientation = np.pi scene_image = self.vision_map.capture_rgb() * 255 scene_image = np.flipud(scene_image) self.rew_weights = [1, 10000, 5000] self.start_position = Dummy("Start").get_position() self.goal_position = Dummy("Goal").get_position() # [x, y, z] self.local_goal_aux = Dummy("Local_goal_aux") self.local_goal_aux_pos_list = [[-3.125, 2.175, 0], [2.9, 3.575, 0], self.goal_position, self.goal_position] self.ind_local_goal_aux = 0 self.max_distance = get_distance([-7.5, -4.1, 0.], self.goal_position) self.distance_to_goal_m1 = get_distance(self.start_position, self.goal_position) self.c_lin_vel = self.c_ang_vel = self.b_lin_vel = self.b_ang_vel = 0. self.action_max = action_max self.action_min = action_min if type_of_planning == 'PID': self.planning_info_logger = get_logger("./loggers", "Planning_Info.log") self.image_path = None if exists("./paths/" + path_name + ".npy") and _load_path: print("Load Path..") self.image_path = np.load("./paths/" + path_name + ".npy", allow_pickle=True) else: print("Planning...") self.image_path = self.Planning( scene_image, self.start, self.goal, self.rand_area, path_resolution=path_resolution, logger=self.planning_info_logger) assert self.image_path is not None, "path should not be a Nonetype" self.real_path = self.path_image2real(self.image_path, self.start_position) # project in coppelia sim sim_drawing_points = 0 point_size = 10 #[pix] duplicate_tolerance = 0 parent_obj_handle = self.floor.get_handle() max_iter_count = 999999 ambient_diffuse = (255, 0, 0) blue_ambient_diffuse = (0, 0, 255) point_container = sim.simAddDrawingObject( sim_drawing_points, point_size, duplicate_tolerance, parent_obj_handle, max_iter_count, ambient_diffuse=ambient_diffuse) local_point_container = sim.simAddDrawingObject( sim_drawing_points, point_size, duplicate_tolerance, parent_obj_handle, max_iter_count, ambient_diffuse=blue_ambient_diffuse) # debug for point in self.real_path: point_data = (point[0], point[1], 0) sim.simAddDrawingObjectItem(point_container, point_data) # You need to get the real coord in the real world assert local_point_container is not None, "point container shouldn't be empty" self.agent.load_point_container(local_point_container) self.agent.load_path(self.real_path)