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)