From 06037accc5049c1f55c287e582cdc6a74a4e06c6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 7 Apr 2012 18:47:31 +0200 Subject: host/simu/robots/guybrush: add upper clamp simulation --- host/simu/robots/guybrush/model/bag.py | 17 +++++--- host/simu/robots/guybrush/model/clamps.py | 67 ++++++++++++++++++++++++++++++- host/simu/robots/guybrush/view/clamps.py | 33 ++++++++++++++- host/simu/robots/guybrush/view/robot.py | 43 +++++++++++++++++++- 4 files changed, 151 insertions(+), 9 deletions(-) (limited to 'host') diff --git a/host/simu/robots/guybrush/model/bag.py b/host/simu/robots/guybrush/model/bag.py index 3b6cc857..8e7250b2 100644 --- a/host/simu/robots/guybrush/model/bag.py +++ b/host/simu/robots/guybrush/model/bag.py @@ -39,12 +39,19 @@ class Bag: self.jack = Switch (link_bag.io_hub.contact[1], invert = True) self.strat_switch = Switch (link_bag.io_hub.contact[2], invert = True) self.position = Position (link_bag.asserv.position) + output = link_bag.io_hub.output self.clamps = Clamps (table, self.position, link_bag.mimot.aux[0], - (PneumaticCylinder (None, link_bag.io_hub.output[8], - scheduler, 0., 30., 150., 75., 30.), - PneumaticCylinder (None, link_bag.io_hub.output[9], - scheduler, 0., 30., 150., 75., 30.)), - [ Switch (c) for c in link_bag.io_hub.contact[3:3+4] ]) + (PneumaticCylinder (None, output[8], scheduler, + 0., 30., 150., 75., 30.), + PneumaticCylinder (None, output[9], scheduler, + 0., 30., 150., 75., 30.)), + [ Switch (c) for c in link_bag.io_hub.contact[3:3+4] ], + PneumaticCylinder (output[4], output[5], scheduler, + 0., 1., 1., 1., 1.), + PneumaticCylinder (output[3], output[2], scheduler, + 0., 1., 1., 1., 0.), + PneumaticCylinder (None, output[1], scheduler, + 0., 30., 150., 75., 30.)) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py index 1cfc30f1..d1545c8d 100644 --- a/host/simu/robots/guybrush/model/clamps.py +++ b/host/simu/robots/guybrush/model/clamps.py @@ -29,7 +29,9 @@ from math import pi class Clamps (Observable): def __init__ (self, table, robot_position, lower_clamp_motor, - lower_clamp_cylinders, lower_clamp_sensors): + lower_clamp_cylinders, lower_clamp_sensors, + upper_clamp_up_down_cylinder, upper_clamp_in_out_cylinder, + upper_clamp_open_cylinder): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -38,9 +40,16 @@ class Clamps (Observable): self.lower_clamp_sensors = lower_clamp_sensors self.lower_clamp_clamping = [ None, None ] self.lower_clamp_content = [ [ ], [ ] ] + self.upper_clamp_up_down_cylinder = upper_clamp_up_down_cylinder + self.upper_clamp_in_out_cylinder = upper_clamp_in_out_cylinder + self.upper_clamp_open_cylinder = upper_clamp_open_cylinder + self.upper_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.upper_clamp_up_down_cylinder.register (self.__upper_clamp_notified) + self.upper_clamp_in_out_cylinder.register (self.__upper_clamp_notified) + self.upper_clamp_open_cylinder.register (self.__upper_clamp_notified) self.robot_position.register (self.__robot_position_notified) def __robot_position_notified (self): @@ -58,6 +67,17 @@ class Clamps (Observable): self.__compute_lower_clamp () self.notify () + def __upper_clamp_notified (self): + self.upper_clamp_up_down = 1 - self.upper_clamp_up_down_cylinder.pos # 1. is down. + self.upper_clamp_in_out = self.upper_clamp_in_out_cylinder.pos # 1. is out. + c = self.upper_clamp_open_cylinder + if c.pos is None: + self.upper_clamp_clamping = None + else: + self.upper_clamp_clamping = c.pos / c.pos_out + self.__compute_upper_clamp () + self.notify () + def __compute_lower_clamp (self): """Compute all operations of lower clamps.""" changed = False @@ -97,6 +117,25 @@ class Clamps (Observable): s.notify () return changed + def __compute_upper_clamp (self): + """Compute all operations of upper clamps.""" + if (self.upper_clamp_up_down is None + or self.upper_clamp_in_out is None + or self.upper_clamp_clamping is None): + return + if (self.upper_clamp_clamping < 0.1 + and self.upper_clamp_up_down > 0.9): + # Examine elements in front of the robot. + for e, y in self.__get_upper_elements (): + self.upper_clamp_content.append ((e, y)) + e.pos = None + e.notify () + if (self.upper_clamp_clamping > 0.9 + and self.upper_clamp_up_down < 0.1): + # Drop elements. + self.__add_load ([ e for e, y in self.upper_clamp_content]) + self.upper_clamp_content = [ ] + def __add_load (self, elements): """Add element list to load.""" pass @@ -120,6 +159,32 @@ class Clamps (Observable): and pos[1] > -ymargin and pos[1] < ymargin): yield (o, pos[1]) + def __get_upper_elements (self): + """Return elements in front of the robot in the upper clamps, with its + y coordinate in the 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 = 150 + xmargin = 20 + ymargin2 = 20 + ymargin3 = 171.5 + for o in self.table.obstacles: + if o.level == 2 and o.pos is not None: + pos = m.apply (o.pos) + if (pos[0] > xoffset + 35 - xmargin + and pos[0] < xoffset + 35 + xmargin + and pos[1] > -ymargin2 and pos[1] < ymargin2): + yield (o, 0) + if o.level == 3 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] > -ymargin3 and pos[1] < ymargin3): + yield (o, pos[1]) + def __get_robot_matrix (self): """Return robot transformation matrix.""" m = TransMatrix () diff --git a/host/simu/robots/guybrush/view/clamps.py b/host/simu/robots/guybrush/view/clamps.py index bdcb08a4..25d3f107 100644 --- a/host/simu/robots/guybrush/view/clamps.py +++ b/host/simu/robots/guybrush/view/clamps.py @@ -23,7 +23,8 @@ # }}} """Guybrush clamps.""" from simu.inter.drawable import Drawable -from math import pi +from math import pi, cos, sin, radians +from simu.view.table_eurobot2012 import YELLOW GREY = '#808080' BLACK = '#000000' @@ -63,3 +64,33 @@ class ClampsSide (Drawable): if not i: self.trans_rotate (pi) self.trans_pop () + # Draw upper clamp. + ud = self.model.upper_clamp_up_down + c = self.model.upper_clamp_clamping + io = self.model.upper_clamp_in_out + if (ud is not None and c is not None and io is not None): + self.trans_push () + # Approximation: map cylinder extension to angle. + a = radians (ud * 75 + 87) + x = 40 + 188 * cos (a) + y = 75 + 188 * sin (a) + self.trans_translate ((x, y)) + self.trans_rotate ((1 - ud) * 0.5 * pi) + self.draw_line ((0, 56), (0, -25), fill = GREY) + # Draw level 2 clamp. + for e, y, in self.model.upper_clamp_content: + if e.level == 2: + self.draw_polygon ((-15, -15), (-15 - 70, -15), + (-15 - 13 - 44, -15 + 48.5), + (-15 - 13, -15 + 48.8), fill = YELLOW, + outline = BLACK) + break + self.draw_line ((-15, -10 + c * 5), (-15 - 70, -10 + c * 5)) + # Draw level 3 clamp. + for e, y in self.model.upper_clamp_content: + if e.level == 3: + self.draw_rectangle ((-12 * io - e.radius * 2, 54), + (-12 * io, 50), fill = GREY) + self.draw_line ((-12 * io - 35, 55), (-12 * io, 55), + (-12 * io, 55 - c * 33), (-12 * io - 35, 55 - c * 33)) + self.trans_pop () diff --git a/host/simu/robots/guybrush/view/robot.py b/host/simu/robots/guybrush/view/robot.py index 79c61d15..a7377a66 100644 --- a/host/simu/robots/guybrush/view/robot.py +++ b/host/simu/robots/guybrush/view/robot.py @@ -23,12 +23,14 @@ # }}} """Guybrush robot view.""" import simu.inter.drawable -from math import pi, cos -from simu.view.table_eurobot2012 import WHITE, BLACK +from math import pi, cos, sin, radians +from simu.view.table_eurobot2012 import WHITE, BLACK, YELLOW COLOR_ROBOT = '#000000' COLOR_AXES = '#202040' +GREY = '#808080' + class Robot (simu.inter.drawable.Drawable): def __init__ (self, onto, position_model, clamps_model): @@ -62,6 +64,43 @@ class Robot (simu.inter.drawable.Drawable): self.draw_oval ((x, y), sx * e.radius, e.radius, fill = color) sx = - sx + # Draw upper clamps. + ud = self.clamps_model.upper_clamp_up_down + c = self.clamps_model.upper_clamp_clamping + io = self.clamps_model.upper_clamp_in_out + if (ud is not None and c is not None and io is not None + and ud > 0.1): + self.trans_push () + # Approximation: map cylinder extension to angle. + cx = -cos (radians (ud * 75 + 87)) + x = -40 + 188 * cx + self.trans_translate ((x, 0)) + # Draw level 2 clamp. + for e, y, in self.clamps_model.upper_clamp_content: + if e.level == 2: + self.draw_rectangle ((15 * cx, 75), (85 * cx, -75), + fill = YELLOW) + self.draw_rectangle ((28 * cx, 62), (72 * cx, -62), + fill = YELLOW) + break + self.draw_line ((15 * cx, 80 - c * 10), (85 * cx, 80 - c * 10)) + self.draw_line ((15 * cx, -80 + c * 10), (85 * cx, -80 + c * 10)) + # Draw level 3 clamp. + for side in (1, -1): + a = side * -0.61 * (1 - io) + rx, ry = cos (a), sin (a) + for e, y in self.clamps_model.upper_clamp_content: + if e.level == 3 and y * side > 0: + color = WHITE if e.value else BLACK + y = 80 - y * side + self.draw_oval ((rx * 72 + ry * y, ry * 72 + rx * y + side * 80), + e.radius * cx, e.radius, fill = color) + self.trans_push () + self.trans_translate ((0, side * 80)) + self.trans_rotate (a) + self.draw_rectangle ((12, -4 * side), (12 + 35, 43 * side), fill = GREY) + self.trans_pop () + self.trans_pop () # 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