From d66664cde28a490759fd6f3a11791b9f20c1933d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 30 Mar 2012 23:09:45 +0200 Subject: host/simu/robots/guybrush: simulate lower clamps elements taking --- host/simu/robots/guybrush/model/clamps.py | 73 +++++++++++++++++++++++++++++++ 1 file changed, 73 insertions(+) (limited to 'host/simu/robots/guybrush/model') 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 + -- cgit v1.2.3