From 233c384c6c53f91e1405d797d9683f3296ddabc1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 Mar 2009 15:57:17 +0100 Subject: * host/inter, host/simu: - moved drawable and trans_matrix to the new directory structure. - new drawable behaviour. --- host/inter/dist_sensor.py | 3 +- host/inter/drawable.py | 172 ---------------------------- host/inter/inter.py | 60 ++++------ host/inter/inter_node.py | 17 ++- host/inter/path.py | 2 +- host/inter/test/test_drawable.py | 65 ----------- host/inter/trans_matrix.py | 174 ---------------------------- host/simu/__init__.py | 0 host/simu/inter/__init__.py | 0 host/simu/inter/drawable.py | 210 ++++++++++++++++++++++++++++++++++ host/simu/inter/test/test_drawable.py | 65 +++++++++++ host/simu/utils/__init__.py | 0 host/simu/utils/trans_matrix.py | 174 ++++++++++++++++++++++++++++ 13 files changed, 479 insertions(+), 463 deletions(-) delete mode 100644 host/inter/drawable.py delete mode 100644 host/inter/test/test_drawable.py delete mode 100644 host/inter/trans_matrix.py create mode 100644 host/simu/__init__.py create mode 100644 host/simu/inter/__init__.py create mode 100644 host/simu/inter/drawable.py create mode 100644 host/simu/inter/test/test_drawable.py create mode 100644 host/simu/utils/__init__.py create mode 100644 host/simu/utils/trans_matrix.py diff --git a/host/inter/dist_sensor.py b/host/inter/dist_sensor.py index 859b311f..1f6a4668 100644 --- a/host/inter/dist_sensor.py +++ b/host/inter/dist_sensor.py @@ -25,7 +25,7 @@ from math import pi, cos, sin, sqrt from Tkinter import * -from drawable import * +from simu.inter.drawable import * from utils.observable import Observable class DistSensor (Drawable, Observable): @@ -34,6 +34,7 @@ class DistSensor (Drawable, Observable): def __init__ (self, onto, pos, angle, range): Drawable.__init__ (self, onto) Observable.__init__ (self) + self.onto = onto self.pos = pos self.angle = angle self.range = range diff --git a/host/inter/drawable.py b/host/inter/drawable.py deleted file mode 100644 index 81331548..00000000 --- a/host/inter/drawable.py +++ /dev/null @@ -1,172 +0,0 @@ -# inter - Robot simulation interface. {{{ -# -# Copyright (C) 2008 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. -# -# }}} -"""Drawable and DrawableCanvas.""" -from math import sqrt, degrees -import trans_matrix -import Tkinter - -__all__ = ('Drawable', 'DrawableCanvas') - -class Drawable: - """Define a drawable area with embedded transformations.""" - - def __init__ (self, onto): - """Initialise the drawable.""" - self.onto = onto - self.trans_matrix = trans_matrix.TransMatrix () - self.items = [ ] - self.trans_apply = self.trans_matrix.apply - self.trans_apply_angle = self.trans_matrix.apply_angle - self.trans_apply_distance = self.trans_matrix.apply_distance - self.trans_rotate = self.trans_matrix.rotate - self.trans_translate = self.trans_matrix.translate - self.trans_scale = self.trans_matrix.scale - - def __draw_rectangle (self, p1, p2, **kw): - if 'outline' not in kw: - kw = kw.copy () - kw['outline'] = 'black' - p = self.trans_apply (p1, (p2[0], p1[1]), p2, (p1[0], p2[1])) - return self.onto.__draw_polygon (*p, **kw) - - def __draw_line (self, *p, **kw): - p = self.trans_apply (*p) - return self.onto.__draw_line (*p, **kw) - - def __draw_polygon (self, *p, **kw): - p = self.trans_apply (*p) - return self.onto.__draw_polygon (*p, **kw) - - def __draw_circle (self, p, r, **kw): - p = self.trans_apply (p) - r = self.trans_apply_distance (r) - return self.onto.__draw_circle (p, r, **kw) - - def __draw_arc (self, p, r, **kw): - p = self.trans_apply (p) - r = self.trans_apply_distance (r) - if 'start' in kw: - kw = kw.copy () - kw['start'] = self.trans_apply_angle (kw['start']) - import math - return self.onto.__draw_arc (p, r, **kw) - - def draw_rectangle (self, *p, **kw): - """Draw a rectangle.""" - self.items.append (self.__draw_rectangle (*p, **kw)) - - def draw_line (self, *p, **kw): - """Draw a line.""" - self.items.append (self.__draw_line (*p, **kw)) - - def draw_polygon (self, *p, **kw): - """Draw a line.""" - self.items.append (self.__draw_polygon (*p, **kw)) - - def draw_circle (self, p, r, **kw): - """Draw a circle of the given radius centered on p.""" - self.items.append (self.__draw_circle (p, r, **kw)) - - def draw_arc (self, p, r, **kw): - """Draw a arc of the given radius centered on p.""" - self.items.append (self.__draw_arc (p, r, **kw)) - - def trans_reset (self): - """Reset transformations.""" - self.trans_matrix.identity () - - def reset (self): - self.__delete (*self.items) - self.items = [ ] - self.trans_reset () - - def __delete (self, *list): - """Delete a list of items.""" - self.onto.__delete (*list) - - -class DrawableCanvas(Tkinter.Canvas): - """Extend a Tkinter.Canvas to use Drawable on it. User should implement - the draw method.""" - - def __init__ (self, width, height, xorigin, yorigin, master = None, **kw): - """Initialise a DrawableCanvas. The width and height parameters - define the requested drawable area virtual size. The xorigin and - yorigin parameters define origin of the virtual coordinates relative - to the drawable center.""" - Tkinter.Canvas.__init__ (self, master, **kw) - self.__width = width - self.__height = height - self.__xorigin = xorigin - self.__yorigin = yorigin - self.bind ('', self.__resize) - - def __resize (self, ev): - # Compute new scale. - w, h = float (ev.width), float (ev.height) - self.__scale = min (w / self.__width, h / self.__height) - self.__xoffset = w / 2 + self.__xorigin * self.__scale - self.__yoffset = h / 2 - self.__yorigin * self.__scale - # Redraw. - self.draw () - - def _Drawable__draw_line (self, *p, **kw): - p = self.__coord (*p) - return self.create_line (*p, **kw) - - def _Drawable__draw_polygon (self, *p, **kw): - p = self.__coord (*p) - return self.create_polygon (*p, **kw) - - def _Drawable__draw_circle (self, p, r, **kw): - p, = self.__coord (p) - r = r * self.__scale - p1 = (p[0] - r, p[1] - r) - p2 = (p[0] + r, p[1] + r) - return self.create_oval (p1, p2, **kw) - - def _Drawable__draw_arc (self, p, r, **kw): - p, = self.__coord (p) - r = r * self.__scale - p1 = (p[0] - r, p[1] - r) - p2 = (p[0] + r, p[1] + r) - for k in ('start', 'extent'): - if k in kw: - kw = kw.copy () - kw[k] = degrees (kw[k]) - return self.create_arc (p1, p2, **kw) - - def _Drawable__delete (self, *list): - self.delete (*list) - - def __coord (self, *args): - return [ (i[0] * self.__scale + self.__xoffset, - -i[1] * self.__scale + self.__yoffset) for i in args ] - - def screen_coord (self, screen): - """Return drawable coordinates corresponding to the given screen - coordinates.""" - return ((self.canvasx (screen[0]) - self.__xoffset) / self.__scale, - -(self.canvasy (screen[1]) - self.__yoffset) / self.__scale) - diff --git a/host/inter/inter.py b/host/inter/inter.py index 8dedd680..a8270e53 100644 --- a/host/inter/inter.py +++ b/host/inter/inter.py @@ -23,17 +23,13 @@ # }}} """Inter and its childrens.""" from Tkinter import * -from drawable import * +from simu.inter.drawable import * from math import pi, cos, sin class Robot (Drawable): """The robot.""" - def __init__ (self, onto): - Drawable.__init__ (self, onto) - self.drawn = [ ] - def draw (self): self.reset () self.trans_rotate (self.angle) @@ -48,8 +44,7 @@ class Robot (Drawable): self.draw_line ((0, +f / 2), (0, -f / 2), fill = axes_fill) self.draw_line ((-wr, f / 2), (+wr, f / 2), fill = axes_fill) self.draw_line ((-wr, -f / 2), (+wr, -f / 2), fill = axes_fill) - for i in self.drawn: - i.draw () + Drawable.draw (self) class Obstacle (Drawable): """An obstacle.""" @@ -64,6 +59,7 @@ class Obstacle (Drawable): self.trans_translate (self.pos) self.draw_circle ((0, 0), self.radius, fill = '#31aa23') self.draw_circle ((0, 0), self.radius + 250, outlinestipple = 'gray25') + Drawable.draw (self) class Table (Drawable): """The table and its elements.""" @@ -130,6 +126,7 @@ class Table (Drawable): if len (b[2]) > 1: self.draw_circle ((b[0], 2100 - b[1]), 72 / 2, **balls_config[b[2][1]]) + Drawable.draw (self) class TableView (DrawableCanvas): """This class handle the view of the table and every items inside it.""" @@ -148,13 +145,6 @@ class TableView (DrawableCanvas): self.robot = Robot (self.table) self.robot.angle = 0 self.robot.pos = (0, 0) - self.drawn = [ ] - - def draw (self): - self.table.draw () - for i in self.drawn: - i.draw () - self.robot.draw () class Arm (Drawable): """The robot arm.""" @@ -173,12 +163,13 @@ class Arm (Drawable): self.draw_line ((0, 1), (0.3, 1), arrow = LAST, fill = '#808080') self.draw_line ((0, 0), (cos (pi / 6), -sin (pi / 6))) self.draw_line ((0, 0), (-cos (pi / 6), -sin (pi / 6))) + Drawable.draw (self) -class Servo: +class Servo (Drawable): """Servo motor.""" def __init__ (self, onto, coord, l, start, extent): - self.onto = onto + Drawable.__init__ (self, onto) self.coord = coord self.l = l self.start = start @@ -186,12 +177,13 @@ class Servo: self.pos = 0 def draw (self): - self.onto.draw_arc (self.coord, self.l, start = self.start, + self.reset () + self.draw_arc (self.coord, self.l, start = self.start, extent = self.extent, style = 'arc', outline = '#808080') a = self.start + self.pos * self.extent - self.onto.draw_line (self.coord, (self.coord[0] + self.l * cos (a), + self.draw_line (self.coord, (self.coord[0] + self.l * cos (a), self.coord[1] + self.l * sin (a))) - + Drawable.draw (self) class Rear (Drawable): """Rear actuators.""" @@ -210,14 +202,13 @@ class Rear (Drawable): def draw (self): self.reset () self.trans_scale (0.9/5) - for i in self.traps: - i.draw () self.draw_line ((-0.5, 1.5), (-0.5, 0.5), (-2.5, 0.2), fill = '#808080') self.draw_line ((-2.5, -1.2), (-2.5, -2.3), (2.5, -2.3), (2.5, 0.2), (0.5, 0.5), (0.5, 1.5), fill = '#808080') for i in (-1.5, -0.5, 0.5, 1.5): self.draw_line ((i, -2.3), (i, -2), fill = '#808080') + Drawable.draw (self) class ActuatorView (DrawableCanvas): """This class handle the view of the actuators inside the robot.""" @@ -234,10 +225,6 @@ class ActuatorView (DrawableCanvas): self.rear_drawable.trans_translate ((0, -0.5)) self.rear = Rear (self.rear_drawable) - def draw (self): - self.arm.draw () - self.rear.draw () - class Inter (Frame): """Robot simulation interface.""" @@ -245,7 +232,6 @@ class Inter (Frame): Frame.__init__ (self, master) self.pack (expand = 1, fill = 'both') self.createWidgets () - self.updated = [ ] def createWidgets (self): self.rightFrame = Frame (self) @@ -271,33 +257,27 @@ class Inter (Frame): self.tableview.pack (expand = True, fill = 'both') def update (self, *args): - """If called with arguments, add them to the list of objects to be - updated. - If called without argument, redraw all objects to be updated.""" - if args: - for i in args: - if i not in self.updated: - self.updated.append (i) - else: - for i in self.updated: - i.draw () - self.updated = [ ] + """Redraw all objects to be updated.""" + self.tableview.update () + self.actuatorview.update () if __name__ == '__main__': app = Inter () app.tableview.robot.angle = pi / 3 app.tableview.robot.pos = (700, 700) + app.tableview.robot.update () if 0: from dist_sensor import DistSensor ds = DistSensor (app.tableview.robot, (150, -127), -pi / 12, 800) - app.tableview.robot.drawn.append (ds) - app.tableview.drawn.append (Obstacle (app.tableview, (1300, 1200), 150)) - ds.obstacles = app.tableview.drawn + obs = Obstacle (app.tableview, (1300, 1200), 150) + ds.obstacles = [ obs ] app.actuatorview.arm.angle = pi/6 + app.actuatorview.arm.update () app.actuatorview.rear.traps[0].pos = 1 app.actuatorview.rear.traps[1].pos = 0 app.actuatorview.rear.traps[2].pos = 0 app.actuatorview.rear.traps[3].pos = 1 app.actuatorview.rear.traps[4].pos = 0 app.actuatorview.rear.traps[5].pos = 0 + app.actuatorview.rear.update () app.mainloop() diff --git a/host/inter/inter_node.py b/host/inter/inter_node.py index a69ea01f..b744cade 100644 --- a/host/inter/inter_node.py +++ b/host/inter/inter_node.py @@ -75,10 +75,8 @@ class InterNode (Inter): s.obstacles = self.obstacles s.hide = True s.register (self.update_sharps) - self.tableview.robot.drawn.extend (self.dist_sensors) self.update_sharps () self.path = Path (self.tableview.table) - self.tableview.drawn.append (self.path) self.io_link.path.register (self.notify_path) def createWidgets (self): @@ -145,11 +143,11 @@ class InterNode (Inter): def notify_position (self): self.tableview.robot.pos = self.asserv_link.position.pos self.tableview.robot.angle = self.asserv_link.position.angle - self.update (self.tableview.robot) + self.tableview.robot.update () def notify_aux0 (self): self.actuatorview.arm.angle = self.asserv_link.aux[0].angle - self.update (self.actuatorview.arm) + self.actuatorview.arm.update () def notify_jack (self): self.io_link.jack.state = self.jackVar.get () @@ -163,7 +161,7 @@ class InterNode (Inter): servo = self.io_link.servo[i] trap = self.actuatorview.rear.traps[i] trap.pos = servo.value - self.update (trap) + trap.update () def update_sharps (self): for ds, adc in zip (self.dist_sensors, self.io_link.adc): @@ -180,7 +178,7 @@ class InterNode (Inter): def notify_path (self): self.path.path = self.io_link.path.path - self.update (self.path) + self.path.update () def place_obstacle (self, ev): pos = self.tableview.screen_coord ((ev.x, ev.y)) @@ -188,16 +186,15 @@ class InterNode (Inter): self.obstacles[0].pos = pos else: self.obstacles.append (Obstacle (self.tableview.table, pos, 150)) - self.tableview.drawn.append (self.obstacles[0]) - self.update (*self.obstacles) - self.update (*self.dist_sensors) + for d in self.obstacles + self.dist_sensors: + d.update () self.update () def show_sensors (self): hide = not self.showSensorsVar.get () for i in self.dist_sensors: i.hide = hide - self.update (*self.dist_sensors) + i.update () self.update () if __name__ == '__main__': diff --git a/host/inter/path.py b/host/inter/path.py index eb5744eb..91e7fcc7 100644 --- a/host/inter/path.py +++ b/host/inter/path.py @@ -24,7 +24,7 @@ """Computed path drawing.""" from Tkinter import * -from drawable import * +from simu.inter.drawable import * from math import pi, cos, sin, sqrt diff --git a/host/inter/test/test_drawable.py b/host/inter/test/test_drawable.py deleted file mode 100644 index 291ace29..00000000 --- a/host/inter/test/test_drawable.py +++ /dev/null @@ -1,65 +0,0 @@ -# inter - Robot simulation interface. {{{ -# -# Copyright (C) 2008 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. -# -# }}} */ -from math import pi - -from inter.drawable import * - -class Test (Drawable): - - def draw (self): - self.draw_rectangle ((-100, -100), (100, 100), fill = '', outline = 'gray') - self.draw_rectangle ((0, 0), (5, 5), fill = 'red') - self.draw_rectangle ((20, 20), (50, 50), fill = '', outline = 'blue') - self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), fill = 'green') - self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), smooth = True) - self.draw_circle ((40, -40), 10) - self.draw_arc ((-40, 0), 20, start = pi / 4, extent = pi / 2) - -class App (DrawableCanvas): - - def __init__ (self, master = None): - DrawableCanvas.__init__ (self, 300, 300, 20, 20, master) - self.pack (expand = True, fill = 'both') - self.test = Test (self) - self.animated = False - self.i = 0 - - def animate (self): - # Real user should reset at each redraw. - self.after (500, self.animate) - self.test.draw () - self.test.trans_rotate (-pi/12) - self.test.trans_translate ((10, 10)) - self.test.trans_scale (1.05) - self.i += 1 - if self.i == 10: - self.test.reset () - - def draw (self): - if not self.animated: - self.animate () - self.animated = True - -app = App () -app.mainloop () diff --git a/host/inter/trans_matrix.py b/host/inter/trans_matrix.py deleted file mode 100644 index a8f3849a..00000000 --- a/host/inter/trans_matrix.py +++ /dev/null @@ -1,174 +0,0 @@ -# inter - Robot simulation interface. {{{ -# -# Copyright (C) 2008 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. -# -# }}} -"""TransMatrix utility.""" -from math import sin, cos, sqrt, atan2 - -class TransMatrix: - """Define a matrix to be used for transformations on the plane. - - This is a "special" kind of matrix, because the last column is omitted as - it is always (0, 0, 1).""" - - IDENTITY = ((1, 0), (0, 1), (0, 0)) - - def __init__ (self, *m): - """Initialise the matrix, with optional initial value. - - >>> TransMatrix () - ((1, 0), (0, 1), (0, 0)) - >>> TransMatrix ((1, 2), (3, 4), (5, 6)) - ((1, 2), (3, 4), (5, 6)) - """ - if m: - assert len (m) == 3 - for i in m: - assert len (i) == 2 - self.matrix = m - else: - self.matrix = self.IDENTITY - - def identity (self): - """Set to identity. - - >>> a = TransMatrix () - >>> a.translate ((2, 3)) - >>> a.identity (); a - ((1, 0), (0, 1), (0, 0)) - """ - self.matrix = self.IDENTITY - - def rotate (self, angle): - """Transform the current matrix to do a rotation. - - >>> from math import pi - >>> a = TransMatrix () - >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS - ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0)) - """ - s = sin (angle) - c = cos (angle) - m = TransMatrix ((c, s), (-s, c), (0, 0)) - self *= m - - def translate (self, by): - """Transform the current matrix to do a translation. - - >>> a = TransMatrix () - >>> a.translate ((2, 3)); a - ((1, 0), (0, 1), (2, 3)) - """ - m = TransMatrix ((1, 0), (0, 1), by) - self *= m - - def scale (self, factor): - """Transform the current matrix to do a scaling. - - >>> a = TransMatrix () - >>> a.scale (2); a - ((2, 0), (0, 2), (0, 0)) - """ - m = TransMatrix ((factor, 0), (0, factor), (0, 0)) - self *= m - - def __imul__ (self, other): - """Multiply by an other matrix. - - >>> a = TransMatrix ((1, 0), (0, 1), (1, 0)) - >>> b = TransMatrix ((0, 1), (1, 0), (0, 1)) - >>> a *= b; a - ((0, 1), (1, 0), (0, 2)) - """ - s = self.matrix - o = other.matrix - self.matrix = ( - (s[0][0] * o[0][0] + s[0][1] * o[1][0], - s[0][0] * o[0][1] + s[0][1] * o[1][1]), - (s[1][0] * o[0][0] + s[1][1] * o[1][0], - s[1][0] * o[0][1] + s[1][1] * o[1][1]), - (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0], - s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1])) - return self - - def apply (self, *args): - """Apply (multiply) the matrix to all the given arguments. - - >>> m = TransMatrix ((1, 2), (4, 8), (16, 32)) - >>> m.apply ((1, 0)) - (17, 34) - >>> m.apply ((0, 1), (1, 1)) - ((20, 40), (21, 42)) - """ - r = tuple ( - (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0] - + self.matrix[2][0], - i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1] - + self.matrix[2][1]) - for i in args) - if len (args) == 1: - return r[0] - else: - return r - - def apply_angle (self, angle): - """Apply the matrix to an angle. - - >>> from math import pi - >>> a = TransMatrix () - >>> a.rotate (pi / 6) - >>> a.translate ((2, 3)) - >>> a.scale (4) - >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS - (1.0471..., 1.0471...) - """ - o, m = self.apply ((0, 0), (cos (angle), sin (angle))) - v = (m[0] - o[0], m[1] - o[1]) - vl = sqrt (v[0] ** 2 + v[1] ** 2) - v = (v[0] / vl, v[1] / vl) - return atan2 (v[1], v[0]) - - def apply_distance (self, distance): - """Apply the matrix to a distance. - - >>> from math import pi - >>> a = TransMatrix () - >>> a.rotate (pi / 6) - >>> a.translate ((2, 3)) - >>> a.scale (4) - >>> round (a.apply_distance (2)) - 8.0 - """ - o, m = self.apply ((0, 0), (distance, 0)) - v = (m[0] - o[0], m[1] - o[1]) - vl = sqrt (v[0] ** 2 + v[1] ** 2) - return vl - - def __repr__ (self): - return self.matrix.__repr__ () - -def _test (): - import doctest - doctest.testmod () - -if __name__ == '__main__': - _test() diff --git a/host/simu/__init__.py b/host/simu/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/host/simu/inter/__init__.py b/host/simu/inter/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/host/simu/inter/drawable.py b/host/simu/inter/drawable.py new file mode 100644 index 00000000..6f04ffd8 --- /dev/null +++ b/host/simu/inter/drawable.py @@ -0,0 +1,210 @@ +# inter - Robot simulation interface. {{{ +# +# Copyright (C) 2008 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. +# +# }}} +"""Drawable and DrawableCanvas.""" +from math import sqrt, degrees +import simu.utils.trans_matrix +import Tkinter + +__all__ = ('Drawable', 'DrawableCanvas') + +class Drawable: + """Define a drawable area with embedded transformations.""" + + def __init__ (self, onto): + """Initialise the drawable.""" + self.__onto = onto + self.__trans_matrix = simu.utils.trans_matrix.TransMatrix () + self.__items = [ ] + self.trans_apply = self.__trans_matrix.apply + self.trans_apply_angle = self.__trans_matrix.apply_angle + self.trans_apply_distance = self.__trans_matrix.apply_distance + self.trans_rotate = self.__trans_matrix.rotate + self.trans_translate = self.__trans_matrix.translate + self.trans_scale = self.__trans_matrix.scale + self.trans_identity = self.__trans_matrix.identity + self.__children = [ ] + self.children = self.__children + self.__onto.__children.append (self) + + def __draw_rectangle (self, p1, p2, **kw): + if 'outline' not in kw: + kw = kw.copy () + kw['outline'] = 'black' + p = self.trans_apply (p1, (p2[0], p1[1]), p2, (p1[0], p2[1])) + return self.__onto.__draw_polygon (*p, **kw) + + def __draw_line (self, *p, **kw): + p = self.trans_apply (*p) + return self.__onto.__draw_line (*p, **kw) + + def __draw_polygon (self, *p, **kw): + p = self.trans_apply (*p) + return self.__onto.__draw_polygon (*p, **kw) + + def __draw_circle (self, p, r, **kw): + p = self.trans_apply (p) + r = self.trans_apply_distance (r) + return self.__onto.__draw_circle (p, r, **kw) + + def __draw_arc (self, p, r, **kw): + p = self.trans_apply (p) + r = self.trans_apply_distance (r) + if 'start' in kw: + kw = kw.copy () + kw['start'] = self.trans_apply_angle (kw['start']) + import math + return self.__onto.__draw_arc (p, r, **kw) + + def draw_rectangle (self, *p, **kw): + """Draw a rectangle.""" + self.__items.append (self.__draw_rectangle (*p, **kw)) + + def draw_line (self, *p, **kw): + """Draw a line.""" + self.__items.append (self.__draw_line (*p, **kw)) + + def draw_polygon (self, *p, **kw): + """Draw a line.""" + self.__items.append (self.__draw_polygon (*p, **kw)) + + def draw_circle (self, p, r, **kw): + """Draw a circle of the given radius centered on p.""" + self.__items.append (self.__draw_circle (p, r, **kw)) + + def draw_arc (self, p, r, **kw): + """Draw a arc of the given radius centered on p.""" + self.__items.append (self.__draw_arc (p, r, **kw)) + + def reset (self): + """Clear all drawn items, reset transformations.""" + for i in self.__children: + i.reset () + self.__delete (*self.__items) + self.__items = [ ] + self.trans_identity () + + def __delete (self, *list): + """Delete a list of items.""" + self.__onto.__delete (*list) + + def update (self, *list): + """Add provided arguments to update list, or self if no argument + provided.""" + if list: + self.__onto.update (*list) + else: + self.__onto.update (self) + + def draw (self): + """Default drawing method which redraw every children.""" + if self.__children: + self.update (*self.__children) + + +class DrawableCanvas(Tkinter.Canvas): + """Extend a Tkinter.Canvas to use Drawable on it. Children are drawn on + redraw.""" + + def __init__ (self, width, height, xorigin, yorigin, master = None, **kw): + """Initialise a DrawableCanvas. The width and height parameters + define the requested drawable area virtual size. The xorigin and + yorigin parameters define origin of the virtual coordinates relative + to the drawable center.""" + Tkinter.Canvas.__init__ (self, master, **kw) + self.__width = width + self.__height = height + self.__xorigin = xorigin + self.__yorigin = yorigin + self.bind ('', self.__resize) + self.__updated = [ ] + self._Drawable__children = [ ] + + def __resize (self, ev): + # Compute new scale. + w, h = float (ev.width), float (ev.height) + self.__scale = min (w / self.__width, h / self.__height) + self.__xoffset = w / 2 + self.__xorigin * self.__scale + self.__yoffset = h / 2 - self.__yorigin * self.__scale + # Redraw. + self.draw () + + def _Drawable__draw_line (self, *p, **kw): + p = self.__coord (*p) + return self.create_line (*p, **kw) + + def _Drawable__draw_polygon (self, *p, **kw): + p = self.__coord (*p) + return self.create_polygon (*p, **kw) + + def _Drawable__draw_circle (self, p, r, **kw): + p, = self.__coord (p) + r = r * self.__scale + p1 = (p[0] - r, p[1] - r) + p2 = (p[0] + r, p[1] + r) + return self.create_oval (p1, p2, **kw) + + def _Drawable__draw_arc (self, p, r, **kw): + p, = self.__coord (p) + r = r * self.__scale + p1 = (p[0] - r, p[1] - r) + p2 = (p[0] + r, p[1] + r) + for k in ('start', 'extent'): + if k in kw: + kw = kw.copy () + kw[k] = degrees (kw[k]) + return self.create_arc (p1, p2, **kw) + + def _Drawable__delete (self, *list): + self.delete (*list) + + def update (self, *list): + """If called with arguments, add them to the update list. + Else, redraw all element in the update list.""" + if list: + for i in list: + if i not in self.__updated: + self.__updated.append (i) + else: + while self.__updated: + updated = self.__updated + self.__updated = [ ] + for i in updated: + i.draw () + + def draw (self): + """Default drawing method which redraw every children.""" + if self._Drawable__children: + self.update (*self._Drawable__children) + self.update () + + def __coord (self, *args): + return [ (i[0] * self.__scale + self.__xoffset, + -i[1] * self.__scale + self.__yoffset) for i in args ] + + def screen_coord (self, screen): + """Return drawable coordinates corresponding to the given screen + coordinates.""" + return ((self.canvasx (screen[0]) - self.__xoffset) / self.__scale, + -(self.canvasy (screen[1]) - self.__yoffset) / self.__scale) + diff --git a/host/simu/inter/test/test_drawable.py b/host/simu/inter/test/test_drawable.py new file mode 100644 index 00000000..f67cb144 --- /dev/null +++ b/host/simu/inter/test/test_drawable.py @@ -0,0 +1,65 @@ +# inter - Robot simulation interface. {{{ +# +# Copyright (C) 2008 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. +# +# }}} */ +from math import pi + +from simu.inter.drawable import * + +class Test (Drawable): + + def draw (self): + self.draw_rectangle ((-100, -100), (100, 100), fill = '', outline = 'gray') + self.draw_rectangle ((0, 0), (5, 5), fill = 'red') + self.draw_rectangle ((20, 20), (50, 50), fill = '', outline = 'blue') + self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), fill = 'green') + self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), smooth = True) + self.draw_circle ((40, -40), 10) + self.draw_arc ((-40, 0), 20, start = pi / 4, extent = pi / 2) + +class App (DrawableCanvas): + + def __init__ (self, master = None): + DrawableCanvas.__init__ (self, 300, 300, 20, 20, master) + self.pack (expand = True, fill = 'both') + self.test = Test (self) + self.animated = False + self.i = 0 + + def animate (self): + # Real user should reset at each redraw. + self.after (500, self.animate) + self.test.draw () + self.test.trans_rotate (-pi/12) + self.test.trans_translate ((10, 10)) + self.test.trans_scale (1.05) + self.i += 1 + if self.i == 10: + self.test.reset () + + def draw (self): + if not self.animated: + self.animate () + self.animated = True + +app = App () +app.mainloop () diff --git a/host/simu/utils/__init__.py b/host/simu/utils/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/host/simu/utils/trans_matrix.py b/host/simu/utils/trans_matrix.py new file mode 100644 index 00000000..7071e188 --- /dev/null +++ b/host/simu/utils/trans_matrix.py @@ -0,0 +1,174 @@ +# inter - Robot simulation interface. {{{ +# +# Copyright (C) 2008 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. +# +# }}} +"""TransMatrix utility.""" +from math import sin, cos, sqrt, atan2 + +class TransMatrix: + """Define a matrix to be used for transformations on the plane. + + This is a "special" kind of matrix, because the last column is omitted as + it is always (0, 0, 1).""" + + IDENTITY = ((1, 0), (0, 1), (0, 0)) + + def __init__ (self, *m): + """Initialise the matrix, with optional initial value. + + >>> TransMatrix () + ((1, 0), (0, 1), (0, 0)) + >>> TransMatrix ((1, 2), (3, 4), (5, 6)) + ((1, 2), (3, 4), (5, 6)) + """ + if m: + assert len (m) == 3 + for i in m: + assert len (i) == 2 + self.matrix = m + else: + self.matrix = self.IDENTITY + + def identity (self): + """Set to identity. + + >>> a = TransMatrix () + >>> a.translate ((2, 3)) + >>> a.identity (); a + ((1, 0), (0, 1), (0, 0)) + """ + self.matrix = self.IDENTITY + + def rotate (self, angle): + """Transform the current matrix to do a rotation. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS + ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0)) + """ + s = sin (angle) + c = cos (angle) + m = TransMatrix ((c, s), (-s, c), (0, 0)) + self *= m + + def translate (self, by): + """Transform the current matrix to do a translation. + + >>> a = TransMatrix () + >>> a.translate ((2, 3)); a + ((1, 0), (0, 1), (2, 3)) + """ + m = TransMatrix ((1, 0), (0, 1), by) + self *= m + + def scale (self, factor): + """Transform the current matrix to do a scaling. + + >>> a = TransMatrix () + >>> a.scale (2); a + ((2, 0), (0, 2), (0, 0)) + """ + m = TransMatrix ((factor, 0), (0, factor), (0, 0)) + self *= m + + def __imul__ (self, other): + """Multiply by an other matrix. + + >>> a = TransMatrix ((1, 0), (0, 1), (1, 0)) + >>> b = TransMatrix ((0, 1), (1, 0), (0, 1)) + >>> a *= b; a + ((0, 1), (1, 0), (0, 2)) + """ + s = self.matrix + o = other.matrix + self.matrix = ( + (s[0][0] * o[0][0] + s[0][1] * o[1][0], + s[0][0] * o[0][1] + s[0][1] * o[1][1]), + (s[1][0] * o[0][0] + s[1][1] * o[1][0], + s[1][0] * o[0][1] + s[1][1] * o[1][1]), + (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0], + s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1])) + return self + + def apply (self, *args): + """Apply (multiply) the matrix to all the given arguments. + + >>> m = TransMatrix ((1, 2), (4, 8), (16, 32)) + >>> m.apply ((1, 0)) + (17, 34) + >>> m.apply ((0, 1), (1, 1)) + ((20, 40), (21, 42)) + """ + r = tuple ( + (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0] + + self.matrix[2][0], + i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1] + + self.matrix[2][1]) + for i in args) + if len (args) == 1: + return r[0] + else: + return r + + def apply_angle (self, angle): + """Apply the matrix to an angle. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 6) + >>> a.translate ((2, 3)) + >>> a.scale (4) + >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS + (1.0471..., 1.0471...) + """ + o, m = self.apply ((0, 0), (cos (angle), sin (angle))) + v = (m[0] - o[0], m[1] - o[1]) + vl = sqrt (v[0] ** 2 + v[1] ** 2) + v = (v[0] / vl, v[1] / vl) + return atan2 (v[1], v[0]) + + def apply_distance (self, distance): + """Apply the matrix to a distance. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 6) + >>> a.translate ((2, 3)) + >>> a.scale (4) + >>> round (a.apply_distance (2)) + 8.0 + """ + o, m = self.apply ((0, 0), (distance, 0)) + v = (m[0] - o[0], m[1] - o[1]) + vl = sqrt (v[0] ** 2 + v[1] ** 2) + return vl + + def __repr__ (self): + return self.matrix.__repr__ () + +def _test (): + import doctest + doctest.testmod () + +if __name__ == '__main__': + _test() -- cgit v1.2.3