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 +++++++++++++++++++++++++++++++ host/simu/robots/guybrush/view/bag.py | 2 +- host/simu/robots/guybrush/view/clamps.py | 5 +++ host/simu/robots/guybrush/view/robot.py | 18 +++++++- 4 files changed, 95 insertions(+), 3 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 + diff --git a/host/simu/robots/guybrush/view/bag.py b/host/simu/robots/guybrush/view/bag.py index f512886a..3d5fb621 100644 --- a/host/simu/robots/guybrush/view/bag.py +++ b/host/simu/robots/guybrush/view/bag.py @@ -37,7 +37,7 @@ class Bag: 'Color') self.strat_switch = Switch (sensor_frame, model_bag.strat_switch, 'Strat') - self.robot = Robot (table, model_bag.position) + self.robot = Robot (table, model_bag.position, model_bag.clamps) self.clamps = ClampsSide (actuator_view.add_view (ClampsSide.width, ClampsSide.height), model_bag.clamps) self.distance_sensor = [DistanceSensorUS (self.robot, ds) diff --git a/host/simu/robots/guybrush/view/clamps.py b/host/simu/robots/guybrush/view/clamps.py index 11ef09b8..bdcb08a4 100644 --- a/host/simu/robots/guybrush/view/clamps.py +++ b/host/simu/robots/guybrush/view/clamps.py @@ -55,6 +55,11 @@ class ClampsSide (Drawable): if c is not None: self.draw_line ((7, -45), (7, -46 - c * 33), (-28, -46 - c * 33)) + if self.model.lower_clamp_content[i]: + e, y = self.model.lower_clamp_content[i][0] + self.draw_rectangle ((7, -45 - c * 15), + (7 - e.radius * 2, -49 - c * 15), + fill = GREY) if not i: self.trans_rotate (pi) self.trans_pop () diff --git a/host/simu/robots/guybrush/view/robot.py b/host/simu/robots/guybrush/view/robot.py index fcf375cb..79c61d15 100644 --- a/host/simu/robots/guybrush/view/robot.py +++ b/host/simu/robots/guybrush/view/robot.py @@ -23,18 +23,21 @@ # }}} """Guybrush robot view.""" import simu.inter.drawable -from math import pi +from math import pi, cos +from simu.view.table_eurobot2012 import WHITE, BLACK COLOR_ROBOT = '#000000' COLOR_AXES = '#202040' class Robot (simu.inter.drawable.Drawable): - def __init__ (self, onto, position_model): + def __init__ (self, onto, position_model, clamps_model): """Construct and make connections.""" simu.inter.drawable.Drawable.__init__ (self, onto) self.position_model = position_model + self.clamps_model = clamps_model self.position_model.register (self.__position_notified) + self.clamps_model.register (self.update) def __position_notified (self): """Called on position modifications.""" @@ -48,6 +51,17 @@ class Robot (simu.inter.drawable.Drawable): if self.pos is not None: self.trans_translate (self.pos) self.trans_rotate (self.angle) + # Draw lower clamps content. + r = self.clamps_model.lower_clamp_rotation + if r is not None: + sx = cos (r) + for content in self.clamps_model.lower_clamp_content: + for e, y in content: + x = 117 + sx * 54 + color = WHITE if e.value else BLACK + self.draw_oval ((x, y), sx * e.radius, e.radius, + fill = color) + sx = - sx # Draw robot body. self.draw_polygon ((150, 171.5), (-80, 171.5), (-130, 121.5), (-130, -121.5), (-80, -171.5), (150, -171.5), -- cgit v1.2.3