def __init__(self, polygon, **options): WorldObject.__init__(self, do_draw=True, do_step=True, **options) # Robots are always drawn and stepped self.simulated = True self.world = None self.pose = Pose(0, 0, 0) self.fv = 0.0 self.rv = 0.0 # Re-instantiate the polygon with the robot's tags del polygon.options['tags'] self.polygon = Polygon(polygon.points, polygon.center, tags=self.tags, **polygon.options) # The maximum radius, used for pushing the robot back before a collision self._radius = sorted([ self.polygon.center.distance(vertex) for vertex in self.polygon ])[-1]
def __init__(self, **options): self.FV_CAP = 1.5 # meters/second self.RV_CAP = 2 * pi # radians/second BaseRobot.__init__(self, polygon=Polygon([(-0.034, -0.219), (0.057, -0.219), (0.139, -0.170), (0.196, -0.098), (0.216, -0.009), (0.196, 0.079), (0.139, 0.150), (0.057, 0.190), (-0.034, 0.190), (-0.051, 0.190), (-0.051, 0.150), (-0.153, 0.150), (-0.223, 0.074), (-0.223, -0.103), (-0.153, -0.179), (-0.051, -0.179), (-0.051, -0.219)], None, fill='black', dummy=True)) self.arcos = None # The ARCOS client, if connected self.serial_device = None # The serial device, if connected self._fv = 0.0 # Internal forward velocity storage self._rv = 0.0 # Internal rotational velocity storage self._collided = False # Private flag to check if the robot has collided self._sonars = None # Super secret calculated sonars (shh) # self._move_data = {'x': set(), 'y': set(), 'item': None} # Drag data for the canvas self._move_data = set() # Drag data for the canvas self._turn_data = { 'x': 0, 'y': 0, 'item': None } # Drag data for the canvas self._serial_ports = None # Serial ports to use when connecting with ARCOS; any if None self._last_x, self._last_y = 0, 0 # For dealing with encoder rollover self._x_sum, self._y_sum = 0, 0 # For dealing with encoder rollover self._ignore_brain_lag = False # For making the step duration fixed self._raw_sonars = False # Whether to cast out-of-range sonars to None self.set_robot_options( **options) # Sets any options passed in on construction self._sonar_max = 1.5 self._target_found = False self._success = False self._done = False
class BaseRobot(WorldObject): type = 'BaseRobot' """ A base robot class, intended to be subclassed and overridden. Any robot usable in SoaR should supplement or re-implement this class' methods with the desired behavior. Attributes: type (str): A human readable name for robots of this type. world: The instance of :class:`soar.sim.world.World` (or a subclass) in which the robot resides, or `None`, if the robot is real. simulated (bool): Any BaseRobot subclass should consider the robot to be simulated if this is `True`, and real otherwise. By default, it is `False`. pose: An instance of :class:`soar.sim.geometry.Pose` representing the robot's `(x, y, theta)` position. In simulation, this is the actual position; on a real robot this may be determined through other means. polygon: A :class:`soar.sim.world.Polygon` that defines the boundaries of the robot and is used for collision. fv (float): The robot's current translational velocity, in arbitrary units. Positive values indicate movement towards the front of the robot, and negative values indicate movement towards the back. rv (float): The robot's current rotational velocity, in radians/second. Positive values indicate counterclockwise rotation (when viewed from above), and negative values indicate clockwise rotation. Args: polygon: A :class:`soar.sim.world.Polygon` that defines the boundaries of the robot and is used for collision. **options: Arbitrary keyword arguments. This may include Tkinter keywords passed to the `WorldObject` constructor, or robot options supported as arguments to `set_robot_options`. """ def __init__(self, polygon, **options): WorldObject.__init__(self, do_draw=True, do_step=True, **options) # Robots are always drawn and stepped self.simulated = True self.world = None self.pose = Pose(0, 0, 0) self.fv = 0.0 self.rv = 0.0 # Re-instantiate the polygon with the robot's tags del polygon.options['tags'] self.polygon = Polygon(polygon.points, polygon.center, tags=self.tags, **polygon.options) # The maximum radius, used for pushing the robot back before a collision self._radius = sorted([ self.polygon.center.distance(vertex) for vertex in self.polygon ])[-1] def set_robot_options(self, **options): """ Set one or many keyworded, robot-specific options. Document these options here. Args: **options: `BaseRobot` does not support any robot options. """ pass @property def pos( self ): # TODO: This property exists only for backwards compatibility. Deprecate by 2.0. return self.pose @pos.setter def pos( self, t ): # TODO: This property exists only for backwards compatibility. Deprecate by 2.0. self.pose = t def to_dict(self): """ Return a dictionary representation of the robot, usable for serialization. """ return { 'x_pos': self.pose[0], 'y_pos': self.pose[1], 't_pos': self.pose[2], 'fv': self.fv, 'rv': self.rv, 'type': self.type } def move(self, pose): """ Move the robot to the specified `(x, y, theta)` pose. Args: pose: An :class:`soar.sim.geometry.Pose` or 3-tuple-like object to move the robot to. """ x, y, t = pose current_theta = self.pose[2] self.pose = Pose(x, y, t) self.polygon.recenter(self.pose) self.polygon.rotate(self.polygon.center, t - current_theta) def collision(self, other, eps=1e-8): """ Determine whether the robot collides with an object. Supported objects include other robots, and subclasses of :class:`soar.sim.world.Polygon` and :class:`soar.sim.world.Wall`. Args: other: A supported `WorldObject` subclass with which this object could potentially collide. eps (float, optional): The epsilon within which to consider a collision to have occurred, different for each subclass. Returns: list: A list of `(x, y)` tuples consisting of all the collision points with `other`, or `None` if there weren't any. """ if isinstance( other, BaseRobot): # Dispatch to collisions between robot polygons return self.polygon.collision(other.polygon, eps=eps) elif isinstance(other, Polygon) or isinstance(other, Wall): return self.polygon.collision(other, eps=eps) def draw(self, canvas): """ Draw the robot on a canvas. Canvas items are preserved. If drawn more than once on the same canvas, the item will be moved and not redrawn. Args: canvas: An instance of :class:`soar.gui.canvas.SoarCanvas`. """ try: # Try and find the drawn polygon on the canvas, in case it already exists canvas_poly = canvas.find_withtag(self.tags)[0] except IndexError: # If no such item exists, draw it for the first time self.polygon.draw(canvas) else: # Remap metered coordinates to pixel coordinates, and change the canvas polygon coords = canvas.remap_coords( [p for pair in self.polygon for p in pair]) canvas.coords(canvas_poly, coords) def delete(self, canvas): # TODO: Deprecate this in 2.0 """ Delete the robot from a canvas. Args: canvas: An instance of :class:`soar.gui.canvas.SoarCanvas`. """ self.polygon.delete(canvas) def on_load(self): """ Called when the controller of the robot is loaded. The behavior of this method should differ depending on the value of `simulated`; if it is `False`, this method should be used to connect with the real robot. If a connection error occurs, a :class:`soar.errors.SoarIOError` should be raised to notify the client that the error was not due to other causes. """ if not self.simulated: raise SoarIOError('BaseRobot has no real interface to connect to') def on_start(self): """ Called when the controller of the robot is started. This method will always be called by the controller at most once per controller session, before the first step. """ pass def on_pause(self): """ Called when the controller is paused. """ pass def on_step(self, step_duration): """ Called when the controller of the robot undergoes a single step of a specified duration. For BaseRobot, this tries to perform an integrated position update based on the forward and rotational velocities. If the robot cannot move to the new position because there is an object in its way, it will be moved to a safe space just before it would have collided. Subclasses will typically have more complex `on_step()` methods, usually with behavior for stepping non-simulated robots. Args: step_duration (float): The duration of the step, in seconds. """ if self.simulated: # Do the simulated move update (with collision preemption) # Try and make sure that the robot can actually move to its new location # Integrate over the path, making the new position at the end of the arc theta = self.pose[2] d_t = self.rv * step_duration new_theta = theta + d_t if self.rv != 0: d_x = self.fv * (sin(new_theta) - sin(theta)) / self.rv d_y = self.fv * (cos(theta) - cos(new_theta)) / self.rv else: d_x, d_y = self.fv * cos(theta) * step_duration, self.fv * sin( theta) * step_duration new_pos = self.pose.transform((d_x, d_y, d_t)) # Build a dummy wall between the old and new position and check if it collides with anything w = Wall(self.pose.point(), new_pos.point(), dummy=True) collisions = self.world.find_all_collisions( w, condition=lambda obj: obj is not self) if collisions: # If there were collisions, push the robot to a safe distance from the closest one collisions.sort(key=lambda tup: self.pose.distance(tup[1])) safe_point = Point(*collisions[0][1]) offset = Point(self._radius, 0.0).rotate((0, 0), new_pos[2]) safe_point = safe_point.sub(offset) new_pos = Pose(safe_point.x, safe_point.y, new_pos[2]) self.pose = new_pos self.polygon.recenter(new_pos) self.polygon.rotate(self.polygon.center, d_t) def on_stop(self): """ Called when the controller of the robot is stopped. """ pass def on_shutdown(self): """ Called when the controller of the robot is shutdown. If interacting with a real robot, the connection should be safely closed and reset for any later connections. """ pass
# This file is intended as an example. Use it as a template for other Soar brains from soar.robot.base import BaseRobot from soar.gui.plot_window import PlotWindow from soar.hooks import sim_completed from soar.sim.world import Polygon robot = BaseRobot(polygon=Polygon( [(-0.5, 0.5), (0.5, 0.5), (0.5, -0.5), (-0.5, -0.5)], tags='base')) # This function is called when the brain is loaded def on_load(): pass # This function is called when the start button is pushed def on_start(): pass # This function is called every step_duration seconds. By default, it is called 10 times/second def on_step(step_duration): pass # This function is called when the stop button is pushed def on_stop(): pass # This function is called when the robot's controller is shut down