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)
