summaryrefslogtreecommitdiff
path: root/host
diff options
context:
space:
mode:
Diffstat (limited to 'host')
-rw-r--r--host/simu/model/distance_sensor_sensopart.py2
-rw-r--r--host/simu/model/rectangular_obstacle.py63
-rw-r--r--host/simu/model/round_obstacle.py17
-rw-r--r--host/simu/model/switch.py8
-rw-r--r--host/simu/model/table.py4
-rw-r--r--host/simu/model/table_eurobot2011.py13
-rw-r--r--host/simu/model/table_eurobot2012.py93
-rw-r--r--host/simu/model/test/test_table.py212
-rw-r--r--host/simu/robots/aquajim/link/bag.py6
-rw-r--r--host/simu/robots/giboulee/link/bag.py6
-rw-r--r--host/simu/robots/marcel/link/bag.py8
-rw-r--r--host/simu/robots/marcel/model/loader.py1
-rw-r--r--host/simu/robots/robospierre/link/bag.py8
-rw-r--r--host/simu/robots/robospierre/model/bag.py31
-rw-r--r--host/simu/robots/robospierre/model/clamp.py164
-rw-r--r--host/simu/robots/robospierre/view/bag.py4
-rw-r--r--host/simu/robots/robospierre/view/clamp.py41
-rw-r--r--host/simu/robots/robospierre/view/robot.py4
-rw-r--r--host/simu/utils/intersect.py103
-rw-r--r--host/simu/utils/vector.py267
-rw-r--r--host/simu/view/switch.py2
-rw-r--r--host/simu/view/table_eurobot2011.py41
-rw-r--r--host/simu/view/table_eurobot2012.py189
-rw-r--r--host/utils/init_proto.py76
24 files changed, 1244 insertions, 119 deletions
diff --git a/host/simu/model/distance_sensor_sensopart.py b/host/simu/model/distance_sensor_sensopart.py
index 16676e5d..26ed604f 100644
--- a/host/simu/model/distance_sensor_sensopart.py
+++ b/host/simu/model/distance_sensor_sensopart.py
@@ -59,8 +59,8 @@ class DistanceSensorSensopart (Observable):
self.link = link
self.scheduler = scheduler
self.value = None
- self.register (self.__update)
self.evaluate ()
+ self.register (self.__update)
def evaluate (self):
# Compute real distance.
diff --git a/host/simu/model/rectangular_obstacle.py b/host/simu/model/rectangular_obstacle.py
new file mode 100644
index 00000000..71cfdc8b
--- /dev/null
+++ b/host/simu/model/rectangular_obstacle.py
@@ -0,0 +1,63 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Obstacle with a rectangular shape."""
+from utils.observable import Observable
+from simu.utils.vector import vector
+from math import pi
+import simu.utils.intersect
+
+class RectangularObstacle (Observable):
+
+ def __init__ (self, dim, level = 0):
+ Observable.__init__ (self)
+ self.pos = None
+ self.angle = 0
+ self.dim = dim
+ self.level = level
+
+ def intersect (self, a, b):
+ """If the segment [AB] intersects the obstacle, return distance from a
+ to intersection point, else, return None."""
+ if self.pos is None or self.angle is None:
+ return None
+ # Find intersection with each rectangle segments. There is at most
+ # two intersections, return the nearest.
+ u = vector.polar (self.angle, self.dim[0] / 2.)
+ v = vector.polar (self.angle + pi / 2, self.dim[1] / 2.)
+ o = vector (self.pos)
+ p1 = o + u + v
+ p2 = o - u + v
+ p3 = o - u - v
+ p4 = o + u - v
+ found = None
+ for c, d in ((p1, p2), (p2, p3), (p3, p4), (p4, p1)):
+ i = simu.utils.intersect.segment_segment (a, b, c, d)
+ if i is not None:
+ if found is not None:
+ found = min (found, i)
+ break
+ else:
+ found = i
+ return found
+
diff --git a/host/simu/model/round_obstacle.py b/host/simu/model/round_obstacle.py
index 6658ed19..e749440b 100644
--- a/host/simu/model/round_obstacle.py
+++ b/host/simu/model/round_obstacle.py
@@ -24,6 +24,7 @@
"""Obstacle with a round shape."""
from math import pi, cos, sin, sqrt
from utils.observable import Observable
+from simu.utils.vector import vector
class RoundObstacle (Observable):
@@ -38,17 +39,19 @@ class RoundObstacle (Observable):
to intersection point, else, return None."""
if self.pos is None:
return None
- ab = sqrt ((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2) # distance AB.
- n = ((b[0] - a[0]) / ab, (b[1] - a[1]) / ab) # vector of length 1.
- o = self.pos # obstacle center.
+ a, b = vector (a), vector (b)
+ vab = b - a
+ ab = abs (vab) # distance AB.
+ n = vab.unit () # vector of length 1.
+ o = vector (self.pos) # obstacle center.
# To check if the line (AB) intersects the circle, compute distance
# from circle center to line using a dot product.
- vao = (o[0] - a[0], o[1] - a[1]) # vector AO.
- # dot product, (-n[1], n[0]) is perpendicular to n.
- doc = abs (vao[0] * -n[1] + vao[1] * n[0])
+ vao = o - a # vector AO.
+ # abs of dot product.
+ doc = abs (vao * n.normal ())
if doc < self.radius:
# Line intersects, check if segment intersects.
- m = vao[0] * n[0] + vao[1] * n[1]
+ m = vao * n
f = sqrt (self.radius ** 2 - doc ** 2)
if m - f > 0 and m - f < ab:
return m - f
diff --git a/host/simu/model/switch.py b/host/simu/model/switch.py
index 30cc1466..6a06a954 100644
--- a/host/simu/model/switch.py
+++ b/host/simu/model/switch.py
@@ -26,13 +26,17 @@ from utils.observable import Observable
class Switch (Observable):
- def __init__ (self, link):
+ def __init__ (self, link, invert = False):
Observable.__init__ (self)
self.link = link
self.state = None
+ self.invert = invert
self.register (self.__update)
def __update (self):
- self.link.state = self.state
+ if not self.invert:
+ self.link.state = self.state
+ else:
+ self.link.state = not self.state
self.link.notify ()
diff --git a/host/simu/model/table.py b/host/simu/model/table.py
index 41767ac4..d79f7758 100644
--- a/host/simu/model/table.py
+++ b/host/simu/model/table.py
@@ -22,6 +22,7 @@
#
# }}}
"""Table model."""
+from utils.observable import Observable
class Intersect:
@@ -29,9 +30,10 @@ class Intersect:
self.obstacle = obstacle
self.distance = distance
-class Table:
+class Table (Observable):
def __init__ (self):
+ Observable.__init__ (self)
self.obstacles = [ ]
def intersect (self, a, b, level = None, comp = None):
diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py
index c9c1d193..f200028e 100644
--- a/host/simu/model/table_eurobot2011.py
+++ b/host/simu/model/table_eurobot2011.py
@@ -31,8 +31,11 @@ class Table (simu.model.table.Table):
def __init__ (self, cards = None):
simu.model.table.Table.__init__ (self)
# Draw cards.
- if cards is None:
- cards = [ randrange (20) for i in xrange (3) ]
+ cards = [ ]
+ while len (cards) != 3:
+ card = randrange (20)
+ if card not in cards:
+ cards.append (card)
self.cards = cards
def pos (card):
king_pos = card // 4
@@ -81,3 +84,9 @@ class Table (simu.model.table.Table):
self.pawns.append (pawn)
# Add everything to obstacles.
self.obstacles += self.pawns
+
+ def add_pawn (self, pawn):
+ self.pawns.append (pawn)
+ self.obstacles.append (pawn)
+ self.notify ()
+
diff --git a/host/simu/model/table_eurobot2012.py b/host/simu/model/table_eurobot2012.py
new file mode 100644
index 00000000..38960513
--- /dev/null
+++ b/host/simu/model/table_eurobot2012.py
@@ -0,0 +1,93 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Table model for Eurobot 2012."""
+import simu.model.table
+from simu.model.round_obstacle import RoundObstacle
+from simu.model.rectangular_obstacle import RectangularObstacle
+from math import pi
+import math
+import random
+
+class Table (simu.model.table.Table):
+
+ def __init__ (self, cards = None):
+ simu.model.table.Table.__init__ (self)
+ # Well, this is a boring write only code which create every elements.
+ # Add coins.
+ self.coins = [ ]
+ def add_coin (pos, angle, level = 1):
+ coin = RoundObstacle (60, level)
+ coin.pos = pos
+ coin.angle = angle
+ coin.value = 1
+ self.coins.append (coin)
+ def add_coin_circle (center, radius, start, step, n, level = 1):
+ angle = start
+ for i in xrange (n):
+ pos = (center[0] + radius * math.cos (angle),
+ center[1] + radius * math.sin (angle))
+ add_coin (pos, angle, level)
+ angle += step
+ add_coin ((1000, 1500), 0)
+ add_coin ((2000, 1500), pi)
+ add_coin ((450, 300), pi)
+ add_coin ((3000 - 450, 300), 0)
+ add_coin_circle ((1500, 300), 90, 0, pi / 2, 4)
+ add_coin_circle ((1500 - 400, 1000), 300 - 60, pi / 4, pi / 4, 7)
+ add_coin_circle ((1500 + 400, 1000), 300 - 60, pi + pi / 4, pi / 4, 7)
+ add_coin_circle ((1500 - 400, 1000), 115, pi / 4, pi / 2, 4)
+ add_coin_circle ((1500 + 400, 1000), 115, pi / 4, pi / 2, 4)
+ add_coin_circle ((1500 - 400, 1000), 105, pi / 4, pi / 2, 4, 3)
+ add_coin_circle ((1500 + 400, 1000), 105, pi / 4, pi / 2, 4, 3)
+ # Add gold bars.
+ self.gold_bars = [ ]
+ def add_gold_bar (pos, angle, level = 1):
+ gold_bar = RectangularObstacle ((150, 70), level)
+ gold_bar.pos = pos
+ gold_bar.angle = angle
+ gold_bar.value = 3
+ self.gold_bars.append (gold_bar)
+ add_gold_bar ((1500, 647), 0)
+ add_gold_bar ((1500 - 400, 1000 + 125 - 35), 0, 2)
+ add_gold_bar ((1500 + 400, 1000 + 125 - 35), 0, 2)
+ add_gold_bar ((1500 - 400, 1000 - 125 + 35), 0, 2)
+ add_gold_bar ((1500 + 400, 1000 - 125 + 35), 0, 2)
+ ba = pi / 2 - 0.04995839
+ cba = math.cos (ba)
+ sba = math.sin (ba)
+ gbx = 400 - (285 + 75) * cba + 35
+ gby = 1500 - (285 + 75) * sba
+ add_gold_bar ((gbx, gby), ba)
+ add_gold_bar ((3000 - gbx, gby), pi - ba)
+ # Set random black coins.
+ nblack = 0
+ while nblack < 4:
+ coin = random.choice (self.coins[2:])
+ if coin.value:
+ coin.value = 0
+ nblack += 1
+ # Add everything to obstacles.
+ self.obstacles += self.coins
+ self.obstacles += self.gold_bars
+
diff --git a/host/simu/model/test/test_table.py b/host/simu/model/test/test_table.py
new file mode 100644
index 00000000..9595f256
--- /dev/null
+++ b/host/simu/model/test/test_table.py
@@ -0,0 +1,212 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Test table model and obstacles."""
+from simu.model.table import Table
+from simu.model.round_obstacle import RoundObstacle
+from simu.model.rectangular_obstacle import RectangularObstacle
+from simu.inter.drawable import Drawable, DrawableCanvas
+from simu.utils.vector import vector
+from Tkinter import *
+from math import pi
+
+class Area (Drawable):
+
+ def __init__ (self, onto):
+ Drawable.__init__ (self, onto)
+ self.table = Table ()
+ self.a = (0, 0)
+ self.b = (10, 10)
+ self.result = None
+
+ def draw (self):
+ self.reset ()
+ for o in self.table.obstacles:
+ if o.pos is None:
+ continue
+ if isinstance (o, RoundObstacle):
+ self.draw_circle (o.pos, o.radius)
+ elif isinstance (o, RectangularObstacle):
+ self.trans_push ()
+ self.trans_translate (o.pos)
+ self.trans_rotate (o.angle)
+ self.draw_circle ((0, 0), 0.2)
+ self.draw_rectangle ((o.dim[0] / 2, o.dim[1] / 2),
+ (-o.dim[0] / 2, -o.dim[1] / 2), fill = '')
+ self.trans_pop ()
+ else:
+ raise TypeError ("unknown obstacle")
+ if self.a is not None:
+ self.draw_circle (self.a, 0.2, fill = 'green')
+ if self.b is not None:
+ self.draw_circle (self.b, 0.2, fill = 'red')
+ if self.a is not None and self.b is not None:
+ self.draw_line (self.a, self.b, arrow = 'last')
+ if self.result is not None:
+ self.draw_circle (self.result, 0.2, fill = 'yellow')
+
+ def update (self, test, **kwargs):
+ self.result = None
+ if self.a is not None and self.b is not None:
+ if test == 'intersect':
+ def nearer (a, b): return a < b
+ i = self.table.intersect (self.a, self.b, comp = nearer,
+ **kwargs)
+ if i is not None:
+ a, b = vector (self.a), vector (self.b)
+ self.result = (b - a).unit () * i.distance
+ elif test == 'nearest':
+ n = self.table.nearest (self.b, **kwargs)
+ if n is not None:
+ self.result = n.pos
+
+class AreaView (DrawableCanvas):
+
+ def __init__ (self, master = None):
+ DrawableCanvas.__init__ (self, 22, 22, 0, 0, master, borderwidth = 1,
+ relief = 'sunken', background = 'white')
+ self.area = Area (self)
+
+ def draw (self):
+ self.area.draw ()
+
+class TestTable (Frame):
+
+ def __init__ (self, master = None):
+ Frame.__init__ (self, master)
+ self.pack (expand = True, fill = 'both')
+ self.create_widgets ()
+ self.move = None
+
+ def create_widgets (self):
+ self.right_frame = Frame (self)
+ self.right_frame.pack (side = 'right', fill = 'y')
+ self.quit_button = Button (self.right_frame, text = 'Quit', command =
+ self.quit)
+ self.quit_button.pack (side = 'top', fill = 'x')
+ self.test_var = StringVar ()
+ self.test_var.set ('intersect')
+ self.test_intersect = Radiobutton (self.right_frame,
+ variable = self.test_var, command = self.update,
+ text = 'Intersect', value = 'intersect')
+ self.test_intersect.pack (side = 'top')
+ self.test_nearest = Radiobutton (self.right_frame,
+ variable = self.test_var, command = self.update,
+ text = 'Nearest', value = 'nearest')
+ self.test_nearest.pack (side = 'top')
+ self.new_obstacle_round_frame = LabelFrame (self.right_frame)
+ self.new_obstacle_round_frame.pack (side = 'top', fill = 'x')
+ self.new_obstacle_radius = Scale (self.new_obstacle_round_frame,
+ label = 'Radius', orient = 'horizontal', from_ = 0.5, to = 10,
+ resolution = 0.5)
+ self.new_obstacle_radius.pack (side = 'top')
+ self.new_obstacle_round = Button (self.new_obstacle_round_frame,
+ text = 'New round obstacle',
+ command = self.new_round_obstacle)
+ self.new_obstacle_round_frame.configure (
+ labelwidget = self.new_obstacle_round)
+ self.new_obstacle_rect_frame = LabelFrame (self.right_frame)
+ self.new_obstacle_rect_frame.pack (side = 'top', fill = 'x')
+ self.new_obstacle_dim0 = Scale (self.new_obstacle_rect_frame,
+ label = 'Dim[0]', orient = 'horizontal', from_ = 1, to = 10,
+ resolution = 1)
+ self.new_obstacle_dim0.pack (side = 'top')
+ self.new_obstacle_dim1 = Scale (self.new_obstacle_rect_frame,
+ label = 'Dim[1]', orient = 'horizontal', from_ = 1, to = 10,
+ resolution = 1)
+ self.new_obstacle_dim1.pack (side = 'top')
+ self.new_obstacle_rect = Button (self.new_obstacle_rect_frame,
+ text = 'New rectangular obstacle',
+ command = self.new_rectangular_obstacle)
+ self.new_obstacle_rect_frame.configure (
+ labelwidget = self.new_obstacle_rect)
+ self.area_view = AreaView (self)
+ self.area_view.pack (expand = True, fill = 'both')
+ self.area_view.bind ('<1>', self.click)
+ self.area_view.bind ('<3>', self.rotate)
+
+ def update (self, draw = True):
+ self.area_view.area.update (self.test_var.get ())
+ if draw:
+ self.area_view.draw ()
+
+ def click (self, ev):
+ def move (o, pos):
+ if callable (o):
+ o (pos)
+ else:
+ o.pos = pos
+ pos = vector (self.area_view.screen_coord ((ev.x, ev.y)))
+ if self.move is None:
+ def move_a (pos):
+ self.area_view.area.a = pos
+ def move_b (pos):
+ self.area_view.area.b = pos
+ objs = [ [ self.area_view.area.a, 0.2, move_a ],
+ [ self.area_view.area.b, 0.2, move_b ] ]
+ for o in self.area_view.area.table.obstacles:
+ objs.append ([ o.pos, getattr (o, 'radius', 0.2), o ])
+ for obj in objs:
+ opos = vector (obj[0])
+ radius = obj[1]
+ if abs (opos - pos) < radius:
+ self.move = obj[2]
+ break
+ if self.move is not None:
+ move (self.move, None)
+ else:
+ move (self.move, pos)
+ self.move = None
+ self.update ()
+
+ def rotate (self, ev):
+ pos = vector (self.area_view.screen_coord ((ev.x, ev.y)))
+ for o in self.area_view.area.table.obstacles:
+ if o.pos is None or not hasattr (o, 'angle'):
+ continue
+ opos = vector (o.pos)
+ if abs (opos - pos) < 0.2:
+ o.angle += pi / 4
+ self.update ()
+
+ def new_round_obstacle (self):
+ o = RoundObstacle (self.new_obstacle_radius.get ())
+ o.pos = (5, -5)
+ self.area_view.area.table.obstacles.append (o)
+ self.update ()
+
+ def new_rectangular_obstacle (self):
+ o = RectangularObstacle ((float (self.new_obstacle_dim0.get ()),
+ float (self.new_obstacle_dim1.get ())))
+ o.pos = (5, -5)
+ o.angle = 0
+ self.area_view.area.table.obstacles.append (o)
+ self.update ()
+
+if __name__ == '__main__':
+ app = TestTable ()
+ o = RoundObstacle (2)
+ o.pos = (5, 5)
+ app.area_view.area.table.obstacles.append (o)
+ app.update (False)
+ app.mainloop ()
diff --git a/host/simu/robots/aquajim/link/bag.py b/host/simu/robots/aquajim/link/bag.py
index 3cb9e1b2..f0f96b9d 100644
--- a/host/simu/robots/aquajim/link/bag.py
+++ b/host/simu/robots/aquajim/link/bag.py
@@ -27,7 +27,7 @@ import asserv.mex
class Bag:
- def __init__ (self, node):
- self.asserv = asserv.mex.Mex (node)
- self.io = io.mex.Mex (node)
+ def __init__ (self, node, instance = 'robot0'):
+ self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance)
+ self.io = io.mex.Mex (node, '%s:io0' % instance)
diff --git a/host/simu/robots/giboulee/link/bag.py b/host/simu/robots/giboulee/link/bag.py
index c8c1b285..4dbf0554 100644
--- a/host/simu/robots/giboulee/link/bag.py
+++ b/host/simu/robots/giboulee/link/bag.py
@@ -27,7 +27,7 @@ import asserv.mex
class Bag:
- def __init__ (self, node):
- self.asserv = asserv.mex.Mex (node)
- self.io = io.mex.Mex (node)
+ def __init__ (self, node, instance = 'robot0'):
+ self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance)
+ self.io = io.mex.Mex (node, '%s:io0' % instance)
diff --git a/host/simu/robots/marcel/link/bag.py b/host/simu/robots/marcel/link/bag.py
index 7cb34c28..c0e36dea 100644
--- a/host/simu/robots/marcel/link/bag.py
+++ b/host/simu/robots/marcel/link/bag.py
@@ -28,8 +28,8 @@ import mimot.mex
class Bag:
- def __init__ (self, node):
- self.asserv = asserv.mex.Mex (node)
- self.io = io.mex.Mex (node)
- self.mimot = mimot.mex.Mex (node)
+ def __init__ (self, node, instance = 'robot0'):
+ self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance)
+ self.io = io.mex.Mex (node, '%s:io0' % instance)
+ self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance)
diff --git a/host/simu/robots/marcel/model/loader.py b/host/simu/robots/marcel/model/loader.py
index ae1caa7d..7f767b5a 100644
--- a/host/simu/robots/marcel/model/loader.py
+++ b/host/simu/robots/marcel/model/loader.py
@@ -91,6 +91,7 @@ class Loader (Observable):
limit = (((self.CLAMP_WIDTH - tickness) * 0.5 + 5)
/ self.CLAMP_PULLEY_RADIUS)
for l in self.clamp_link:
+ l.limits.min = 0
l.limits.max = limit
l.limits.notify ()
self.tickness = tickness
diff --git a/host/simu/robots/robospierre/link/bag.py b/host/simu/robots/robospierre/link/bag.py
index ac68889a..e3368d55 100644
--- a/host/simu/robots/robospierre/link/bag.py
+++ b/host/simu/robots/robospierre/link/bag.py
@@ -28,8 +28,8 @@ import mimot.mex
class Bag:
- def __init__ (self, node):
- self.asserv = asserv.mex.Mex (node)
- self.io_hub = io_hub.mex.Mex (node)
- self.mimot = mimot.mex.Mex (node)
+ def __init__ (self, node, instance = 'robot0'):
+ self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance)
+ self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance)
+ self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance)
diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py
index ce55f997..cedb3805 100644
--- a/host/simu/robots/robospierre/model/bag.py
+++ b/host/simu/robots/robospierre/model/bag.py
@@ -28,29 +28,36 @@ from simu.model.motor_basic import MotorBasic
from simu.model.distance_sensor_sensopart import DistanceSensorSensopart
from simu.robots.robospierre.model.clamp import Clamp
from math import pi
+import random
class Bag:
def __init__ (self, scheduler, table, link_bag):
- self.color_switch = Switch (link_bag.io_hub.contact[0])
- self.jack = Switch (link_bag.io_hub.contact[1])
+ self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True)
+ self.color_switch.state = random.choice ((False, True))
+ self.color_switch.notify ()
+ self.jack = Switch (link_bag.io_hub.contact[1], invert = True)
+ self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True)
self.contact = [ Switch (contact)
- for contact in link_bag.io_hub.contact[2:] ]
+ for contact in link_bag.io_hub.contact[2:9] ]
self.position = Position (link_bag.asserv.position)
- self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[0], scheduler,
- 2 * pi, 0, pi)
+ self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler,
+ 8 * pi, 0, pi)
+ self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler,
+ 8 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ]
self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0],
- link_bag.mimot.aux[1], self.clamping_motor)
+ link_bag.mimot.aux[1], self.clamping_motor, self.door_motors,
+ self.contact[0:7], link_bag.io_hub.codebar)
self.distance_sensor = [
DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table,
- (20, -20), -pi * 10 / 180, (self.position, ), 2),
- DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table,
(20, 20), pi * 10 / 180, (self.position, ), 2),
+ DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table,
+ (20, -20), -pi * 10 / 180, (self.position, ), 2),
DistanceSensorSensopart (link_bag.io_hub.adc[2], scheduler, table,
- (-20, 20), pi - pi * 10 / 180, (self.position, ), 2),
- DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table,
(-20, -20), pi + pi * 10 / 180, (self.position, ), 2),
+ DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table,
+ (-20, 20), pi - pi * 10 / 180, (self.position, ), 2),
]
- for adc in link_bag.io_hub.adc[4:]:
- adc.value = 0
+ self.path = link_bag.io_hub.path
+ self.pos_report = link_bag.io_hub.pos_report
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
index ff27a7ea..4bbe988f 100644
--- a/host/simu/robots/robospierre/model/clamp.py
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -24,16 +24,20 @@
"""Robospierre clamp."""
from utils.observable import Observable
from simu.utils.trans_matrix import TransMatrix
+from simu.model.round_obstacle import RoundObstacle
from math import pi, cos, sin
class Slot:
"""Slot which can contain a pawn."""
- def __init__ (self, x, y, z, side):
+ def __init__ (self, x, y, z, side, door_motor, contact, codebar = None):
self.x = x
self.y = y
self.z = z
self.side = side
+ self.door_motor = door_motor
+ self.contact = contact
+ self.codebar = codebar
self.pawn = None
class Clamp (Observable):
@@ -42,7 +46,7 @@ class Clamp (Observable):
ELEVATION_MOTOR_STROKE = 120.0 * 5.0 / 6.0
ROTATION_STROKE = pi
- ROTATION_MOTOR_STROKE = pi * 115.0 / 12.0
+ ROTATION_MOTOR_STROKE = 2 * pi * 36.088 / 16
CLAMPING_STROKE = 10
CLAMPING_MOTOR_STROKE = pi
@@ -59,21 +63,41 @@ class Clamp (Observable):
SLOT_SIDE = 6
def __init__ (self, table, robot_position, elevation_motor,
- rotation_motor, clamping_motor):
+ rotation_motor, clamping_motor, door_motors, slot_contacts,
+ codebars):
Observable.__init__ (self)
self.table = table
self.robot_position = robot_position
self.elevation_motor = elevation_motor
self.rotation_motor = rotation_motor
self.clamping_motor = clamping_motor
+ self.rotation_motor.limits.min = 0
+ self.rotation_motor.limits.notify ()
+ self.door_motors = (door_motors[0], None, door_motors[1],
+ door_motors[2], None, door_motors[3], None)
self.slots = (
- Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0),
- Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0),
- Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0),
- Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1),
- Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1),
- Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1),
- Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None))
+ Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0,
+ door_motors[0], slot_contacts[0], codebars[0]),
+ Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0,
+ None, slot_contacts[1]),
+ Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0,
+ door_motors[1], slot_contacts[2]),
+ Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1,
+ door_motors[2], slot_contacts[3], codebars[1]),
+ Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1,
+ None, slot_contacts[4]),
+ Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1,
+ door_motors[3], slot_contacts[5]),
+ Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None,
+ None, slot_contacts[6]))
+ self.front_slots = (
+ self.slots[self.SLOT_FRONT_BOTTOM],
+ self.slots[self.SLOT_FRONT_MIDDLE],
+ self.slots[self.SLOT_FRONT_TOP])
+ self.back_slots = (
+ self.slots[self.SLOT_BACK_BOTTOM],
+ self.slots[self.SLOT_BACK_MIDDLE],
+ self.slots[self.SLOT_BACK_TOP])
self.load = None
self.robot_position.register (self.__robot_position_notified)
self.elevation_motor.register (self.__elevation_notified)
@@ -81,18 +105,34 @@ class Clamp (Observable):
self.clamping_motor.register (self.__clamping_notified)
def __robot_position_notified (self):
+ # Compute robot direction.
+ direction = self.__get_robot_direction ()
# Update bottom slots.
changed = False
for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM):
slot = self.slots[sloti]
- if slot.pawn is None:
- p = self.__get_floor_elements (slot.side)
- if p is not None:
- slot.pawn = p
- p.pos = None
- p.notify ()
+ if direction == slot.side or direction is None:
+ # If pushing, can take new elements.
+ if slot.pawn is None:
+ p = self.__get_floor_elements (slot.side)
+ if p is not None:
+ slot.pawn = p
+ p.pos = None
+ p.notify ()
+ changed = True
+ else:
+ # Else, can drop elements.
+ if slot.pawn is not None and slot.door_motor.angle:
+ m = TransMatrix ()
+ m.translate (self.robot_position.pos)
+ m.rotate (self.robot_position.angle)
+ xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[slot.side]
+ slot.pawn.pos = m.apply ((xoffset, 0))
+ slot.pawn.notify ()
+ slot.pawn = None
changed = True
if changed:
+ self.update_contacts ()
self.notify ()
def __elevation_notified (self):
@@ -123,29 +163,74 @@ class Clamp (Observable):
if self.clamping == 0 and self.load is None:
# Load an element.
slot = self.__get_clamp_slot ()
- if slot:
- self.load, slot.pawn = slot.pawn, self.load
+ if slot and slot.pawn is not None:
+ self.load = slot.pawn
+ slot.pawn = None
elif self.clamping == self.CLAMPING_STROKE \
and self.load is not None:
# Unload an element.
slot = self.__get_clamp_slot ()
if slot and slot.pawn is None:
- self.load, slot.pawn = slot.pawn, self.load
+ slot.pawn = self.load
+ self.load = None
+ # Little resources saving hack: done here, all motors are notified
+ # at the same time.
+ self.check_tower ()
+ self.update_contacts ()
self.notify ()
+ def check_tower (self):
+ """Check whether several elements can make a tower."""
+ for slots in (self.front_slots, self.back_slots):
+ if slots[0].pawn is not None and slots[1].pawn is not None:
+ assert slots[0].pawn.kind != 'tower'
+ tower = RoundObstacle (100, 1)
+ tower.kind = 'tower'
+ tower.tower = [ slots[0].pawn, slots[1].pawn ]
+ slots[0].pawn, slots[1].pawn = tower, None
+ self.table.add_pawn (tower)
+ if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \
+ and slots[2].pawn and slots[2].door_motor.angle:
+ slots[0].pawn.tower.append (slots[2].pawn)
+ slots[2].pawn = None
+ if slots[0].pawn is None and slots[1].pawn is None \
+ and slots[2].pawn and slots[2].door_motor.angle:
+ slots[0].pawn, slots[2].pawn = slots[2].pawn, None
+
+ def update_contacts (self):
+ """Update pawn contacts."""
+ for slots in (self.front_slots, self.back_slots):
+ slots[0].contact.state = not (slots[0].pawn is not None)
+ # A tower at level 0 is seen at level 1.
+ slots[1].contact.state = not (
+ slots[1].pawn is not None
+ or (slots[0].pawn is not None
+ and slots[0].pawn.kind == 'tower'))
+ slots[2].contact.state = not (slots[2] is not None)
+ if slots[0].pawn:
+ slots[0].codebar.element_type = slots[0].pawn.kind
+ else:
+ slots[0].codebar.element_type = None
+ slots[0].codebar.notify ()
+ slot_side = self.slots[self.SLOT_SIDE]
+ slot_side.contact.state = slot_side.pawn is None
+ clamp_slot = self.__get_clamp_slot ()
+ if clamp_slot is not None and clamp_slot != self.SLOT_FRONT_TOP \
+ and clamp_slot != self.SLOT_BACK_TOP:
+ clamp_slot.contact.state = False
+ for slot in self.slots:
+ slot.contact.notify ()
+
def __get_floor_elements (self, side):
"""Return an elements in front (side = 0) or in back (side = 1) of the
robot, on the floor."""
if self.robot_position.pos is None:
return None
# Matrix to transform an obstacle position into robot coordinates.
- m = TransMatrix ()
- m.rotate (-self.robot_position.angle)
- m.translate ((-self.robot_position.pos[0],
- -self.robot_position.pos[1]))
+ m = self.__get_robot_matrix ()
# Look up elements.
xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side]
- xmargin = 20
+ xmargin = 40
ymargin = 50
for o in self.table.obstacles:
if o.level == 1 and o.pos is not None:
@@ -170,3 +255,34 @@ class Clamp (Observable):
return slot
return None
+ def __get_robot_direction (self):
+ """Is robot going forward (0), backward (1), or something else
+ (None)?"""
+ if self.robot_position.pos is None:
+ return None
+ filt = 5
+ if hasattr (self, 'old_robot_position'):
+ m = self.__get_robot_matrix ()
+ oldrel = m.apply (self.old_robot_position)
+ if oldrel[0] < 0 and self.direction_counter < filt:
+ self.direction_counter += 1
+ elif oldrel[0] > 0 and self.direction_counter > -filt:
+ self.direction_counter -= 1
+ else:
+ self.direction_counter = 0
+ self.old_robot_position = self.robot_position.pos
+ # Filter oscillations.
+ if self.direction_counter > 0:
+ return 0
+ elif self.direction_counter < 0:
+ return 1
+ else:
+ return None
+
+ def __get_robot_matrix (self):
+ """Return robot transformation matrix."""
+ m = TransMatrix ()
+ m.rotate (-self.robot_position.angle)
+ m.translate ((-self.robot_position.pos[0],
+ -self.robot_position.pos[1]))
+ return m
diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py
index 0b5a85ad..d87cecb9 100644
--- a/host/simu/robots/robospierre/view/bag.py
+++ b/host/simu/robots/robospierre/view/bag.py
@@ -35,6 +35,8 @@ class Bag:
self.jack = Switch (sensor_frame, model_bag.jack, 'Jack')
self.color_switch = Switch (sensor_frame, model_bag.color_switch,
'Color')
+ self.strat_switch = Switch (sensor_frame, model_bag.strat_switch,
+ 'Strat')
self.robot = Robot (table, model_bag.position, model_bag.clamp)
self.clamp = (
ClampTop (actuator_view.add_view (ClampTop.width,
@@ -43,4 +45,6 @@ class Bag:
ClampSide.height), model_bag.clamp))
self.distance_sensor = [DistanceSensorUS (self.robot, ds)
for ds in model_bag.distance_sensor]
+ self.path = Path (table, model_bag.path)
+ self.pos_report = PosReport (table, model_bag.pos_report)
diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py
index bd5f6dab..07a10929 100644
--- a/host/simu/robots/robospierre/view/clamp.py
+++ b/host/simu/robots/robospierre/view/clamp.py
@@ -68,7 +68,7 @@ class ClampTop (Drawable):
self.trans_push ()
self.trans_scale (1 - slot.z / 1000.0)
self.trans_translate ((slot.x, slot.y))
- draw_pawn (self, slot.pawn.radius, slot.pawn.kind)
+ draw_pawn (self, slot.pawn)
self.trans_pop ()
# Draw clamp.
if self.model.rotation is not None:
@@ -82,7 +82,7 @@ class ClampTop (Drawable):
if load is not None:
self.trans_push ()
self.trans_translate ((150, 0))
- draw_pawn (self, load.radius, load.kind)
+ draw_pawn (self, load)
self.trans_pop ()
# Mobile side.
self.trans_rotate (-self.model.clamping / 43)
@@ -107,16 +107,23 @@ class ClampSide (Drawable):
if pawn is not None:
self.trans_push ()
self.trans_translate (pos)
- self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW)
- if pawn.kind == 'king':
- self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), (-50, 190),
- (-10, 190), (-10, 230), (10, 230), (10, 190), (50, 190),
- (50, 170), (5, 170), (50, 50), fill = YELLOW,
- outline = BLACK)
- elif pawn.kind == 'queen':
- self.draw_polygon ((-50, 50), (-10, 180), (10, 180), (50, 50),
- fill = YELLOW, outline = BLACK)
- self.draw_circle ((0, 180), 50, fill = YELLOW)
+ if pawn.kind == 'tower':
+ pawns = pawn.tower
+ else:
+ pawns = (pawn, )
+ for p in pawns:
+ self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW)
+ if p.kind == 'king':
+ self.draw_polygon ((-50, 50), (-10, 170), (-50, 170),
+ (-50, 190), (-10, 190), (-10, 230), (10, 230),
+ (10, 190), (50, 190), (50, 170), (5, 170),
+ (50, 50), fill = YELLOW,
+ outline = BLACK)
+ elif p.kind == 'queen':
+ self.draw_polygon ((-50, 50), (-10, 180), (10, 180),
+ (50, 50), fill = YELLOW, outline = BLACK)
+ self.draw_circle ((0, 180), 50, fill = YELLOW)
+ self.trans_translate ((0, 50))
self.trans_pop ()
def draw (self):
@@ -133,6 +140,7 @@ class ClampSide (Drawable):
self.draw_pawn ((-slot.x, slot.z), slot.pawn)
# Draw clamp.
if self.model.rotation is not None:
+ self.trans_push ()
self.trans_translate ((0, self.model.elevation))
m = TransMatrix ()
m.rotate (pi + self.model.rotation)
@@ -169,4 +177,13 @@ class ClampSide (Drawable):
self.draw_pawn ((lcenter, 0), self.model.load)
self.draw_rectangle ((lbase, 10), (lcenter - 103, 40), **attr)
self.draw_rectangle ((rbase, 10), (rtip, 40), **attr)
+ self.trans_pop ()
+ # Draw doors.
+ for slot in self.model.slots:
+ if slot.door_motor is not None:
+ self.trans_push ()
+ self.trans_translate ((-slot.x, slot.z + 50))
+ self.trans_rotate (-0.5 * pi + slot.door_motor.angle)
+ self.draw_line ((0, 0), (40, 0))
+ self.trans_pop ()
diff --git a/host/simu/robots/robospierre/view/robot.py b/host/simu/robots/robospierre/view/robot.py
index c0f6624c..90624000 100644
--- a/host/simu/robots/robospierre/view/robot.py
+++ b/host/simu/robots/robospierre/view/robot.py
@@ -63,7 +63,7 @@ class Robot (simu.inter.drawable.Drawable):
self.trans_push ()
self.trans_scale (1 - slot.z / 1000.0)
self.trans_translate ((slot.x, slot.y))
- draw_pawn (self, slot.pawn.radius, slot.pawn.kind)
+ draw_pawn (self, slot.pawn)
self.trans_pop ()
# Draw clamp.
if self.clamp_model.rotation is not None:
@@ -77,7 +77,7 @@ class Robot (simu.inter.drawable.Drawable):
if load:
self.trans_push ()
self.trans_translate ((150, 0))
- draw_pawn (self, load.radius, load.kind)
+ draw_pawn (self, load)
self.trans_pop ()
# Mobile side.
self.trans_rotate (- self.clamp_model.clamping / 47)
diff --git a/host/simu/utils/intersect.py b/host/simu/utils/intersect.py
new file mode 100644
index 00000000..6074dda0
--- /dev/null
+++ b/host/simu/utils/intersect.py
@@ -0,0 +1,103 @@
+# simu - Robot simulation. {{{
+# encoding: utf-8
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Intersection between different kind of primitives."""
+from simu.utils.vector import vector
+
+def segment_segment (a, b, c, d):
+ """Find intersection between [AB] and [CD] line segments. Return i such
+ that A + i * (B - A).unit () gives this intersection. Return None if no
+ intersection found.
+
+ If line segments are parallel or collinear, None is returned.
+ """
+ a, b, c, d = vector (a), vector (b), vector (c), vector (d)
+ # For each point P on the line segment [AB], there is a real u in [0, 1]
+ # for which P = A + u (B - A)
+ #
+ # An intersection point must be on both line segments:
+ #
+ # A + u (B - A) = C + v (D - C)
+ #
+ # xa + u (xb - xa) = xc + v (xd - xc)
+ # ya + u (yb - ya) = yc + v (yd - yc)
+ #
+ # (xc - xa) (yd - yc) - (xd - xc) (yc - ya)
+ # u = -----------------------------------------
+ # (xb - xa) (yd - yc) - (xd - xc) (yb - ya)
+ # (xc - xa) (yb - ya) - (xb - xa) (yc - ya)
+ # v = -----------------------------------------
+ # (xb - xa) (yd - yc) - (xd - xc) (yb - ya)
+ #
+ # u = (vac . vcd.normal()) / (vab . vcd.normal())
+ # v = (vac . vab.normal()) / (vab . vcd.normal())
+ #
+ # If vab . vcd.normal () is 0, AB and CD are parallel.
+ vab = b - a
+ vcd = d - c
+ vac = c - a
+ # Cannot test for 0 because we are using float, cannot test for a very
+ # small number because we do not know what is small enough, therefore,
+ # compare with numerator.
+ den = vab * vcd.normal ()
+ unum = vac * vcd.normal ()
+ if abs (den) <= 1e-6 * abs (unum):
+ return None
+ else:
+ u = unum / den
+ if u >= 0 and u <= 1:
+ v = vac * vab.normal () / den
+ if v >= 0 and v <= 1:
+ return u * vab.norm ()
+ return None
+
+if __name__ == '__main__':
+ import sys
+ import math
+ v00 = vector (0, 0)
+ v02 = vector (0, 2)
+ v20 = vector (2, 0)
+ v22 = vector (2, 2)
+ v44 = vector (4, 4)
+ failed = 0
+ verbose = True
+ def check (test, result):
+ r = eval (test)
+ if r != result:
+ print test, 'is', r, 'but expected', result
+ return 1
+ elif verbose:
+ print test, 'is', r, 'as expected'
+ return 0
+ v2 = math.sqrt (2)
+ failed += check ('segment_segment (v00, v22, v20, v02)', v2)
+ failed += check ('segment_segment (v00, v22, v02, v20)', v2)
+ failed += check ('segment_segment (v22, v00, v20, v02)', v2)
+ failed += check ('segment_segment (v22, v00, v02, v20)', v2)
+ failed += check ('segment_segment (v00, v22, v00, v22)', None)
+ failed += check ('segment_segment (v00, v22, v00, v44)', None)
+ failed += check ('segment_segment (v22, v44, v02, v20)', None)
+ failed += check ('segment_segment (v00, v44, v02, v20)', v2)
+ failed += check ('segment_segment (v44, v00, v02, v20)', 3 * v2)
+ sys.exit (0 if not failed else 1)
diff --git a/host/simu/utils/vector.py b/host/simu/utils/vector.py
new file mode 100644
index 00000000..e5a8bd05
--- /dev/null
+++ b/host/simu/utils/vector.py
@@ -0,0 +1,267 @@
+# simu - Robot simulation. {{{
+# encoding: utf-8
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""2D vector."""
+import math
+
+__all__ = ('vector')
+
+class vector (object):
+ """2D vector, with useful operators."""
+
+ __slots__ = ('x', 'y')
+
+ def __init__ (self, *xy):
+ """Initialise the vector with given coordinates.
+
+ >>> vector (1, 2)
+ vector(1.0, 2.0)
+ >>> vector ((1, 2))
+ vector(1.0, 2.0)
+ >>> vector (vector (1, 2))
+ vector(1.0, 2.0)
+ >>> vector ("hello")
+ Traceback (most recent call last):
+ ...
+ TypeError
+ >>> vector ("ab")
+ Traceback (most recent call last):
+ ...
+ ValueError: invalid literal for float(): a
+ """
+ if len (xy) == 2:
+ self.x, self.y = float (xy[0]), float (xy[1])
+ elif len (xy) == 1:
+ v = xy[0]
+ if isinstance (v, vector):
+ self.x, self.y = v.x, v.y
+ elif len (v) == 2:
+ self.x, self.y = float (v[0]), float (v[1])
+ else:
+ raise TypeError
+ else:
+ raise TypeError
+
+ def __len__ (self):
+ """Return vector length, always 2.
+
+ >>> len (vector (1, 2))
+ 2
+ """
+ return 2
+
+ def __repr__ (self):
+ """Return text representation.
+
+ >>> vector (1, 2)
+ vector(1.0, 2.0)
+ """
+ return 'vector(%r, %r)' % (self.x, self.y)
+
+ def __str__ (self):
+ """Return informal text representation.
+
+ >>> str (vector (1, 2))
+ '(1.0, 2.0)'
+ """
+ return '(%s, %s)' % (self.x, self.y)
+
+ def __getitem__ (self, index):
+ """Return a coordinate by its index, for compatibility with
+ sequences."""
+ return (self.x, self.y)[index]
+
+ def __eq__ (self, other):
+ """Test for equality.
+
+ >>> vector (1, 2) == vector (1, 3)
+ False
+ >>> vector (1, 2) == vector (2, 2)
+ False
+ >>> vector (1, 2) == vector (1, 2)
+ True
+ """
+ return (self.x, self.y) == (other.x, other.y)
+
+ def __ne__ (self, other):
+ """Test for inequality.
+
+ >>> vector (1, 2) != vector (1, 2)
+ False
+ """
+ return not (self == other)
+
+ def __cmp__ (self, other):
+ """No comparison apart from equality test.
+
+ >>> vector (1, 2) < vector (1, 2)
+ Traceback (most recent call last):
+ ...
+ TypeError
+ """
+ raise TypeError
+
+ def __add__ (self, other):
+ """Addition.
+
+ >>> vector (1, 2) + vector (3, 4)
+ vector(4.0, 6.0)
+ """
+ return vector (self.x + other.x, self.y + other.y)
+
+ def __sub__ (self, other):
+ """Subtraction.
+
+ >>> vector (1, 2) - vector (3, 4)
+ vector(-2.0, -2.0)
+ """
+ return vector (self.x - other.x, self.y - other.y)
+
+ def __mul__ (self, other):
+ """Multiplication or dot product.
+
+ >>> vector (1, 2) * 2
+ vector(2.0, 4.0)
+ >>> 3 * vector (1, 2)
+ vector(3.0, 6.0)
+ >>> vector (1, 2) * vector (3, 4)
+ 11.0
+ """
+ if isinstance (other, vector):
+ return self.x * other.x + self.y * other.y
+ else:
+ return vector (self.x * other, self.y * other)
+ __rmul__ = __mul__
+
+ def __div__ (self, other):
+ """Division.
+
+ >>> vector (1, 2) / 2
+ vector(0.5, 1.0)
+ >>> vector (1, 2) / vector (3, 4)
+ Traceback (most recent call last):
+ ...
+ TypeError: unsupported operand type(s) for /: 'float' and 'vector'
+ >>> 3.0 / vector (1, 2)
+ Traceback (most recent call last):
+ ...
+ TypeError: unsupported operand type(s) for /: 'float' and 'vector'
+ """
+ return vector (self.x / other, self.y / other)
+ __truediv__ = __div__
+
+ def __neg__ (self):
+ """Negate.
+
+ >>> -vector (1.0, 2.0)
+ vector(-1.0, -2.0)
+ """
+ return vector (-self.x, -self.y)
+
+ def __pos__ (self):
+ """No-op.
+
+ >>> +vector (1.0, 2.0)
+ vector(1.0, 2.0)
+ """
+ return self
+
+ def __abs__ (self):
+ """Norm.
+
+ >>> abs (vector (3.0, 4.0))
+ 5.0
+ >>> vector (3.0, 4.0).norm ()
+ 5.0
+ """
+ return math.hypot (self.x, self.y)
+ norm = __abs__
+
+ def norm_squared (self):
+ """Norm squared.
+
+ >>> vector (3.0, 4.0).norm_squared ()
+ 25.0
+ """
+ return self.x ** 2 + self.y ** 2
+
+ def unit (self):
+ """Return normalized vector.
+
+ >>> print vector (3.0, 4.0).unit ()
+ (0.6, 0.8)
+ """
+ norm = math.hypot (self.x, self.y)
+ return vector (self.x / norm, self.y / norm)
+
+ def normal (self):
+ """Return the vector rotated by pi/2.
+
+ >>> vector (1, 2).normal ()
+ vector(-2.0, 1.0)
+ """
+ return vector (-self.y, self.x)
+
+ def angle (self):
+ """Return angle from (1, 0).
+
+ >>> vector (1, 1).angle ()
+ 0.78539816339744828
+ >>> vector (0, -1).angle ()
+ -1.5707963267948966
+ >>> vector (-1, -1).angle ()
+ -2.3561944901923448
+ """
+ return math.atan2 (self.y, self.x)
+
+ @staticmethod
+ def polar (angle, norm):
+ """Return a vector constructed from polar coordinates (angle, norm).
+
+ >>> print vector.polar (math.pi / 4, math.sqrt (2))
+ (1.0, 1.0)
+ """
+ return vector (norm * math.cos (angle), norm * math.sin (angle))
+
+if __name__ == '__main__':
+ def _test ():
+ import doctest
+ doctest.testmod ()
+ def _perf_test ():
+ import timeit
+ def do (title, stmt, setup = None):
+ n = 10000
+ if setup is None:
+ setup = 'import vector; v = vector.vector (1.2, 3.4)'
+ t = timeit.timeit (stmt, setup, number = n)
+ print title, '%.3f µs' % (1e6 * t / n)
+ do ('init from floats', 'v = vector.vector (1.2, 3.4)')
+ do ('init from tuple', 'v = vector.vector ((1.2, 3.4))')
+ do ('init from vector', 'v2 = vector.vector (v)')
+ do ('init from polar', 'v = vector.vector.polar (1.0, 1.0)')
+ do ('abs', 'abs (v)')
+ do ('norm_squared', 'v.norm_squared()')
+ do ('unit', 'v.unit()')
+ _test()
+ _perf_test ()
diff --git a/host/simu/view/switch.py b/host/simu/view/switch.py
index c0ec2d58..445cbd02 100644
--- a/host/simu/view/switch.py
+++ b/host/simu/view/switch.py
@@ -28,6 +28,8 @@ class Switch:
def __init__ (self, frame, model, text):
self.var = IntVar ()
+ if model.state is not None:
+ self.var.set ((0, 1)[model.state])
self.button = Checkbutton (frame, variable = self.var,
command = self.__update, text = text, indicatoron = False)
self.button.pack ()
diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py
index f0a65ce1..b39f2834 100644
--- a/host/simu/view/table_eurobot2011.py
+++ b/host/simu/view/table_eurobot2011.py
@@ -31,17 +31,25 @@ GREEN = '#268126'
BLACK = '#181818'
YELLOW = '#cccc00'
-def draw_pawn (d, radius, kind):
- d.draw_circle ((0, 0), radius, fill = YELLOW)
- if kind == 'king':
- a = 0.1 * radius
- b = 0.5 * radius
- d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b),
- (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b),
- (a, b))
- elif kind == 'queen':
- d.draw_circle ((0, 0), 0.5 * radius)
- d.draw_circle ((0, 0), 0.4 * radius)
+def draw_pawn (d, pawn):
+ d.trans_push ()
+ if pawn.kind == 'tower':
+ pawns = pawn.tower
+ else:
+ pawns = (pawn, )
+ for p in pawns:
+ d.draw_circle ((0, 0), p.radius, fill = YELLOW)
+ if p.kind == 'king':
+ a = 0.1 * p.radius
+ b = 0.5 * p.radius
+ d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b),
+ (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b),
+ (a, b))
+ elif p.kind == 'queen':
+ d.draw_circle ((0, 0), 0.5 * p.radius)
+ d.draw_circle ((0, 0), 0.4 * p.radius)
+ d.trans_scale (0.95)
+ d.trans_pop ()
class Pawn (Drawable):
@@ -59,7 +67,7 @@ class Pawn (Drawable):
self.reset ()
if self.pos:
self.trans_translate (self.pos)
- draw_pawn (self, self.model.radius, self.model.kind)
+ draw_pawn (self, self.model)
Drawable.draw (self)
class Table (Drawable):
@@ -68,8 +76,13 @@ class Table (Drawable):
def __init__ (self, onto, model):
Drawable.__init__ (self, onto)
self.model = model
- for e in model.pawns:
- Pawn (self, e)
+ self.model.register (self.__notified)
+ self.__notified ()
+
+ def __notified (self):
+ for e in self.model.pawns:
+ if e not in self.children:
+ Pawn (self, e)
def draw_both (self, primitive, *args, **kargs):
"""Draw a primitive on both sides."""
diff --git a/host/simu/view/table_eurobot2012.py b/host/simu/view/table_eurobot2012.py
new file mode 100644
index 00000000..89ada042
--- /dev/null
+++ b/host/simu/view/table_eurobot2012.py
@@ -0,0 +1,189 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""Eurobot 2012 table."""
+from simu.inter.drawable import Drawable
+import math
+from math import pi
+
+PURPLE = '#a737ff'
+RED = '#fe4543'
+BLUE = '#01c3ff'
+GREEN = '#81ff00'
+BLACK = '#1f1b1d'
+YELLOW = '#f7ff00'
+WHITE = '#f5fef2'
+TRANS = '#c0c2b5'
+BROWN = '#9d4e01'
+
+def draw_coin (d, coin):
+ d.trans_push ()
+ color = WHITE if coin.value else BLACK
+ d.draw_circle ((0, 0), coin.radius, fill = color)
+ d.draw_circle ((0, 0), 7.5, fill = color)
+ d.trans_translate ((40.5, 0))
+ d.draw_rectangle ((-9, -9), (9, 9), fill = color, outline = TRANS)
+ d.trans_pop ()
+
+class Coin (Drawable):
+
+ def __init__ (self, onto, model):
+ Drawable.__init__ (self, onto)
+ self.model = model
+ self.model.register (self.__notified)
+ self.__notified ()
+
+ def __notified (self):
+ self.pos = self.model.pos
+ self.angle = self.model.angle
+ self.update ()
+
+ def draw (self):
+ self.reset ()
+ if self.pos:
+ self.trans_translate (self.pos)
+ self.trans_rotate (self.angle)
+ draw_coin (self, self.model)
+ Drawable.draw (self)
+
+def draw_gold_bar (d, gold_bar):
+ d.draw_rectangle ((-gold_bar.dim[0] / 2, -gold_bar.dim[1] / 2),
+ (gold_bar.dim[0] / 2, gold_bar.dim[1] / 2), fill = YELLOW)
+ d.draw_rectangle ((-gold_bar.dim[0] / 2 + 13, -gold_bar.dim[1] / 2 + 13),
+ (gold_bar.dim[0] / 2 - 13, gold_bar.dim[1] / 2 - 13), fill = YELLOW)
+
+class GoldBar (Drawable):
+
+ def __init__ (self, onto, model):
+ Drawable.__init__ (self, onto)
+ self.model = model
+ self.model.register (self.__notified)
+ self.__notified ()
+
+ def __notified (self):
+ self.pos = self.model.pos
+ self.angle = self.model.angle
+ self.update ()
+
+ def draw (self):
+ self.reset ()
+ if self.pos:
+ self.trans_translate (self.pos)
+ self.trans_rotate (self.angle)
+ draw_gold_bar (self, self.model)
+ Drawable.draw (self)
+
+class Table (Drawable):
+ """The table and its elements."""
+
+ def __init__ (self, onto, model):
+ Drawable.__init__ (self, onto)
+ self.model = model
+ for e in self.model.coins:
+ if e.level <= 2:
+ Coin (self, e)
+ for e in self.model.gold_bars:
+ GoldBar (self, e)
+ for e in self.model.coins:
+ if e.level > 2:
+ Coin (self, e)
+
+ def draw_both (self, primitive, *args, **kargs):
+ """Draw a primitive on both sides."""
+ primitive (*args, **kargs)
+ primitive (*((3000 - x, y) for x, y in args), **kargs)
+
+ def draw (self):
+ # Redraw.
+ self.reset ()
+ # Table.
+ self.draw_rectangle ((-22, -22), (3000 + 22, 2000 + 22), fill = BLUE)
+ self.draw_rectangle ((0, 0), (3000, 2000), fill = BLUE)
+ # Start zones.
+ self.draw_rectangle ((0, 2000 - 500), (500, 2000), fill = PURPLE)
+ self.draw_rectangle ((3000, 2000 - 500), (3000 - 500, 2000), fill = RED)
+ # Black lines.
+ pup = 2000 - 500 + 50
+ self.draw_both (self.draw_polygon, (500, pup), (650, pup), (650, 0), (630, 0),
+ (630, pup - 20), (500, pup - 20), fill = BLACK, outline = 'black')
+ # Ships.
+ ba = 0.04995839
+ self.draw_both (self.draw_polygon, (0, 2000 - 500), (400, 2000 - 500), (325, 0), (0, 0),
+ fill = BROWN, outline = 'black')
+ self.draw_both (self.draw_rectangle, (0, 2000 - 500), (400, 2000 - 500 - 18), fill = BROWN)
+ cba = 750 * math.cos (ba)
+ sba = 750 * math.sin (ba)
+ self.draw_both (self.draw_polygon, (325, 0), (325 + sba, cba), (325 + sba + 18, cba), (325 + 18, 0),
+ fill = BROWN, outline = 'black')
+ self.draw_both (self.draw_rectangle, (-22, -22), (340, 610 - 22), fill = TRANS)
+ self.draw_both (self.draw_rectangle, (-4, -4), (340 - 18, 610 - 22 - 18), fill = TRANS)
+ # Peanut island.
+ self.draw_both (self.draw_circle, (1500 - 400, 1000), r = 300, fill = YELLOW)
+ self.draw_rectangle ((1500 - 400 + 141, 1000 + 265), (1500 + 400 - 141, 1000 - 265), fill = YELLOW, outline = '')
+ self.draw_arc ((1500, 1000 + 750), 550, start = pi + 1, extent = pi - 2, fill = BLUE, outline = '')
+ self.draw_arc ((1500, 1000 + 750), 550, start = pi + 1.08083, extent = pi - 2 * 1.08083, style = 'arc')
+ self.draw_arc ((1500, 1000 - 750), 550, start = 1, extent = pi - 2, fill = BLUE, outline = '')
+ self.draw_arc ((1500, 1000 - 750), 550, start = 1.08083, extent = pi - 2 * 1.08083, style = 'arc')
+ self.draw_both (self.draw_circle, (1500 - 400, 1000), r = 200, fill = GREEN)
+ self.draw_circle ((1500, 1000), r = 75, fill = GREEN)
+ self.draw_both (self.draw_rectangle, (1500 - 400 - 125, 1000 - 125), (1500 - 400 + 125, 1000 + 125), fill = BROWN)
+ # Map island.
+ self.draw_arc ((1500, 2000), 400, start = pi, extent = pi, fill = YELLOW)
+ self.draw_arc ((1500, 2000), 300, start = pi, extent = pi, fill = GREEN)
+ # Map.
+ self.draw_rectangle ((1500 - 200 - 22, 2000 + 148), (1500 + 200 + 22, 2000 + 140), fill = BLUE)
+ self.draw_rectangle ((1500 - 200, 2000 + 140), (1500 + 200, 2000 - 8), fill = BLUE)
+ self.draw_both (self.draw_rectangle, (1500 - 200 - 22, 2000 + 140), (1500 - 200, 2000 + 125), fill = BLUE)
+ self.draw_both (self.draw_rectangle, (1500 - 200 - 22, 2000 + 125), (1500 - 200, 2000 - 23), fill = BLUE)
+ self.draw_rectangle ((1500 - 22, 2000 + 140), (1500 + 22, 2000 + 125), fill = BLUE)
+ self.draw_rectangle ((1500 - 22, 2000 + 125), (1500 + 22, 2000 + 115), fill = BLUE)
+ self.draw_rectangle ((1500 - 22, 2000 + 2), (1500 + 22, 2000 - 13), fill = BLUE)
+ self.draw_rectangle ((1500 - 22, 2000 - 13), (1500 + 22, 2000 - 23), fill = BLUE)
+ # Bottles.
+ def draw_bottle (x, color):
+ self.draw_rectangle ((x - 100, 0), (x + 100, -44), fill = color)
+ self.draw_rectangle ((x - 100, -44), (x + 100, -66), fill = color)
+ self.draw_rectangle ((x - 11, 44), (x + 11, 0), fill = color)
+ self.draw_rectangle ((x - 50, 22), (x + 100, -44), fill = color)
+ self.draw_polygon ((x - 50, 22), (x - 80, 0), (x - 100, 0), (x - 100, -22), (x - 80, -22), (x - 50, -44),
+ fill = color, outline = 'black')
+ draw_bottle (640, PURPLE)
+ draw_bottle (640 + 477, RED)
+ draw_bottle (3000 - 640, RED)
+ draw_bottle (3000 - 640 - 477, PURPLE)
+ # Axes.
+ self.draw_line ((0, 200), (0, 0), (200, 0), arrow = 'both')
+ # Beacons.
+ self.draw_both (self.draw_rectangle, (-22, 2000 + 22), (-22 - 80, 2000 + 22 + 80), fill = BLACK)
+ self.draw_both (self.draw_rectangle, (-22, 1000 - 40), (-22 - 80, 1000 + 40), fill = BLACK)
+ self.draw_both (self.draw_rectangle, (-22, -80 - 22), (-22 - 80, -22), fill = BLACK)
+ # Children.
+ Drawable.draw (self)
+
+if __name__ == '__main__':
+ from simu.inter.inter import Inter
+ import simu.model.table_eurobot2012 as model
+ app = Inter ()
+ m = model.Table ()
+ Table (app.table_view, m)
+ app.mainloop ()
diff --git a/host/utils/init_proto.py b/host/utils/init_proto.py
index f5c2bfa4..8125543c 100644
--- a/host/utils/init_proto.py
+++ b/host/utils/init_proto.py
@@ -3,34 +3,54 @@ import proto.popen_io
import serial
import optparse
-def init_proto (default_robot, proto_class, init_module = None, init = None):
+class InitProto:
"""Helper to create a Proto instance from command line arguments."""
- if init_module is None and init is None:
- init = { }
- # Parse arguments.
- parser = optparse.OptionParser (
- usage = "usage: %prog [options] TTY|! PROGRAM...",
- description = "TTY is a device name (example: %prog "
- "/dev/ttyUSB0), PROGRAM is a host program with its arguments "
- "(example: %prog -- ! ../src/board.host board_arg).")
- if init_module:
- parser.add_option ('-r', '--robot', help = "use specified robot",
- metavar = 'NAME', default = default_robot)
- (options, args) = parser.parse_args ()
- if init_module and options.robot is None:
- parser.error ("no robot specified")
- if not args:
- parser.error ("not enough arguments")
- # Create parameters.
- if args[0] == '!':
- io = proto.popen_io.PopenIO (args[1:])
- if init_module:
- init = init_module.host[options.robot]
- else:
- if len (args) != 1:
- parser.error ("too many arguments after device")
- io = serial.Serial (args[0])
+
+ def __init__ (self, default_robot, proto_class, init_module = None,
+ init = None):
+ """Initialise helper."""
+ if init_module is None and init is None:
+ init = { }
+ self.proto_class = proto_class
+ self.init_module = init_module
+ self.init = init
+ # Prepare parser.
+ self.parser = optparse.OptionParser (
+ usage = "usage: %prog [options] TTY|! PROGRAM...",
+ description = "TTY is a device name (example: %prog "
+ "/dev/ttyUSB0), PROGRAM is a host program with its arguments "
+ "(example: %prog -- ! ../src/board.host board_arg).")
if init_module:
- init = init_module.target[options.robot]
- return proto_class (io, **init)
+ self.parser.add_option ('-r', '--robot',
+ help = "use specified robot",
+ metavar = 'NAME', default = default_robot)
+
+ def parse_args (self):
+ """Parse command line."""
+ (self.options, self.args) = self.parser.parse_args ()
+ if self.init_module and self.options.robot is None:
+ self.parser.error ("no robot specified")
+
+ def get_proto (self):
+ """Return the Proto instance."""
+ if not self.args:
+ self.parser.error ("not enough arguments")
+ # Create parameters.
+ if self.args[0] == '!':
+ io = proto.popen_io.PopenIO (self.args[1:])
+ if self.init_module:
+ self.init = self.init_module.host[self.options.robot]
+ else:
+ if len (self.args) != 1:
+ self.parser.error ("too many arguments after device")
+ io = serial.Serial (self.args[0])
+ if self.init_module:
+ self.init = self.init_module.target[self.options.robot]
+ return self.proto_class (io, **self.init)
+
+def init_proto (default_robot, proto_class, init_module = None, init = None):
+ """Helper to create a Proto instance from command line arguments."""
+ ip = InitProto (default_robot, proto_class, init_module, init)
+ ip.parse_args ()
+ return ip.get_proto ()