def run_checking_sampling_bridge(self, rob, grapple2, ee1flag):
tester = test_robot(self)
tolerate_error = 1e-5
minMax = self.get_min_max_len()
numSeg = self.get_num_segment()
robPos = rob.get_position()
headPos = robPos[0]
endPos = rob.get_EndeePos()
arr = []
con = True
flag, arr = self.check_sampling_bridge(rob, grapple2, ee1flag)
if flag:
return arr
else:
# print(rob)
# print(robPos[-2])
xlen = -(robPos[-2][0] - grapple2[0])
ylen = -(robPos[-2][1] - grapple2[1])
difflen = math.sqrt(xlen * xlen + ylen * ylen)
if (difflen >= minMax[0][-1] and difflen <= minMax[1][-1]):
newAng = Angle.atan2(ylen, xlen)
newAng1 = Angle.atan2(-ylen, -xlen)
if ee1flag:
robArr = rob.str2list()
angSum = 0
for a in range(numSeg - 1):
angSum += robArr[2 + a]
robArr[2 + numSeg - 1] = newAng.in_degrees() - angSum
robArr[2 + numSeg * 2 - 1] = difflen
robNew = make_robot_config_with_arr(
robArr[0], robArr[1], robArr[2:(2 + numSeg)],
robArr[(2 + numSeg):(2 + numSeg * 2)], robArr[-1])
if (tester.self_obstacle_test(robNew)):
# print(robNew)
flagNew, arrNew = self.check_sampling_bridge(
robNew, grapple2, ee1flag)
if flagNew:
return arrNew
else:
#===== here might have problem when grapple is ee2 ====
robArr = rob.str2list()
angSum = 0
for a in range(numSeg - 1):
angSum += robArr[2 + a]
robArr[2 + numSeg - 1] = newAng.in_degrees() - angSum
robArr[2 + numSeg] = difflen
robNew = make_robot_config_with_arr(
robArr[0], robArr[1], robArr[2:(2 + numSeg)],
robArr[(2 + numSeg):(2 + numSeg * 2)], robArr[-1])
robNew1 = make_robot_config_with_arr(
robArr[0], robArr[1], robArr[2:(2 + numSeg)],
robArr[(2 + numSeg):(2 + numSeg * 2)], robArr[-1])
flagNew, arrNew = self.check_sampling_bridge(
robNew, grapple2, ee1flag)
if flagNew:
return arrNew
return arr
def get_nearest_obstacle_angle(q, obstacles):
lst = []
p = get_tip(q)
for o in obstacles:
for e in o.edges:
x0 = p[0]
y0 = p[1]
x1 = e[0][0]
y1 = e[0][1]
x2 = e[1][0]
y2 = e[1][1]
yf = abs(y1 - y2)
xf = abs(x1 - x2)
# print("yf = " + str(yf))
# print("xf = " + str(xf))
dist = Angle.atan2(yf, xf)
# print("dist = " + str(dist))
lst.append(dist)
return min(lst)