summaryrefslogtreecommitdiff
path: root/host/simu/robots/guybrush/model/clamps.py
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu/robots/guybrush/model/clamps.py')
-rw-r--r--host/simu/robots/guybrush/model/clamps.py73
1 files changed, 73 insertions, 0 deletions
diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py
index b5278694..91e24f9b 100644
--- a/host/simu/robots/guybrush/model/clamps.py
+++ b/host/simu/robots/guybrush/model/clamps.py
@@ -23,6 +23,8 @@
# }}}
"""Guybrush clamps."""
from utils.observable import Observable
+from simu.utils.trans_matrix import TransMatrix
+from math import pi
class Clamps (Observable):
@@ -34,9 +36,15 @@ class Clamps (Observable):
self.lower_clamp_motor = lower_clamp_motor
self.lower_clamp_cylinders = lower_clamp_cylinders
self.lower_clamp_clamping = [ None, None ]
+ self.lower_clamp_content = [ [ ], [ ] ]
self.lower_clamp_motor.register (self.__lower_clamp_notified)
for c in self.lower_clamp_cylinders:
c.register (self.__lower_clamp_notified)
+ self.robot_position.register (self.__robot_position_notified)
+
+ def __robot_position_notified (self):
+ if self.__compute_lower_clamp ():
+ self.notify ()
def __lower_clamp_notified (self):
self.lower_clamp_rotation = self.lower_clamp_motor.angle
@@ -46,4 +54,69 @@ class Clamps (Observable):
else:
self.lower_clamp_clamping[i] = ((c.pos - c.pos_in)
/ (c.pos_out - c.pos_in))
+ self.__compute_lower_clamp ()
self.notify ()
+
+ def __compute_lower_clamp (self):
+ """Compute all operations of lower clamps."""
+ changed = False
+ if self.lower_clamp_rotation is None:
+ return False
+ # If a lower clamp is on the floor:
+ rot_mod = self.lower_clamp_rotation % (2 * pi)
+ if rot_mod < pi * 0.2 or rot_mod >= pi * 1.9:
+ floor_clamp = 0
+ elif rot_mod >= pi * 0.9 and rot_mod < pi * 1.2:
+ floor_clamp = 1
+ else:
+ floor_clamp = None
+ if floor_clamp is not None:
+ top_clamp = 1 - floor_clamp
+ # Examine floor and add found element to the clamp content.
+ for e, y in self.__get_floor_elements ():
+ self.lower_clamp_content[floor_clamp].append ((e, y))
+ e.pos = None
+ e.notify ()
+ changed = True
+ # Drop element if possible.
+ if self.lower_clamp_clamping[top_clamp] > 0.1:
+ self.__add_load ([ e
+ for e, y in self.lower_clamp_content[top_clamp]])
+ self.lower_clamp_content[top_clamp] = [ ]
+ changed = True
+ # If something is found, there can be updates.
+ if changed:
+ pass
+ return changed
+
+ def __add_load (self, elements):
+ """Add element list to load."""
+ pass
+
+ def __get_floor_elements (self):
+ """Return an elements in front of the robot, on the floor, with its y
+ coordinate in robot base."""
+ if self.robot_position.pos is None:
+ return
+ # Matrix to transform an obstacle position into robot coordinates.
+ m = self.__get_robot_matrix ()
+ # Look up elements.
+ xoffset = 117 - 6
+ xmargin = 20
+ ymargin = 171.5 - 60
+ for o in self.table.obstacles:
+ if o.level == 1 and o.value <= 1 and o.pos is not None:
+ pos = m.apply (o.pos)
+ if (pos[0] > xoffset + o.radius - xmargin
+ and pos[0] < xoffset + o.radius + xmargin
+ and pos[1] > -ymargin and pos[1] < ymargin):
+ yield (o, pos[1])
+
+ 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
+