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