def setUp(self): self.arene = Arene() # Création aléatoire des variables l = [random.randint(1, 5) * random.choice([1, -1]) for i in range(2)] origine = Point(l[0], l[1], 0) teta = random.random() * random.choice( [1, -1]) * 2 * pi # Angle du vecteur entre l'origine et le pavé v = Vecteur(1, 0, 0).rotate(teta).norm() # True est obligatoire pour que test_vue_dessus affiche le pavé au centre self.view_mat = VueMatriceArene(self.arene, origine.clone(), v.clone(), True)
def setUp(self): c = Point(2, 2, 1) v = Vecteur(1, 0, 0) dist = 3.0 self.arene = Arene() self.strat = DeplacementDroitAmeliore(robot=RobotMotorise( forme=Pave(1, 1, 1, c.clone()), direction=v.clone()), arene=self.arene) self.v = self.strat.robot.direction * dist self.p = Pave(0.5, 0.5, 0.5, self.strat.robot.centre + self.v) self.arene.add(self.p) self.arene.add(self.strat.robot)
def get_mesure(self, arene, d_ignore=0.0, ignore=None, direction=None): """ Retourne la distance entre le capteur et le premier objet dans la matrice Retourne -1 si pas d'obstacle detecté :param arene: arène dans laquelle effectuer la mesure :type arene: Arene :param d_ignore: Distance à partir de laquelle commencer à mesurer :type d_ignore: float :param ignore: Liste d'objets à masquer des mesures :type ignore: [Objet3D] :return: float ou None """ self.get_matrix_view(arene, direction) m = self.arena_v.vueDessus(self.portee, ignore) # On parcours la matrice sur la diagonale, dans le sens croissant v = Vecteur(1, 1, 0).norm() * VueMatriceArene.res n = int(self.portee / CapteurIR.dp) for cpt in range(n): pos = (v * cpt * CapteurIR.dp).to_point().clone(type_coords=int) try: # Au cas ou on sortirait du tableau if m[int(pos.x)][int(pos.y)] == 1: res = cpt * CapteurIR.dp if res >= d_ignore: return res except: print(pos, " est hors de portée") return -1
def test_move(self): v = Vecteur(random.choice([1, -1]) * random.randint(1, 5), random.choice([1, -1]) * random.randint(0, 5), random.choice([1, -1]) * random.randint(1, 5)) self.r.move(v) t=Tete(centre=self.r.tete.clone()) vect_diff=(t.centre.move(v)-self.r.tete.centre).to_vect() self.assertLess(vect_diff.get_mag(), 0.001)
def test_tete(self): """ Vérifie la liaison entre la tête et le robot """ print("Testing movement dependencies...") d = (random.random() * 0.5 + 0.5) * random.randint(5, 10) angle = random.random() * 2 * pi self.assertEqual(self.r.centre, self.r.tete.centre) self.r.move(Vecteur(1, 0, 0) * d) self.r.rotate_all_around(self.r.centre, pi / 4) self.r.tete.dir_rel = Vecteur(1, 0, 0) self.r.update() self.assertEqual(self.r.centre, self.r.tete.centre) self.assertEqual(self.r.direction, self.r.tete.direction) print("Done")
def test_eq(self): p = self.p.clone() self.assertEqual(p, self.p, None) v = Vecteur( random.randint(1, 5) * random.choice([1, -1]), random.randint(1, 5) * random.choice([1, -1]), random.randint(1, 5) * random.choice([1, -1])) p = self.p.clone().move(v) self.assertNotEqual(p, self.p, None)
def hook(dct): if dct["__class__"] == Vecteur.__name__: return Vecteur.hook(dct) elif dct["__class__"] == Roue.__name__: return Roue.hook(dct) elif dct["__class__"] == Pave.__name__: return Pave.hook(dct) elif dct["__class__"] == Robot.__name__: return Robot(**dct)
def __init__(self, centre: Point = Point(0, 0, 0), direction: Vecteur = Vecteur(1, 0, 0), portee: int or float = 3, arena_v=None, l_ignore=list()): Capteur.__init__(self, centre=centre, direction=direction) self.portee = portee self.arena_v = arena_v self.l_ignore = l_ignore
def hook(dct): """ On ne copie pas la liste d'objets à ingorer""" if dct["__class__"] == Point.__name__: return Point.hook(dct) elif dct["__class__"] == Vecteur.__name__: return Vecteur.hook(dct) elif dct["__class__"] == Arene.__name__: return Arene.hook(dct) elif dct["__class__"] == VueMatriceArene.__name__: return VueMatriceArene(dct["arene"], dct["origine"], dct["ox"], dct["ajuste"], dct["matrice"])
def test_add_list(self): self.a.empty() l = [Objet3D(Point(0, 0, 0)).clone() for i in range(self.n)] self.a.add_list(l) self.assertEqual(len(l), len(self.a.objets3D)) for i in range(self.n): self.a.objets3D[i].centre.move(Vecteur(1, 0, 0)) self.assertNotEqual(l[0], self.a.objets3D[0])
def test_move(self): """ Teste si les capteurs suivent bien la tête (en position et direction) """ # Déplacement d'une distance aléatoire d = (random.random()*0.5+0.5)*random.randint(5, 10) self.tete.move(Vecteur(1,0,0)*d) self.tete.update() # On teste l'égalité des centre pour chaque capteur for key in self.tete.sensors.keys(): self.assertEqual(self.tete.centre, self.tete.sensors[key].centre)
def test_rotate_all_around(self): v = Vecteur( random.randint(0, 5) * random.choice([1, -1]), random.randint(0, 5) * random.choice([1, -1]), random.randint(0, 5) * random.choice([1, -1])) teta = random.choice([1, -1]) * random.random() * 2 * pi v2 = (self.p.vertices[1] - self.p.vertices[0]).to_vect() self.p.rotate_all_around(self.p.centre + v, teta) angle = v2.get_angle() - (self.p.vertices[1] - self.p.vertices[0]).to_vect().get_angle() # On veut une précision au millième self.assertLess(abs(abs(angle) % pi - abs(teta) % pi), 0.001, None)
def __init__(self, arene: Arene, origine=Point(0, 0, 0), ox=Vecteur(1, 0, 0), ajuste=False): """Initialise la vue de la matrice :param arene: Arène à analyser :param origine: Origine O du repère de la vue :param ox: Vecteur Ox unitaire de ce même repère :param ajuste: A True, à l'analyse, prends Ox tourné de -45, pour centrer la vue """ self.origine = origine self.arene = arene self.ox = ox self.ajuste = ajuste self.matrix = None
def __init__(self, arene: Arene = Arene(), origine: Point = Point(0, 0, 0), ox: Vecteur = Vecteur(1, 0, 0), ajuste: bool = False, matrice: [[int]] = None): """ Initialise la matrice :param arene: Arène dont on prélève une portion :param origine: Origine de la vue, dans le repère de l'arène :param ox: Direction horizontale de la vue, dans le repère de l'arène :param ajuste: On représente la matrice selon Ox tourné de -45 degrés dans le sens trigonométrique """ self.origine = origine self.arene = arene self.ox = ox self.ajuste = ajuste self.matrice = matrice # Destiné à stocker la dernière matrice crée
def test_rotate_around(self): v = Vecteur( random.randint(0, 5) * random.choice([1, -1]), random.randint(0, 5) * random.choice([1, -1]), random.randint(0, 5) * random.choice([1, -1])) teta = random.choice([1, -1]) * random.random() * 2 * pi v2 = (self.p.vertices[1] - self.p.vertices[0]).to_vect() self.p.rotate_around(self.p.centre + v, teta) angle = v2.get_angle() - (self.p.vertices[1] - self.p.vertices[0]).to_vect().get_angle() self.assertLess(abs(angle) % pi, 0.001, None) p = self.p.clone() n_centre = (self.p.centre + v) + (self.p.centre - (self.p.centre + v)).to_vect().rotate(teta) v = n_centre - self.p.centre self.assertEqual(p.move(v), self.p, None)
def test_move(self): v = Vecteur( random.choice([1, -1]) * random.randint(0, 5), random.choice([1, -1]) * random.randint(0, 5), random.choice([1, -1]) * random.randint(0, 5)) f = self.r.forme.clone() c = self.r.centre.clone() rg = self.r.rg.clone() rd = self.r.rd.clone() self.r.move(v) rd.move(v) rg.move(v) f.move(v) c.move(v) self.assertEqual(self.r.forme, f) self.assertEqual(self.r.centre, c) self.assertEqual(self.r.rd, rd) self.assertEqual(self.r.rg, rg)
class TestVecteur(unittest.TestCase): def setUp(self): self.vector = Vecteur(1, 0, 0) def test_initialisation(self): print("Testing initialisation...") self.assertIsInstance(self.vector, Vecteur, msg="Error initialisation") coords_types_bools = [ type(self.vector.x) is int or type(self.vector.x) is float or int, type(self.vector.y) is int or type(self.vector.y) is float or int, type(self.vector.z) is int or type(self.vector.z) is float or int ] for b in coords_types_bools: self.assertEqual(b, True, msg="Error initialisation") print("Done") def test_to_point(self): """ Teste la conversion en point """ print("Testing to_point...") v = self.vector.to_point() self.assertIsInstance(v, Point, msg="Error conversion") coords_equal = [ v.x == self.vector.x, v.y == self.vector.y, v.z == self.vector.z ] for b in coords_equal: self.assertEqual(b, True) print("Done") def test_rotate(self): """ Tourne un vecteur et vérifie qu'il a tourné du bon angle """ print("Testing rotate...") s = [1, -1] teta = rm.random() * rm.choice(s) * 2 * pi n = self.vector.get_mag() self.vector.rotate(teta) err_angle = abs( fonctions.positive_angle(teta) - self.vector.get_angle(signe=1)) * 180 / pi err_distance = abs(n - self.vector.get_mag()) self.assertAlmostEqual(err_distance, 0, msg="Error distance") self.assertAlmostEqual(err_angle, 0, msg="Error angle") print("Done")
def test_attach(self): """ Vérifie que la tête suit bien le centre et la direction à laquelle elle est attachée """ # On part de sa position et sa direction direction=self.tete.dir_robot.clone() centre = self.tete.centre.clone() self.tete.attach(centre, direction) # Déplacement et rotation des références d = (random.random()*0.5+0.5)*random.randint(5, 10) angle = random.random() * 2 * pi centre.move(Vecteur(1,0,0)*d) direction.rotate(angle) self.tete.update() # On vérifie que la tête a bien suivi le mouvement self.assertEqual(direction, self.tete.direction) self.assertEqual(centre, self.tete.centre) for key in self.tete.sensors.keys(): self.assertEqual(direction, self.tete.sensors[key].direction) self.assertEqual(centre, self.tete.sensors[key].centre)
def __init__(self, centre: Point = Point(0, 0, 0), direction: Vecteur = Vecteur(1, 0, 0)): Objet3D.__init__(self, centre) self.direction = direction
def setUp(self): """ Crée une caméra """ self.obj = Camera(Point(0.2, 0.2, 0.5), Vecteur(1, 0, 0))
def setUp(self): self.v2 = Vecteur(1,0,0) self.p0 = Point(1,1,0.5) self.strat = DroitVersBaliseVision(RobotMotorise(forme=Pave(1,1,1,self.p0.clone()), direction=self.v2.clone()), AreneFermee(5,5,5)) self.target = RobotTarget(forme=Pave(1,1,1, self.p0.clone()+self.v2*3), direction=self.v2.clone()) self.strat.arene.add(self.target)
class TestDroitVersBalise(unittest.TestCase): """ TODO: S'arranger pour que la direction u capteur ir suive bien celle de la tête """ def setUp(self): self.v2 = Vecteur(1,0,0) self.p0 = Point(1,1,0.5) self.strat = DroitVersBaliseVision(RobotMotorise(forme=Pave(1,1,1,self.p0.clone()), direction=self.v2.clone()), AreneFermee(5,5,5)) self.target = RobotTarget(forme=Pave(1,1,1, self.p0.clone()+self.v2*3), direction=self.v2.clone()) self.strat.arene.add(self.target) def test_target_straith_trajectory(self): self.strat2 = DeplacementDroit(robot=self.target,vitesse= 30, distance_max=2) td = Thread(target=self.strat.start_3D) sim = Simulation(strategies=[self.strat2, self.strat], tmax=15, tic=3, tic_display=[self.strat], final_actions=[self.strat.stop_3D]) td.start() while not self.strat.robot.tete.sensors["cam"].is_set: pass sim.start() while not sim.stop: pass def test_target_circle_trajectectory(self): self.strat = DroitVersBaliseVision(RobotMotorise(forme=Pave(1,1,1,self.p0.clone()), direction=self.v2.clone()), AreneFermee(10,10,10)) self.strat2 = DeplacementCercle(robot=self.target,vitesse=30, diametre=3, angle_max=360) td = Thread(target=self.strat.start_3D) sim = Simulation(strategies=[self.strat2, self.strat], tmax=15, tic=1,tic_display=[self.strat], final_actions=[self.strat.stop_3D]) td.start() while not self.strat.robot.tete.sensors["cam"].is_set: pass sim.start() while not sim.stop: pass self.assertNotEqual(self.strat.last_detected, None)
def setUp(self): self.r = RobotMotorise(pave=Pave(1, 1, 0, centre=Point(5, 5, 0)), direction=Vecteur(1, 0, 0))
def get_inclinaisont(self): """ Retourne la différence d'angle entre un des côtés du pavé sur le plan Oxy, et le vecteur (1,0,0) """ return (self.vertices[1] - self.vertices[0]).to_vect().diff_angle( Vecteur(1, 0, 0))
def __init__(self, arene): Thread.__init__(self) self.arene = arene self.window = None def run(self): self.window = AppArene(self.arene) self.window.mainloop() def stop(self): self.window.quit() if __name__ == '__main__': from gl_lib.sim.robot.strategy.deplacement import DeplacementDroitAmeliore from gl_lib.sim.robot import RobotMotorise from gl_lib.sim import Simulation from gl_lib.sim.geometry import Vecteur a = Arene() strat = DeplacementDroitAmeliore( robot=RobotMotorise(direction=Vecteur(1, 1, 0).norm()), distance_max=1, arene=a) a.add(strat.robot) sim = Simulation(strategies=[strat]) app = AppAreneThread(strat.arene) app.start() sim.start()
def setUp(self): self.vector = Vecteur(1, 0, 0)
class Robot(Objet3D, ApproximableAPave): """ Classe definissant les elements essentiels d'un robot """ KEYS = ["direction", "forme", "rg", "rd"] INIT = { "direction": Vecteur(0, 1, 0), "forme": Pave(1, 1, 1, Point(0, 0, 0)), "rg": Objet3D(), "rd": Objet3D() } def __init__(self, **kwargs): """ Initialise les attributs, et place les roues par rapport à la direction On place les roues de part et d'autre de la direction, à 45° :param forme: Forme du robot :type forme: Pave :param rg: Roue droite, représentée comme un point :type rg: Objet3D :param rd: Roue gauche, représentée comme un point :type rd: Objet3D :param direction: Direction du robot :type direction: Vecteur """ Objet3D.__init__(self) keys = kwargs.keys() for key in Robot.INIT.keys(): if not key in keys: kwargs[key] = Robot.INIT[key] self.direction = kwargs["direction"] self.forme = kwargs["forme"] self.rd = kwargs["rd"] self.rg = kwargs["rg"] self.forme.rotate(self.direction.get_angle()) self.centre = self.forme.centre # initalise le centre au centre du pave self.forme.centre = self.centre # initialisation des centres des roues self.rd.centre = self.centre + (self.direction.rotate(-pi / 4) * self.forme.width * sqrt(2) / 2) self.rg.centre = self.centre + (self.direction.rotate(pi / 4) * self.forme.width * sqrt(2) / 2) self.dist_wheels = self.forme.width def __dict__(self): dct = OrderedDict() dct["__class__"] = Robot.__name__ dct["direction"] = self.direction.__dict__() dct["forme"] = self.forme.__dict__() dct["rg"] = self.rg.__dict__() dct["rd"] = self.rd.__dict__() return dct def __str__(self): s = "{}; direction: {}; forme: {}".format(self.__class__.__name__, self.direction, self.forme) return s def __eq__(self, other): if not Objet3D.__eq__(self, other): return False if other.forme != self.forme: return False if other.rd != self.rd or other.rg != self.rg: return False if other.direction != self.direction: return False if other.dist_wheels != self.dist_wheels: return False return True def rotate_around(self, point, angle, axis=None): """ Tourne le robot autour de point d'un angle angle en radians :param point: Point autour duquel tourner :type point: Point :param teta: Angle en radians :type teta: float :param axis: 'x', 'y' ou 'z' """ self.forme.rotate_around(point, angle, axis) self.rd.rotate_around(point, angle, axis) self.rg.rotate_around(point, angle, axis) def rotate_all_around(self, point, angle, axis=None): """ Tourne le Robot et sa direction par rapport à son centre, et autour de point d'un angle en radians :param point: Point autour duquel tourner :type point: Point :param teta: Angle en radians :type teta: float :param axis: 'x', 'y' ou 'z' """ Objet3D.rotate_around(self, point, angle, axis) self.forme.rotate_all_around(point, angle, axis) self.rg.rotate_around(point, angle, axis) self.rd.rotate_around(point, angle, axis) self.direction.rotate(angle, axis) def move(self, vecteur): """ Déplace les composants du robot :param vecteur: Vecteur Déplacement :type vecteur: Vecteur """ self.forme.move(vecteur) self.rg.move(vecteur) self.rd.move(vecteur) def get_pave(self): return self.forme.get_pave() @staticmethod def load(filename): with open(filename, 'r', encoding='utf-8') as f: return json.load(f, object_hook=Robot.hook) @staticmethod def hook(dct): if dct["__class__"] == Vecteur.__name__: return Vecteur.hook(dct) elif dct["__class__"] == Roue.__name__: return Roue.hook(dct) elif dct["__class__"] == Pave.__name__: return Pave.hook(dct) elif dct["__class__"] == Robot.__name__: return Robot(**dct)
elif dct["__class__"] == Arene.__name__: return Arene.hook(dct) elif dct["__class__"] == VueMatriceArene.__name__: return VueMatriceArene(dct["arene"], dct["origine"], dct["ox"], dct["ajuste"], dct["matrice"]) @staticmethod def load(filename): """ Charge un objet VueMatriceArene depuis un fichier au format json adapté :param filename: """ with open(filename, 'r', encoding='utf-8') as f: return json.load(f, object_hook=VueMatriceArene.hook) if __name__ == '__main__': v = Vecteur(1, 1, 0).norm() c = CapteurIR() c.save("capteurIR.json") dct = c.__dict__() print(dct) c3 = CapteurIR.hook(dct) print(c3) # c2 = CapteurIR.load("capteurIR.json")