summaryrefslogtreecommitdiffhomepage
path: root/host/simu
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu')
-rw-r--r--host/simu/model/distance_sensor.py76
-rw-r--r--host/simu/model/round_obstacle.py54
-rw-r--r--host/simu/model/table.py47
3 files changed, 177 insertions, 0 deletions
diff --git a/host/simu/model/distance_sensor.py b/host/simu/model/distance_sensor.py
new file mode 100644
index 00000000..0fa34d15
--- /dev/null
+++ b/host/simu/model/distance_sensor.py
@@ -0,0 +1,76 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2009 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.
+#
+# }}}
+"""Generic distance sensor model."""
+from math import cos, sin
+from simu.utils.trans_matrix import TransMatrix
+
+class DistanceSensor:
+
+ def __init__ (self, table, pos, angle, range, into = None, level = 0):
+ self.table = table
+ self.pos = pos
+ self.angle = angle
+ self.range = range
+ self.target = (pos[0] + cos (angle) * range,
+ pos[1] + sin (angle) * range)
+ self.into = into or ()
+ self.level = level
+ self.distance = None
+
+ def evaluate (self):
+ # Transform in the table base.
+ pos, target = self.pos, self.target
+ m = TransMatrix ()
+ for i in self.into:
+ m.rotate (i.angle)
+ m.translate (i.pos)
+ pos, target = m.apply (pos, target)
+ # Find intersection.
+ i = self.table.intersect (pos, target, level = self.level,
+ comp = lambda a, b: a < b)
+ if i is not None:
+ self.distance = i.distance
+ else:
+ self.distance = None
+
+if __name__ == '__main__':
+ from simu.model.table import Table
+ from simu.model.round_obstacle import RoundObstacle
+ from math import pi
+ t = Table ()
+ ro1 = RoundObstacle (0.5)
+ ro1.pos = (0, 0)
+ t.obstacles.append (ro1)
+ ro2 = RoundObstacle (1)
+ ro2.pos = (1, 0)
+ t.obstacles.append (ro2)
+ ds = DistanceSensor (t, (-1, 0), 0, 1)
+ ds.evaluate ()
+ assert ds.distance == 0.5
+ ds = DistanceSensor (t, (-1, -1), 0, 1)
+ ds.evaluate ()
+ assert ds.distance is None
+ ds = DistanceSensor (t, (3, 0), pi, 3)
+ ds.evaluate ()
+ assert ds.distance == 1.0
diff --git a/host/simu/model/round_obstacle.py b/host/simu/model/round_obstacle.py
new file mode 100644
index 00000000..fc0cfc6a
--- /dev/null
+++ b/host/simu/model/round_obstacle.py
@@ -0,0 +1,54 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2009 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 round shape."""
+from math import pi, cos, sin, sqrt
+
+class RoundObstacle:
+
+ def __init__ (self, radius, level = 0):
+ self.pos = None
+ self.radius = radius
+ 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."""
+ 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.
+ # 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])
+ if doc < self.radius:
+ # Line intersects, check if segment intersects.
+ m = vao[0] * n[0] + vao[1] * n[1]
+ f = sqrt (self.radius ** 2 - doc ** 2)
+ if m - f > 0 and m - f < ab:
+ return m - f
+ elif m + f > 0 and m + f < ab:
+ return m + f
+ return None
+
diff --git a/host/simu/model/table.py b/host/simu/model/table.py
new file mode 100644
index 00000000..90cff707
--- /dev/null
+++ b/host/simu/model/table.py
@@ -0,0 +1,47 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2009 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."""
+
+class Intersect:
+
+ def __init__ (self, obstacle, distance):
+ self.obstacle = obstacle
+ self.distance = distance
+
+class Table:
+
+ def __init__ (self):
+ self.obstacles = [ ]
+
+ def intersect (self, a, b, level = None, comp = None):
+ i = None
+ for o in self.obstacles:
+ if level is None or level == o.level:
+ d = o.intersect (a, b)
+ if d is not None and (i is None or comp (d, i.distance)):
+ i = Intersect (o, d)
+ if comp is None:
+ return i
+ return i
+