def teleport(self,pos=None,th=None):
""" Teleport to pos,th """
if len(self.vertices)>0:
polygon=Polygon(self.vertices)
if pos!=None:
pos=Point(pos)
polygon=translate(polygon,pos-self.pos)
if th!=None:
th=pipi(th)
polygon=rotate(polygon,pipi(th-self.th),origin='centroid',use_radians=True)
self.vertices=list(polygon.exterior.coords)
if pos!=None: self.pos=pos
if th!=None: self.th=pipi(th)
self.time=time() # something changed
def update(self):
""" Update pose of self and time-stamp. Typically called (frequently) when v,w>0
Assumed dtime short wrt. velocity values: linear approx
"""
dt = super().update()
if self.v + abs(self.w) > 0:
dth = self.w * dt
dx = self.v * dt * np.cos(
self.th + dth / 2 +
self.vth) # vth is the rel angle of velocity vector
dy = self.v * dt * np.sin(self.th + dth / 2 + self.vth)
# Going beyond one limit teleports to the contrary
if self.pos.x + dx > W:
dx = dx - 2 * W
elif self.pos.x + dx < -W:
dx = dx + 2 * W
if self.pos.y + dy > H:
dy = dy - 2 * H
elif self.pos.y + dy < -H:
dy = dy + 2 * H
self.pos = translate(self.pos, dx, dy)
self.th = pipi(self.th + dth)
if len(self.vertices) > 0:
polygon = Polygon(self.vertices)
polygon = translate(polygon, dx, dy)
polygon = rotate(polygon,
dth,
origin='centroid',
use_radians=True)
self.vertices = list(polygon.exterior.coords)
return dt
def update(self):
super().update()
if self.where != None:
arrow = Point2D(self.where.x - self.body.pos.x,
self.where.y - self.body.pos.y)
self.vertices = [(self.body.pos.x, self.body.pos.y),
(self.where.x, self.where.y)]
if arrow.r < self.tol:
self.where = None
else:
if self.when > self.time:
v = arrow.r / (self.when - self.time)
else:
v = np.Infinity
w = self.Kp * ((pipi(arrow.a - self.body.th)))
self.body.cmd_vel(v, w)
else:
self.vertices = None
if self.nw == 'stop': # reset v and w
self.body.cmd_vel(v=0, w=0)
elif self.nw == 'wander': # random changes of w
if uniform(0, 1) < self.p:
self.body.cmd_vel(w=choice(('+', '-', '=')))
elif self.nw == 'zigzag': # random changes of direction
if uniform(0, 1) < self.p:
self.body.teleport(
th=self.body.th +
uniform(-1, 1) * self.body.w_max * self.T)
elif self.nw == 'keepgoing': # keep v and w
pass
def update(
self): # random velocity changes in the central area of the room
super().update()
if randint(0, 10) > 9:
self.cmd_vel(choice(('+', '-', '=')), choice(('+', '-', '=')), '=')
if self.pos.x > W / 2 and abs(
self.th
) < np.pi / 2 or self.pos.x < -W / 2 and abs(
self.th
) > np.pi / 2 or self.pos.y > H / 2 and self.th > 0 or self.pos.y < -H / 2 and self.th < 0:
self.th = pipi(self.th + np.pi)
def __init__(self, name, pos=-1, th=0, area=-1, vertices=[], fc='k'):
""" name should be unique, it is for info but also used to find a Body
pos is (x,y), within [-W:W,-H:H] save for borders; -1 for absolute vertices, typically for obstacles
th in rad, 0 is pointing right
area is -1 for absolute vertices; constant values below have been chosen to be nice with W=16 H=9 (total area=576)
vertices are relative (save otherwise indicated by -1's), they are affected by area scale, pos translation and th rotation
fc='k' by default; the default for subclasses are different (grey, blue, yellow, green, red, cyan), and typically objects are fc-ish
"""
self.name = name # str
self.fc = fc #(R,G,B)
if len(vertices) > 0:
self.vertices = vertices # list of (x,y) defining a polygon, for graphical representation
else:
self.vertices = [(0, 0), (0.01, 0.0025),
(0, 0.005)] # good for a MoBody
polygon = Polygon(self.vertices)
if area == -1: # do not scale
self.area = polygon.area
else:
self.area = area
fact = np.sqrt(self.area / polygon.area)
polygon = scale(polygon, fact, fact, origin='centroid')
centroid = polygon.centroid
if pos == -1: # vertices positions are absolute
self.pos = centroid
self.th = 0
else:
self.pos = Point(
pos
) # unconstrained, but usually within [-W:W,-H:H] -- except borders
self.th = pipi(th) # rad in [-pi:pi]
polygon = translate(polygon, self.pos.x - centroid.x,
self.pos.y - centroid.y)
polygon = rotate(polygon,
self.th,
origin='centroid',
use_radians=True)
self.vertices = list(polygon.exterior.coords)
self.r_encl = 0 # radius of enclosing circle centered at c
for vertex in self.vertices:
Pv = Point(vertex)
d = self.pos.distance(Pv)
if d > self.r_encl:
self.r_encl = d
if visual:
self.pp = None # to store the current plot patch
self.time = time()
self.updates = -1
self.avgT = 0