diff --git a/guided_mrmp/simulator.py b/guided_mrmp/simulator.py index 0c731334875e1e40da3fc9a61c33cba91b83f410..952b4b006a76319413226be791a4ba732d93bd6b 100644 --- a/guided_mrmp/simulator.py +++ b/guided_mrmp/simulator.py @@ -27,7 +27,7 @@ class Simulator: def all_robots_at_goal(self): for r in self.robots: - if (np.sqrt((r.current_position[0] - r.goal[0]) ** 2 + (r.current_position[1] - r.goal[1]) ** 2) > 1): + if (np.sqrt((r.current_position[0] - r.goal[0]) ** 2 + (r.current_position[1] - r.goal[1]) ** 2) > 10): return False return True @@ -59,10 +59,12 @@ class Simulator: def draw_environment(self, screen): screen.fill((255,255,255)) for obs in self.circ_obstacles: - pygame.draw.circle(screen, (0,0,0), obs[0:2]*self.scaling_factor, obs[2]*self.scaling_factor) + pygame.draw.circle(screen, (0,0,0), (obs[0]*self.scaling_factor,obs[1]*self.scaling_factor), obs[2]*self.scaling_factor) for obs in self.rect_obstacles: pygame.draw.rect(screen, (0,0,0), obs) + + def draw_robots(self, screen): for robot in self.robots: x,y,yaw = robot.current_position @@ -102,4 +104,4 @@ class Simulator: if show_vis: pygame.display.flip() - +