-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSpecialFormations.py
60 lines (51 loc) · 3.05 KB
/
SpecialFormations.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
from Formation import *
class SquareFormationDisplacementSingleIntegrator6Edges(SquareFormationDisplacementSingleIntegrator):
def setFormationCostraint(self, robotVector):
robotVector = sorted(robotVector, key=lambda item: item.role)
robotVector[0].disconnect()
robotVector[1].disconnect()
robotVector[2].disconnect()
robotVector[3].disconnect()
Robot.connect(robotVector[0], robotVector[1])
Robot.connect(robotVector[1], robotVector[2])
Robot.connect(robotVector[2], robotVector[3])
Robot.connect(robotVector[3], robotVector[0])
Robot.connect(robotVector[0], robotVector[2])
Robot.connect(robotVector[1], robotVector[3])
robotVector[0].hashRole[1] = [0, self.side]
robotVector[1].hashRole[0] = [0, -self.side]
robotVector[1].hashRole[2] = [self.side, 0]
robotVector[2].hashRole[1] = [-self.side, 0]
robotVector[2].hashRole[3] = [0, -self.side]
robotVector[3].hashRole[2] = [0, self.side]
robotVector[0].hashRole[3] = [self.side, 0]
robotVector[3].hashRole[0] = [-self.side, 0]
robotVector[0].hashRole[2] = [self.side, self.side]
robotVector[2].hashRole[0] = [-self.side, -self.side]
robotVector[1].hashRole[3] = [self.side, -self.side]
robotVector[3].hashRole[1] = [-self.side, self.side]
class SquareFormationDisplacementDoubleIntegrator6Edges(SquareFormationDisplacementDoubleIntegrator):
def setFormationCostraint(self, robotVector):
robotVector = sorted(robotVector, key=lambda item: item.role)
robotVector[0].disconnect()
robotVector[1].disconnect()
robotVector[2].disconnect()
robotVector[3].disconnect()
Robot.connect(robotVector[0], robotVector[1])
Robot.connect(robotVector[1], robotVector[2])
Robot.connect(robotVector[2], robotVector[3])
Robot.connect(robotVector[3], robotVector[0])
Robot.connect(robotVector[0], robotVector[2])
Robot.connect(robotVector[1], robotVector[3])
robotVector[0].hashRole[1] = [[0, self.side], [self.desVelX, self.desVelY]]
robotVector[1].hashRole[0] = [[0, -self.side], [self.desVelX, self.desVelY]]
robotVector[1].hashRole[2] = [[self.side, 0], [self.desVelX, self.desVelY]]
robotVector[2].hashRole[1] = [[-self.side, 0], [self.desVelX, self.desVelY]]
robotVector[2].hashRole[3] = [[0, -self.side], [self.desVelX, self.desVelY]]
robotVector[3].hashRole[2] = [[0, self.side], [self.desVelX, self.desVelY]]
robotVector[0].hashRole[3] = [[self.side, 0], [self.desVelX, self.desVelY]]
robotVector[3].hashRole[0] = [[-self.side, 0], [self.desVelX, self.desVelY]]
robotVector[0].hashRole[2] = [[self.side, self.side], [self.desVelX, self.desVelY]]
robotVector[2].hashRole[0] = [[-self.side, -self.side], [self.desVelX, self.desVelY]]
robotVector[1].hashRole[3] = [[self.side, -self.side], [self.desVelX, self.desVelY]]
robotVector[3].hashRole[1] = [[-self.side, self.side], [self.desVelX, self.desVelY]]