def prizma(ugol1, ugol2, maxHUst):
from math import tan as mt
from math import radians as rd
'''Угол устойчивого,рабочего уступов, максим высота уступа'''
z(1 / (mt(rd(ugol1))))
z(1 / (mt(rd(ugol2))))
return round(maxHUst * (1 / (mt(rd(ugol1))) - 1 / (mt(rd(ugol2)))), 2)
def dist_calculation(self, lat, lon):
"""Function for radial distance calculation
between two locations lat being latitude
and lon being longitude"""
lat = rd(lat)
lon = rd(lon)
del_phi = acos(
sin(lat) * sin(self.OLAT) +
cos(lat) * cos(self.OLAT) * cos(lon - self.OLON))
return round(del_phi * self.r, 2)
def draw_orientation_star(self, color, point, scaled_x, scaled_y, window,
windows_qty, x_bounds, y_bounds, relation_ctrl,
m):
i = 0
px = 7. * cos(rd(360 - self.orientation)) + scaled_x
py = 7. * sin(rd(360 - self.orientation)) + scaled_y
pygame.draw.line(window, color, (int(scaled_x), int(scaled_y)),
(int(px), int(py)), 1)
pygame.draw.circle(window, color, (int(scaled_x), int(scaled_y)), 3, 0)
self.draw_starvars(window, color, point, scaled_x, scaled_y, i,
windows_qty, x_bounds, y_bounds, relation_ctrl, m)
def rotate(self, a, dim, norm=(0, 0, 0)):
#rotates through dimension by angle a.
a = rd(a)
if dim == "x":
m = ((
1,
0,
0,
), (0, cos(a), -sin(a)), (0, sin(a), cos(a)))
elif dim == "y":
m = ((cos(a), 0, -sin(a)), (0, 1, 0), (sin(a), 0, cos(a)))
elif dim == "z":
m = ((cos(a), -sin(a), 0), (sin(a), cos(a), 0), (0, 0, 1))
else:
raise ValueError
self.map(m, norm)
def rotate(self, a, dim, norm = (0, 0, 0)):
#rotates through dimension by angle a.
a = rd(a)
if dim == "x":
m = ((1, 0, 0,),
(0, cos(a), -sin(a)),
(0, sin(a), cos(a)))
elif dim =="y":
m = ((cos(a), 0, -sin(a)),
(0, 1, 0),
(sin(a), 0, cos(a)))
elif dim == "z":
m = ((cos(a), -sin(a), 0),
(sin(a), cos(a), 0),
(0, 0, 1))
else:
raise ValueError
self.map(m, norm)
def modeling(self, data_matrix):
xy = [[]]
#print xy
for i in range(2 * self.number_of_entities):
xy[0].append([0])
ref_dist = 1
triangles_angles = []
distances = []
for i in range(0, self.number_of_entities - 2):
a = 0
b = 1
c = 2 + i
triangles_angles.append([])
distances.append([])
for j in range(0, 2): #2 angles
filtering = data_matrix[np.where(data_matrix[:, 0] == a)]
print 'filtering\n', filtering
filtering2 = filtering[np.where(filtering[:, 5] == b)]
print 'filtering2\n', filtering2
alpha = filtering2[0][2]
filtering2 = filtering[np.where(filtering[:, 5] == c)]
print 'filtering2\n', filtering2
beta = filtering2[0][2]
angle = abs(alpha - beta)
if angle >= 180:
angle = abs(angle - 360)
triangles_angles[i].append(angle)
aux = a
a = b
b = c
c = aux
triangles_angles[i].append(180 - (triangles_angles[i][0] +
triangles_angles[i][1]))
#distances
for j in range(0, len(triangles_angles[i])):
if j == len(triangles_angles[i]) - 1:
distances[i].append(ref_dist)
else:
try:
distances[i].append(
ref_dist * sin(rd(triangles_angles[i][j])) / sin(
rd(triangles_angles[i][len(triangles_angles[i])
- 1])))
except:
distances[i].append(ref_dist *
sin(rd(triangles_angles[i][j])) /
sin(rd(1)))
filtering = data_matrix[np.where(data_matrix[:, 0] == 0)]
for i in range(self.number_of_entities):
if i != 0:
filtering2 = filtering[np.where(filtering[:, 5] == i)]
alpha = filtering2[0][2]
if i == 1:
#x:
xy[0][i] = [cos(rd(alpha)) * ref_dist]
#y:
xy[0][(i + self.number_of_entities)] = [
sin(rd(alpha)) * ref_dist
]
else:
#x:
xy[0][i] = [cos(rd(alpha)) * distances[i - 2][1]]
#y:
xy[0][(i + self.number_of_entities)] = [
sin(rd(alpha)) * distances[i - 2][1]
]
#print "============="
#print 'triangles_angles', triangles_angles
#print 'distances', distances
#print xy
#print "============="
self.xy = xy
for i in range(self.active_entities):
filtering = data_matrix[np.where(data_matrix[:, 0] == i)]
#if Debug: print 'filtering\n', filtering
self.overall_orientation_answer.append(filtering[0][1])
def Decide_StarVars(self, variable_list, h_element):
G = []
for i in range(0, 2 * len(self.data_matrix)):
G.append(2 * self.total_entities * [0.])
G = np.array(G)
k = 0
self.overall_orientation = list(self.overall_orientation)
number_missing_orientations = self.overall_orientation.count(-1)
ctrl_orient = 0
ctrl_relations = number_missing_orientations
for i in range(0, len(self.oriented_points)):
aux = self.data_matrix[np.where(self.data_matrix[:, 0] == i)]
angle = rd(aux[0][1])
if self.overall_orientation[i] == -1:
angle = rd(variable_list[ctrl_orient] * self.eta)
ctrl_orient += 1
for j in range(0, self.total_entities):
if (i != j):
aux2 = aux[np.where(aux[:, 5] == j)]
relation1 = rd(aux2[0][2] * self.eta)
relation2 = rd(((aux2[0][3] + 1) % self.m) * self.eta)
if self.overall_relations[i][j] == -1:
relation1 = rd(variable_list[ctrl_relations] *
self.eta)
relation2 = rd(
((variable_list[ctrl_relations] + 1) % self.m) *
self.eta)
ctrl_relations += 1
G[k][i] = -sin(angle + relation1)
G[k][j] = sin(angle + relation1)
G[k + 1][i] = sin(angle + relation2)
G[k + 1][j] = -sin(angle + relation2)
G[k][i + self.total_entities] = cos(angle + relation1)
G[k][j + self.total_entities] = -cos(angle + relation1)
G[k + 1][i + self.total_entities] = -cos(angle + relation2)
G[k + 1][j + self.total_entities] = cos(angle + relation2)
k += 2
h = []
for i in range(0, len(G)):
if i % 2 == 0:
if self.h_ctrl == 0:
h.append(-0.3) #-0.3
else:
h.append(0.)
else:
h.append(h_element)
h = matrix(h)
Aeq = None
beq = None
### to keep standing points at the same place:
if self.model != None:
b = []
A = np.identity(self.total_entities * 2)
A = np.delete(A, self.driven, 0)
A = np.delete(A, self.driven + self.total_entities - 1, 0)
A_array = np.array(A)
A_t = [list(i) for i in zip(*A_array)]
Aeq = matrix(A_t)
Aeq2 = np.array(Aeq)
for i in range(0, len(self.model.points)):
if i != self.driven:
b.append(self.model.points[i].x)
for i in range(0, len(self.model.points)):
if i != self.driven:
b.append(self.model.points[i].y)
beq = matrix(b)
#print '===== original constraints matrix ====='
#print G
#print '======================='
G = [list(i) for i in zip(*G)]
G = matrix(G)
c = 2 * self.total_entities * [0.]
c = matrix(c)
solvers.options['glpk'] = {
'msg_lev': 'GLP_MSG_OFF',
'meth': 'GLP_PRIMAL',
'tol_bnd': 1e-4
}
sol = solvers.lp(c, G, h, Aeq, beq, solver='glpk')
#print 'variable_list', variable_list
#print(sol['x'])
if sol['status'] == 'optimal':
self.xy_answer.append(np.array(sol['x']))
#print 'StarVars answer:'
#print(sol['x'])
#print 'variable_list', variable_list
temp = list(self.overall_orientation)
for i in range(0, number_missing_orientations):
for j in range(0, len(temp)):
if temp[j] == -1: # or temp[j] == -2:
temp[j] = variable_list[i] * self.eta
break
self.overall_orientation_answer.append(temp)
#print 'self.overall_relations', self.overall_relations
temp_r = copy.deepcopy(self.overall_relations)
print "variable_list", variable_list
print "temp_r", temp_r
h = number_missing_orientations
for i in range(0, len(temp_r)):
for j in range(0, len(temp_r[0])):
if temp_r[i][j] == -1: # or temp_r[i][j] == -2:
temp_r[i][j] = variable_list[h]
h += 1
#break
self.overall_relations_answer = copy.deepcopy(temp_r)
print "self.overall_relations_answer_teste", self.overall_relations_answer
#if self.model != None:
self.force_recursive_break = 1
###loop for allowing that a robot can be found
while i < 25:
i = i + 1
time.sleep(1)
if bkb.read_int(mem, 'VISION_LOST') == 1:
bkb.write_int(mem, 'DECISION_SEARCH_ON', 1)
vision_orient = "NF"
else:
bkb.write_int(mem, 'DECISION_SEARCH_ON', 0)
if j == goal:
sumx_cos = 0
sumy_sin = 0
for k in range(30):
time.sleep(0.05)
print 'VISION DEG', bkb.read_float(mem, 'VISION_PAN_DEG')
sumy_sin = sumy_sin + sin(rd(bkb.read_float(mem, 'VISION_PAN_DEG')))
sumx_cos = sumx_cos + cos(rd(bkb.read_float(mem, 'VISION_PAN_DEG')))
vision_orient = degrees(atan2(sumy_sin, sumx_cos))
relation_goal = starvars_discretization(vision_orient, m, 2)
else:
sumx_cos = 0
sumy_sin = 0
for k in range(30):
time.sleep(0.05)
print 'VISION_RBT01_ANGLE', bkb.read_float(mem, 'VISION_RBT01_ANGLE')
sumy_sin = sumy_sin + sin(rd(bkb.read_float(mem, 'VISION_RBT01_ANGLE')))
sumx_cos = sumx_cos + cos(rd(bkb.read_float(mem, 'VISION_RBT01_ANGLE')))
vision_orient = degrees(atan2(sumy_sin, sumx_cos))
print vision_orient
###StarVars discretization
def __init__(self):
self.OLAT = rd(conf.OLAT)
self.OLON = rd(conf.OLON)
self.r = conf.r
def draw_starvars(self, screen, color, point, scaled_x, scaled_y, j,
windows_qty, x_bounds, y_bounds, relation_ctrl, m):
windows_qty = 1
width, height = screen.get_size()
resolution = 360.0 / m
side_size = width / windows_qty
farthest_boundary = 10
new_x = cos(rd(self.orientation)) * farthest_boundary + int(scaled_x)
new_y = int(scaled_y) - sin(rd(self.orientation)) * farthest_boundary
#print 'j, windows_qty', j, windows_qty
while (new_x > (j % windows_qty) * side_size and new_x <
((j % windows_qty) + 1) * side_size
and new_y > int(j / windows_qty) * side_size and new_y <
(int(j / windows_qty) + 1) * side_size):
new_x = cos(rd(
self.orientation)) * farthest_boundary + int(scaled_x)
#line_ctrl = int(j / windows_qty) + 1
new_y = int(scaled_y) - sin(rd(
self.orientation)) * farthest_boundary
farthest_boundary += 2
#print new_x, new_y, farthest_boundary
pygame.draw.line(screen, color, (int(scaled_x), int(scaled_y)),
(new_x, new_y), 3)
angles = []
for i in range(m):
angles.append(resolution * i)
for i in range(1, len(angles)):
farthest_boundary = 10
new_x = cos(rd(self.orientation +
angles[i])) * farthest_boundary + int(scaled_x)
new_y = int(scaled_y) - sin(
rd(self.orientation + angles[i])) * farthest_boundary
while (new_x > (j % windows_qty) * side_size and new_x <
((j % windows_qty) + 1) * side_size
and new_y > int(j / windows_qty) * side_size and new_y <
(int(j / windows_qty) + 1) * side_size):
new_x = cos(rd(self.orientation +
angles[i])) * farthest_boundary + int(scaled_x)
new_y = int(scaled_y) - sin(
rd(self.orientation + angles[i])) * farthest_boundary
farthest_boundary += 2
if relation_ctrl[point] == "even":
if i % 2 == 0:
pygame.draw.line(screen, color,
(int(scaled_x), int(scaled_y)),
(new_x, new_y), 1)
else:
self.draw_dashed_line(screen, color,
(int(scaled_x), int(scaled_y)),
(int(new_x), int(new_y)))
elif relation_ctrl[point] == "odd":
if i % 2 == 0:
self.draw_dashed_line(screen, color,
(int(scaled_x), int(scaled_y)),
(int(new_x), int(new_y)))
else:
pygame.draw.line(screen, color,
(int(scaled_x), int(scaled_y)),
(new_x, new_y), 1)
else:
pygame.draw.line(screen, color, (int(scaled_x), int(scaled_y)),
(new_x, new_y), 1)
def draw_orientation(self, color, scaled_x, scaled_y, window):
px = 7. * cos(rd(360 - self.orientation)) + scaled_x
py = 7. * sin(rd(360 - self.orientation)) + scaled_y
pygame.draw.line(window, color, (int(scaled_x), int(scaled_y)),
(int(px), int(py)), 1)
pygame.draw.circle(window, color, (int(scaled_x), int(scaled_y)), 3, 0)
