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 = vrep.simAddDrawingObject(
objectType=vrep.sim_drawing_lines, size=3, duplicateTolerance=0,
parentObjectHandle=-1, maxItemCount=99999,
ambient_diffuse=[1, 0, 1])
vrep.simAddDrawingObjectItem(self._drawing_handle, None)
init_pose = self._mobile.get_2d_pose()
self._mobile.set_2d_pose(self._path_points[0][:3])
prev_point = tip.get_position()
for i in range(len(self._path_points)):
points = self._path_points[i]
self._mobile.set_2d_pose(points[:3])
p = tip.get_position()
vrep.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 step(self) -> bool:
#print(self._path_points[0][0], self._path_points[0][1])
pos_inter = self.intermediate_target_base.get_position(
relative_to=None)
vrep.simAddDrawingObjectItem(self.drawing_handle,
pos_inter[:2] + [0.0])
pos_inter = self.intermediate_target_base.get_position(
relative_to=self)
#print(pos_inter)
if self.inter_done:
self._next_i_path()
self._set_inter_target(self.i_path)
self.inter_done = False
if sqrt((pos_inter[0])**2 + (pos_inter[1])**2) < 0.1:
self.inter_done = True
#actuation, _ = self.get_base_actuation()
#else:
#actuation, _ = self.get_base_actuation()
#self._mobile.set_joint_target_velocities(actuation)
#if self.i_path == len(self._path_points) - 1:
# self._path_done = True
#actuation, self._path_done = self._mobile.get_base_actuation()
#self._mobile.set_joint_target_velocities(actuation)
return self.get_base_actuation(), False
def clear_visualization(self) -> None:
"""Clears/removes a visualization of the path in the scene.
"""
if self._drawing_handle is not None:
vrep.simAddDrawingObjectItem(self._drawing_handle, None)