Source code for jyro.simulator.robot

from jyro.simulator.simulator import Segment, Line
from jyro.simulator.device import Speech
from calysto.graphics import Color as GColor

import math
from collections import defaultdict

PIOVER180 = math.pi / 180.0
PIOVER2   = math.pi / 2.0

[docs]class Robot(): def __init__(self, name, x, y, a, boundingBox=None, color="red"): """ a - angle in radians """ if boundingBox is None: boundingBox = [] self.name = name.replace(" ", "_") self.type = "robot" self.stepScalar = 1.0 # normally = 1.0 self._xya = (x, y, a) # original, for reset self.reset() self.boundingBox = boundingBox # ((x1, x2), (y1, y2)) NOTE: Xs then Ys of bounding box self.boundingSeg = [] if boundingBox != []: self.radius = max(max(map(abs, boundingBox[0])), max(map(abs, boundingBox[1]))) # meters else: self.radius = 0.0 self.color = color self.colorParts = {"ir": "pink", "sonar": "lightgray", "bumper": "black", "trail": color} self.devices = [] self.device = defaultdict(lambda: None) self.physics = None # will be set when added to simulator # -1: don't automatically turn display on when subscribing: self.display = {"body": 1, "boundingBox": 0, "gripper": -1, "camera": 0, "sonar": 0, "light": -1, "lightBlocked": 0, "trail": -1, "ir": -1, "bumper": 1, "speech": 1, "robots": 1, "devices": 1} self.shapes = [] self.trail = [] self.useTrail = False self.addDevice(Speech()) def __getitem__(self, item): return self.device[item]
[docs] def brain(self, robot): pass
[docs] def reset(self): self._gx, self._gy, self._ga = self._xya self._px, self._py, self._pa = self._xya self.stall = 0 self.energy = 10000.0 self.x, self.y, self.a = (0.0, 0.0, 0.0) # localize self.maxEnergyCostPerStep = 1.0 self.sayText = "" self.friction = 1.0 self.vx, self.vy, self.va = (0.0, 0.0, 0.0) # meters / second, rads / second self.trail = []
def _repr_svg_(self): from jyro.simulator import Physics, Canvas canvas = Canvas((240, 240)) canvas.max_x = 1.0 # meters canvas.max_y = 1.0 # meters canvas.scale = min(canvas.width/canvas.max_x, canvas.height/canvas.max_y) xy = self._gx, self._gy self._gx, self._gy = (0.5, 0.5) remove_sim = False if self.physics is None: self.physics = Physics() # for drawings self.updateDevices() # ASSUME: if old sim, it is up to date (self.shapes are there) remove_sim = True self.draw(canvas) if remove_sim: self.physics = None svg = canvas._repr_svg_() self._gx, self._gy = xy return svg
[docs] def drawRay(self, dtype, x1, y1, x2, y2, color): self.shapes.append(Line((x1, y1), (x2, y2), outline=color))
[docs] def additionalSegments(self, propose, x, y, cos_a90, sin_a90, **dict): """ propose is where it would go, if moved with device velocity, etc. """ # dynamic segments retval = [] for device in self.devices: segs = device.additionalSegments(propose, x, y, cos_a90, sin_a90, **dict) if segs: retval.extend(segs) return retval
[docs] def addBoundingSeg(self, boundingSeg): if self.boundingSeg == []: self.boundingSeg = boundingSeg else: self.boundingSeg[0].extend(boundingSeg[0]) self.boundingSeg[1].extend(boundingSeg[1]) segradius = max(max(map(abs, boundingSeg[0])), max(map(abs, boundingSeg[1]))) # meters self.radius = max(self.radius, segradius)
[docs] def localize(self, x = 0, y = 0, th = 0): self.x, self.y, self.a = (x, y, th)
[docs] def say(self, text): self.sayText = text
[docs] def updatePosition(self, x, y, a=None): # first, figure out how much we moved in the global coords: a90 = -self._ga cos_a90 = math.cos(a90) sin_a90 = math.sin(a90) dx = (x - self._gx) * cos_a90 - (y - self._gy) * sin_a90 dy = (x - self._gx) * sin_a90 + (y - self._gy) * cos_a90 # then, move that much in the local coords: local90 = -self.a cos_local90 = math.cos(local90) sin_local90 = math.sin(local90) a90 = -self.a self.y += dx * cos_local90 - dy * sin_local90 self.x += dx * sin_local90 + dy * cos_local90 self._px, self._py = self._gx, self._gy self._gx, self._gy = x, y if a is not None: self.setAngle(a) self.updateTrail()
[docs] def updateTrail(self): if self.useTrail: if len(self.trail) == 0 or self.trail[-1] != (self._gx, self._gy, self._ga): self.trail.append((self._gx, self._gy, self._ga))
[docs] def setAngle(self, a): # if our angle changes, update localized position: diff = a - self._ga self.a += diff self.a = self.a % (2 * math.pi) # keep in the positive range self._ga = a % (2 * math.pi) # keep in the positive range self.updateTrail()
[docs] def setPose(self, x, y, a=None): self._px, self._py = self._gx, self._gy self._gx = x self._gy = y if a is not None: self._ga = a % (2 * math.pi) # keep in the positive range self.updateDevices() self.updateTrail()
[docs] def move(self, vx, va): self.vx = vx self.va = va
[docs] def rotate(self, va): self.va = va
[docs] def translate(self, vx): self.vx = vx
[docs] def getPose(self): """ Returns global coordinates. """ return (self._gx, self._gy, self._ga)
[docs] def getIndex(self, dtype, i): index = 0 for d in self.devices: if d.type == dtype: if i == index: return d index += 1 return None
[docs] def updateDevices(self): # FIXME: updateDevices() has side effect: # puts robot.shapes in sim.canvas coords self.shapes[:] = [] for device in self.devices: if device.active: device.update(self)
[docs] def step(self, timeslice=100): """ Move the robot self.velocity amount, if not blocked. """ self.ovx, self.ovy, self.ova = self.vx, self.vy, self.va gvx = self.ovx * self.stepScalar gvy = self.ovy * self.stepScalar vx = gvx * math.sin(-self._ga) + gvy * math.cos(-self._ga) vy = gvx * math.cos(-self._ga) - gvy * math.sin(-self._ga) va = self.ova # proposed positions: p_x = self._gx + vx * (timeslice / 1000.0) # miliseconds p_y = self._gy + vy * (timeslice / 1000.0) # miliseconds p_a = self._ga + va * (timeslice / 1000.0) # miliseconds # for each of the robot's bounding box segments: a90 = p_a + PIOVER2 cos_a90 = math.cos(a90) sin_a90 = math.sin(a90) if vx != 0 or vy != 0 or va != 0: self.energy -= self.maxEnergyCostPerStep # let's check if that movement would be ok: segments = [] if self.boundingBox != []: xys = map(lambda x, y: (p_x + x * cos_a90 - y * sin_a90, p_y + x * sin_a90 + y * cos_a90), self.boundingBox[0], self.boundingBox[1]) xys = list(xys) for i in range(len(xys)): bb = Segment( xys[i], xys[i - 1]) segments.append(bb) if self.boundingSeg != []: xys = map(lambda x, y: (p_x + x * cos_a90 - y * sin_a90, p_y + x * sin_a90 + y * cos_a90), self.boundingSeg[0], self.boundingSeg[1]) xys = list(xys) for i in range(0, len(xys), 2): bb = Segment( xys[i], xys[i + 1]) segments.append(bb) for s in self.additionalSegments(True, p_x, p_y, cos_a90, sin_a90): segments.append(s) for bb in segments: # check each segment of the robot's bounding segs for wall obstacles: for w in self.physics.world: if bb.intersects(w): self.stall = 1 return False # collision # check each segment of the robot's bounding box for other robots: for r in self.physics.robots: if r.name == self.name: continue # don't compare with your own! r_a90 = r._ga + PIOVER2 cos_r_a90 = math.cos(r_a90) sin_r_a90 = math.sin(r_a90) r_segments = [] if r.boundingBox != []: r_xys = map(lambda x, y: (r._gx + x * cos_r_a90 - y * sin_r_a90, r._gy + x * sin_r_a90 + y * cos_r_a90), r.boundingBox[0], r.boundingBox[1]) r_xys = list(r_xys) for j in range(len(r_xys)): r_seg = Segment(r_xys[j], r_xys[j - 1]) r_segments.append(r_seg) if r.boundingSeg != []: r_xys = map(lambda x, y: (r._gx + x * cos_r_a90 - y * sin_r_a90, r._gy + x * sin_r_a90 + y * cos_r_a90), r.boundingSeg[0], r.boundingSeg[1]) r_xys = list(r_xys) for j in range(0, len(r_xys), 2): r_seg = Segment(r_xys[j], r_xys[j + 1]) r_segments.append(r_seg) for s in r.additionalSegments(False, r._gx, r._gy, cos_r_a90, sin_r_a90): r_segments.append(s) for r_seg in r_segments: bbintersect = bb.intersects(r_seg) if bbintersect: self.stall = 1 return False # collision # ok! move the robot, if it wanted to move for device in self.devices: device.step() if self.friction != 1.0: self.ovx *= self.friction self.ovy *= self.friction if 0.0 < self.ovx < 0.1: self.ovx = 0.0 if 0.0 < self.ovy < 0.1: self.ovy = 0.0 if 0.0 > self.ovx > -0.1: self.ovx = 0.0 if 0.0 > self.ovy > -0.1: self.ovy = 0.0 self.stall = 0 self.updatePosition(p_x, p_y, p_a) return True # able to move
[docs] def draw(self, canvas): for shape in self.shapes: shape.draw(canvas) if self.display["robots"] == 1: self.drawRobot(canvas) if self.display["devices"] == 1: self.drawDevices(canvas)
[docs] def drawDevices(self, canvas): for device in self.devices: device.draw(self, canvas)
[docs] def addDevice(self, dev): dev.robot = self self.devices.append(dev) self.device[dev.type] = dev dev.update(self) return self # so can give nice display with new device visual
[docs] def serialize(self, item='all'): """ """ d = {} d["pose"] = self.getPose() for dname in self.device.keys(): if self.device[dname]: d[dname] = self.device[dname].serialize(item) return d
[docs]class Blimp(Robot): def __init__(self, *args, **kwargs): Robot.__init__(self, *args, **kwargs) self.radius = 0.44 # meters self.color = "purple"
[docs] def drawRobot(self, canvas): a90 = self._ga + PIOVER2 # angle is 90 degrees off for graphics cos_a90 = math.cos(a90) sin_a90 = math.sin(a90) radius = self.radius canvas.drawOval(self._gx - radius, self._gy - radius, self._gx + radius, self._gy + radius, fill=self.color, outline="blue") x = (self._gx + radius * cos_a90 - 0 * sin_a90) y = (self._gy + radius * sin_a90 + 0 * cos_a90) canvas.drawLine(self._gx, self._gy, x, y, outline="blue", width=3)
[docs]class Puck(Robot): def __init__(self, *args, **kwargs): Robot.__init__(self, *args, **kwargs) self.radius = 0.05 self.friction = 0.90 self.type = "puck"
[docs] def drawRobot(self, canvas): """ Draws the body of the robot. Not very efficient. """ if self.display["body"] == 1: radius = self.radius x1, y1, x2, y2 = ((self._gx - radius), (self._gy - radius), (self._gx + radius), (self._gy + radius)) canvas.drawOval(x1, y1, x2, y2, fill=self.color, outline="black") if self.display["boundingBox"] == 1 and self.boundingBox != []: # Body Polygon, by x and y lists: a90 = self._ga + PIOVER2 # angle is 90 degrees off for graphics cos_a90 = math.cos(a90) sin_a90 = math.sin(a90) xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), self.boundingBox[0], self.boundingBox[1]) xy = list(xy) canvas.drawPolygon(xy, fill="", outline="purple")
[docs]class Pioneer(Robot): def __init__(self, name, x, y, a, color="red"): Robot.__init__(self, name, x, y, a, boundingBox=((.225, .225, -.225, -.225), (.175, -.175, -.175, .175)), color=color) self.radius = 0.4
[docs] def drawRobot(self, canvas): """ Draws the body of the robot. Not very efficient. """ a90 = self._ga + PIOVER2 # angle is 90 degrees off for graphics cos_a90 = math.cos(a90) sin_a90 = math.sin(a90) # Draw trail: if self.display["trail"] == 1: cx, cy = -1, -1 for (x, y, a) in self.trail: # poses if (cx, cy) != (-1, -1): canvas.drawLine(cx, cy, x, y, width=1, outline="purple") cx, cy = x, y # Body Polygon, by x and y lists: sx = [.225, .15, -.15, -.225, -.225, -.15, .15, .225] sy = [.08, .175, .175, .08, -.08, -.175, -.175, -.08] if self.display["body"] == 1: xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), sx, sy) xy = list(xy) if self.stall: canvas.drawPolygon(xy, fill="white", outline="black") else: canvas.drawPolygon(xy, fill=self.color, outline=self.color) if self.display["boundingBox"] == 1: if self.boundingBox != []: xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), self.boundingBox[0], self.boundingBox[1]) xy = list(xy) canvas.drawPolygon(xy, fill="", outline="purple") if self.boundingSeg != []: xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), self.boundingSeg[0], self.boundingSeg[1]) xy = list(xy) for i in range(0, len(xy), 2): canvas.drawLine(xy[i][0], xy[i][1], xy[i + 1][0], xy[i + 1][1], outline="purple") additionalSegments = self.additionalSegments(False, self._gx, self._gy, cos_a90, sin_a90) if additionalSegments != []: for s in additionalSegments: canvas.drawLine((s.start[0]), (s.start[1]), (s.end[0]), (s.end[1]), outline="purple") # Draw an arrowhead at (x, y), pointing in heading direction canvas.drawArrow(self._gx, self._gy, self._ga, 0.05)
[docs]class Scribbler(Robot): def __init__(self, name, x, y, a, color="red"): Robot.__init__(self, name, x, y, a, boundingBox=(( .09, .09,-.09,-.09), ## front, back ( .08,-.08,-.08, .08)), ## sides color=color) self.radius = 0.09
[docs] def drawRobot(self, canvas): sx = [0.05, 0.05, 0.07, 0.07, 0.09, 0.09, 0.07, 0.07, 0.05, 0.05, -0.05, -0.05, -0.07, -0.08, -0.09, -0.09, -0.08, -0.07, -0.05, -0.05] sy = [0.06, 0.08, 0.07, 0.06, 0.06, -0.06, -0.06, -0.07, -0.08, -0.06, -0.06, -0.08, -0.07, -0.06, -0.05, 0.05, 0.06, 0.07, 0.08, 0.06] a90 = self._ga + PIOVER2 # angle is 90 degrees off for graphics cos_a90 = math.cos(a90) sin_a90 = math.sin(a90) if self.display["body"] == 1: xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), sx, sy) xy = list(xy) canvas.drawPolygon(xy, fill=self.color, outline="black") # -------------------------------------------------------------------------- # Parts: wheel, wheel, battery bx = [[ .04, .04, -.04, -.04], [ .04, .04, -.04, -.04]] by = [[ .08, .07, .07, .08], [ -.08, -.07, -.07, -.08]] colors = ["black", "black", "gray", "yellow", "yellow"] for i in range(len(bx)): xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), bx[i], by[i]) xy = list(xy) canvas.drawPolygon(xy, fill=colors[i]) if self.display["boundingBox"] == 1: if self.boundingBox != []: xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), self.boundingBox[0], self.boundingBox[1]) xy = list(xy) canvas.drawPolygon(xy, fill=GColor(0,0,0,0), outline="purple") if self.boundingSeg != []: xy = map(lambda x, y: (self._gx + x * cos_a90 - y * sin_a90, self._gy + x * sin_a90 + y * cos_a90), self.boundingSeg[0], self.boundingSeg[1]) xy = list(xy) for i in range(0, len(xy), 2): canvas.drawLine(xy[i][0], xy[i][1], xy[i + 1][0], xy[i + 1][1], outline="purple") additionalSegments = self.additionalSegments(False, self._gx, self._gy, cos_a90, sin_a90) if additionalSegments != []: for s in additionalSegments: canvas.drawLine( (s.start[0]), (s.start[1]), (s.end[0]), (s.end[1]), outline="purple") # Draw an arrowhead at (x, y), pointing in heading direction canvas.drawArrow(self._gx, self._gy, self._ga, 0.012)