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 test_rotate_all_around(self):
p = Point(
random.choice([1, -1]) * random.randint(0, 5),
random.choice([1, -1]) * random.randint(0, 5),
random.choice([1, -1]) * random.randint(0, 5)) + self.r.centre
teta = random.random() * 2 * pi * random.choice([1, -1])
f = self.r.forme.clone()
c = self.r.centre.clone()
rg = self.r.rg.clone()
rd = self.r.rd.clone()
v = self.r.direction.clone()
print(v)
self.r.rotate_all_around(p, teta)
rd.rotate_around(p, teta)
rg.rotate_around(p, teta)
f.rotate_all_around(p, teta)
c.rotate_around(p, teta)
v.rotate(teta)
self.assertEqual(self.r.direction, v)
self.assertEqual(self.r.forme, f)
self.assertEqual(self.r.centre, c)
self.assertEqual(self.r.direction, v)
self.assertEqual(self.r.rd, rd)
self.assertEqual(self.r.rg, rg)
def test_rotate_around(self):
"""
Teste la rotation
TODO: Il y à un problème quand la coordonnée z est non nulle
TODO: Peut être très imprécis dans certains cas, à corriger, ou trouver les cas pathologiques
"""
print("Testing rotate_around...")
s = [-1, 1]
p = Point(
rm.choice(s) * rm.randint(2, 5),
rm.choice(s) * rm.randint(2, 5), 0)
teta = rm.random() * rm.choice(s) * pi
v = (self.dot - p).to_vect()
n = v.get_mag()
aux = (v * 100).rotate(teta)
aux = aux * n / aux.get_mag() + p
self.dot.rotate_around(p, teta)
v2 = (aux.to_point() - p).to_vect()
d_dist = abs(v.get_mag() - v2.get_mag())
d_angle = v.diff_angle(v2, signe=1)
self.assertAlmostEqual(d_angle,
fonctions.positive_angle(teta),
msg="Error angle")
self.assertAlmostEqual(d_dist, 0, msg="Error distance")
print("Done")
def setUp(self):
self.distance = 1
self.strat = DeplacementDroit(
robot=RobotMotorise(forme=Pave(1, 1, 1, Point(3, 3, 0))),
distance_max=self.distance)
self.arene = Arene()
self.arene.add(self.strat.robot)
def hook(dct):
if dct["__class__"] == Point.__name__:
return Point.hook(dct)
elif dct["__class__"] == Pave.__name__:
return Pave(
dct["width"], dct["length"], dct["height"], dct["centre"],
[dct["vertices"][i] for i in range(len(dct["vertices"]))])
def hook(dct):
if dct["__class__"] == Point.__name__:
return Point.hook(dct)
elif dct["__class__"] == Polygone3D.__name__:
return Polygone3D(centre=dct["centre"],
vertices=[
dct["vertices"][i]
for i in range(len(dct["vertices"]))
])
def test_json(self):
print("Testing json format...")
try:
self.dot.save("dot.json")
d2 = Point.load("dot.json")
self.assertEqual(self.dot, d2)
except PermissionError:
print("Permission Error")
print("Done")
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 __init__(self, centre=Point(0, 0, 0), vertices=list()):
"""
Initialise la liste des sommets
:param centre: Centre du polygône
:type centre: Point
:param vertices: Centre du polygône
:type vertices: [Point]
"""
Objet3D.__init__(self, centre=centre)
self.vertices = vertices
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 __init__(self, centre=Point(0, 0, 0), diametre=0.5, height=1):
"""
:param centre: Centre du cylindre
:type centre: Point
:param diametre: Diametre du cylindre
:type diametre: float
:param height: Hauteur du cylindre
:type height: float
"""
Objet3D.__init__(self, centre)
self.diametre = diametre
self.height = height
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 __init__(self,
width: float or int,
length: float or int,
height: float or int,
centre=Point(0, 0, 0),
vertices: [Point] = None):
"""
Constructeur ajoutant les 8 sommets autour du centre par défaut: (0,0,0)
"""
Polygone3D.__init__(self, centre, vertices)
self.width = float(width)
self.length = float(length)
self.height = float(height)
if vertices is None:
self.vertices = list()
self.add_vertex(
Point(self.centre.x - width / 2, self.centre.y - length / 2,
self.centre.z + height / 2))
self.add_vertex(
Point(self.centre.x + width / 2, self.centre.y - length / 2,
self.centre.z + height / 2))
self.add_vertex(
Point(self.centre.x + width / 2, self.centre.y + length / 2,
self.centre.z + height / 2))
self.add_vertex(
Point(self.centre.x - width / 2, self.centre.y + length / 2,
self.centre.z + height / 2))
self.add_vertex(
Point(self.centre.x - width / 2, self.centre.y - length / 2,
self.centre.z - height / 2))
self.add_vertex(
Point(self.centre.x + width / 2, self.centre.y - length / 2,
self.centre.z - height / 2))
self.add_vertex(
Point(self.centre.x + width / 2, self.centre.y + length / 2,
self.centre.z - height / 2))
self.add_vertex(
Point(self.centre.x - width / 2, self.centre.y + length / 2,
self.centre.z - height / 2))
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):
p = Point(
random.choice([1, -1]) * random.randint(0, 5),
random.choice([1, -1]) * random.randint(0, 5),
random.choice([1, -1]) * random.randint(0, 5)) + self.r.centre
teta = random.random() * 2 * pi * random.choice([1, -1])
f = self.r.forme.clone()
c = self.r.centre.clone()
rg = self.r.rg.clone()
rd = self.r.rd.clone()
self.r.rotate_around(p, teta)
rd.centre.rotate_around(p, teta)
rg.centre.rotate_around(p, teta)
f.centre.rotate_around(p, teta)
c.rotate_around(p, teta)
self.assertEqual(self.r.forme.centre, f.centre)
self.assertEqual(self.r.centre, c)
self.assertEqual(self.r.rd.centre, rd.centre)
self.assertEqual(self.r.rg.centre, rg.centre)
def test_view(self):
"""
Affiche une arène vide en 3D, effectue quelques rotations
"""
print("Testing video...")
td = Thread(target=self.obj.run)
p = RobotTarget(forme=Pave(1, 1, 1, Point(3, 3, 0.5)))
a = Arene()
a.add(p)
n_rot = 100
teta = 2 * pi / n_rot
self.obj.direction = (p.centre - self.obj.centre).to_vect().norm()
td.start()
while not self.obj.is_set:
pass
for i in range(n_rot):
p.rotate_all_around(p.centre, teta)
sleep(PAS_TEMPS)
self.obj.stop()
print("Done")
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):
self.r = RobotMotorise(pave=Pave(1, 1, 0, centre=Point(5, 5, 0)),
direction=Vecteur(1, 0, 0))
def setUp(self):
self.dot = Point(1, 1, 0)
: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)
if __name__ == '__main__':
r = Robot(forme=Pave(1, 1, 1, Point(30, 30, 0)))
print(r.forme)
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)
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)
return s
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))
@staticmethod
def load(filename):
with open(filename, 'r', encoding='utf-8') as f:
return json.load(f, object_hook=Pave.hook)
@staticmethod
def hook(dct):
if dct["__class__"] == Point.__name__:
return Point.hook(dct)
elif dct["__class__"] == Pave.__name__:
return Pave(
dct["width"], dct["length"], dct["height"], dct["centre"],
[dct["vertices"][i] for i in range(len(dct["vertices"]))])
def get_pave(self):
return self
if __name__ == '__main__':
p = Pave(150, 200, 0, Point(50, 50, 0))
print(p)
:param point: Point autour duquel tourner
:type point: Point
:param teta: Angle en radians
:type teta: float
:param axis: 'z', 'x' ou 'y'
"""
self.centre.rotate_around(point, teta, axis)
@staticmethod
def hook(dct):
if dct["__class__"] == Objet3D.__name__:
return Objet3D(dct["centre"])
elif dct["__class__"] == Point.__name__:
return Point.hook(dct)
@staticmethod
def load(filename):
with open(filename, 'r', encoding='utf-8') as f:
return json.load(f, object_hook=Objet3D.hook)
if __name__ == '__main__':
o = Objet3D(Point(0, 0, 0))
o.save("objet3D.json")
o2 = Objet3D.load("objet3D.json")
print(o2)
for i in range(len(dct["vertices"]))
])
@staticmethod
def load(filename):
"""
Permet de charger un objet Polygône3D depuis un fichier au format json adapté
:param filename: Nom du fichier
"""
with open(filename, 'r', encoding='utf-8') as f:
return json.load(f, object_hook=Polygone3D.hook)
def clone(self):
d = self.__dict__()
return Polygone3D(centre=d["centre"], vertices=d["vertices"])
if __name__ == '__main__':
from gl_lib.sim.geometry import *
p = Polygone3D(vertices=[Point(1, 1, 1)])
d = p.__dict__()
p.save("polygone3D.json")
p2 = Polygone3D.hook(d)
print(p2)
p3 = Polygone3D.load("polygone3D.json")
def setUp(self):
self.angle = rm.random() * rm.choice([-1, 1]) * pi
self.strat = Tourner(robot=RobotMotorise(Pave(1,1,1,Point(2,2,0))), angle_max=(self.angle * 180 / pi))
self.arene = Arene()
self.arene.add(self.strat.robot)
def hook(dct):
if dct["__class__"] == Objet3D.__name__:
return Objet3D(dct["centre"])
elif dct["__class__"] == Point.__name__:
return Point.hook(dct)
