def main():
# def main(arglist):
file = './testcases/4g1_m1.txt'
outfile = 'out/4g1_m1_output.txt'
# file = arglist[0]
# outfile = arglist[1]
prm = EST(file)
# config = prm.sampling(1000)
# robot = prm.get_init_state();
f = open("tmp_output.txt", "w")
f.write("")
f.close()
prm.run_EST(outfile)
# D = [1e-3,1e-3]
# sampleRobot = prm.sampling_withinD(robot,D,100)
# robot = prm.assign_config(config.tolist(),0)
# print(sampleRobot)
# print(config.tolist())
# config = config[:,:-1]
# prm.write_config('test.txt',config.tolist())
aa = test_robot(prm)
qq = aa.load_output('output.txt')
vis = Visualiser(prm, qq)
def expandTree(self):
k = 1
D = [0.4, 1e-2]
tau = 0.4
pr = round(random.random())
T = self.gGoal if pr == 1 else self.gInit
tester = test_robot(self)
while True:
prVertex = math.floor(random.random() * T.getNumbVertices())
mVertex = T.getVertex(T.getVerticeByInt(prVertex))
m = self.str2robotConfig(mVertex.getId())
# m need to become robot
robot_conf = []
# robot_conf = self.sampling_withinD(m,D,k)
for i in range(k):
# print(robot_conf[i])
q = self.assign_config(robot_conf, i)
# check q is collision or not
if (tester.self_collision_test(q)
) and tester.test_config_distance_tau(m, q, self, tau):
print(str(q)[:-3])
f = open("tmp_output.txt", "a")
f.write(str(q)[:-3] + '\n')
f.close()
T.addEdge(str(m), str(q))
return q, pr
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 check_obstacle_primitive(self, A, B):
stepslength = 0.05
tester = test_robot(self)
ee1flag = A[-1]
rob1 = np.asarray(A[:-1])
rob2 = np.asarray(B[:-1])
numSeg = int((len(rob1) - 2) / 2)
diff = abs(rob1 - rob2)
step = (np.ceil(diff / stepslength))
step = step.astype(int)
maxStep = max(step)
stepslengthlist = diff / maxStep
# maxSteplist = [10,maxStep-10]
for i in range(len(stepslengthlist)):
stepslength = stepslengthlist[i]
stepslength = (
-1) * stepslength if rob1[i] > rob2[i] else stepslength
arr = []
if (step[i] == 0):
# for j in maxSteplist:
for j in range(1, maxStep, 10):
if i > 1 and i < 2 + numSeg:
tmpAng = rob1[i]
arr.append(tmpAng)
else:
arr.append(rob1[i])
else:
# for j in maxSteplist:
for j in range(1, maxStep, 10):
if i > 1 and i < 2 + numSeg:
tmpAng = rob1[i] + j * stepslength
arr.append(tmpAng)
else:
tmp = rob1[i] + j * stepslength
arr.append(tmp)
if i == 0:
whole = np.asarray(arr)
else:
whole = np.vstack([whole, arr])
whole = np.transpose(whole)
for pr in whole:
pr_rob = make_robot_config_with_arr(
A[0], A[1], pr[2:(2 + self.num_segments)],
pr[(2 + self.num_segments):(2 + 2 * self.num_segments)],
ee1flag)
# print(pr_rob)
if not tester.self_obstacle_env_test(pr_rob):
return True # have collision
return False # no collision
def test3g(self):
# files = ['./testcases/3g3_m1.txt','./testcases/4g3_m2.txt']
# colide
# check collision more times in long distance one
# files = ['./testcases/g2_m1.txt','./testcases/3g2_m2.txt']
files = ['./testcases/4g3_m1.txt']
# files = ['./testcases/4g3_m2.txt']
# no result
# files = ['./testcases/3g2_m2.txt','./testcases/3g3_m1.txt']
# use global is enough, or the last with local
# files = ['./testcases/3g2_m1.txt','./testcases/3g2_m2.txt', './testcases/3g3_m1.txt']
files = [
'./testcases/3g1_m0.txt', './testcases/3g1_m1.txt',
'./testcases/3g1_m2.txt', './testcases/4g1_m1.txt',
'./testcases/4g1_m2.txt', './testcases/3g2_m1.txt',
'./testcases/3g2_m2.txt', './testcases/3g3_m1.txt'
]
for i in range(len(files)):
fileName = files[i].split('/')[-1]
sol = './out/' + fileName[:-4] + '_output.txt'
prm = PRM(files[i])
flagPRM, ee1flag = prm.run_PRM(sol)
# flagPRM=True
if flagPRM:
aa = test_robot(prm)
qq = aa.load_output(sol)
strlist = []
for j in qq:
strlist.append(str(j))
# ee1flag=[]
gginterpolat = Interpolation(strlist, ee1flag)
interpolation_path = sol[:-4] + '_ip.txt'
print("Run interpolation")
gg = gginterpolat.run_Interpolate()
write_sampling_config(interpolation_path, prm.num_segments, gg)
result = tester_main(files[i], interpolation_path)
if (result == 0):
print(files[i] + ' success')
else:
print(files[i] + ' fails')
else:
print(files[i] + ' fails')
result = 1
self.assertEqual(result, 0)
def main(arglist):
# def main():
inputfilename = arglist[0]
sol = inputfilename + '_output.txt'
# interpolation_path = sol[:-4] +'_ip.txt'
interpolation_path = arglist[1]
prm = PRM(inputfilename)
flagPRM, ee1flag = prm.run_PRM(sol)
# flagPRM=True
aa = test_robot(prm)
qq = aa.load_output(sol)
strlist = []
for j in qq:
strlist.append(str(j))
# ee1flag=[]
gginterpolat = Interpolation(strlist, ee1flag)
print("Run interpolation")
gg = gginterpolat.run_Interpolate()
write_sampling_config(interpolation_path, prm.num_segments, gg)
def collision_check(self, A, B, n, checkList):
if n <= 0:
return False
# check in the same ee grapple
if (A[0] != B[0] or A[1] != B[1] or A[-1] != B[-1]):
return False
tester = test_robot(self)
mid = (A + B) / 2
mid[-1] = A[-1]
midRobot = make_robot_config_with_arr(
A[0], A[1], mid[2:(2 + self.num_segments)],
mid[(2 + self.num_segments):(2 + 2 * self.num_segments)], A[-1])
# print(midRobot)
# checkList.append(str(midRobot))
if not tester.self_obstacle_env_test(midRobot):
return True # have collision
flagA = self.collision_check(A, mid, n - 1, checkList)
flagB = self.collision_check(mid, B, n - 1, checkList)
return flagA or flagB
def KDtreeVertex(self, graph, arr, knearest, tau):
tester = test_robot(self)
tree = KDTree(arr, leaf_size=2)
r, c = arr.shape
visited = set()
# print(output)
for sp in range(r):
dist, ind = tree.query(arr[sp:sp + 1], k=knearest)
curNode = graph.getVerticeByInt(sp)
mVer = graph.getVertex(curNode)
m = self.str2robotConfig(curNode)
for kn in range(1, knearest):
knNode = graph.getVerticeByInt(ind[0][kn])
q = self.str2robotConfig(knNode)
conns = mVer.getConnectionsIds()
if ((knNode, curNode) not in visited
and (curNode, knNode) not in visited):
if (knNode not in conns
and tester.test_config_distance(m, q, self, tau)):
# if knNode == "0.5 0.1; 8.27754461 14.87806786 139.17317948; 0.1753264 0.1 0.10378391; 1" or knNode == "0.5 0.9; -4.82905941 12.32042486 127.10152851; 0.2 0.1 0.12652526; 1":
# qq=1
if curNode[-1] == knNode[-1]:
if int(curNode[-1]) == 1:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
graph.addEdge(curNode, knNode)
graph.addEdge(knNode, curNode)
visited.add((curNode, knNode))
visited.add((knNode, curNode))
else:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
graph.addEdge(curNode, knNode)
graph.addEdge(knNode, curNode)
visited.add((curNode, knNode))
visited.add((knNode, curNode))
return tree
def test3g(self):
files = ['./testcases/4g1_m1.txt']
for i in range(len(files)):
fileName = files[i].split('/')[-1]
sol = './out/' + fileName[:-4] + '_output.txt'
prm = EST(files[i])
prm.run_EST(sol)
aa = test_robot(prm)
qq = aa.load_output(sol)
strlist = []
for j in qq:
strlist.append(str(j))
gginterpolat = Interpolation(strlist)
interpolation_path = sol[:-4] + '_ip.txt'
gg = gginterpolat.run_Interpolate()
write_sampling_config(interpolation_path, prm.num_segments, gg)
result = tester_main(files[i], interpolation_path)
if (result == 0):
print(files[i] + ' success')
else:
print(files[i] + ' fails')
self.assertEqual(result, 0)
def connectTree(self, added_m, flagInit):
tester = test_robot(self)
tau = 0.4
# check added_m in the other tree
T = self.gInit if flagInit == 1 else self.gGoal
visited = set()
for i in range(T.getNumbVertices()):
VertexTmp = T.getVertex(i)
prVertex = math.floor(random.random() * T.getNumbVertices())
if (prVertex not in visited):
visited.add(prVertex)
mVertex = T.getVertex(T.getVerticeByInt(prVertex))
q = self.str2robotConfig(mVertex.getId())
# print("comp:",added_m)
# print("other tree:",q)
# should find the minimum q to calculate the distance, but not implement now
if tester.test_config_distance_tau(added_m, q, self, tau):
# print(added_m)
# print(q)
# self.combineTwoGraph(flagInit,added_m,q)
return True, q
return False, None
def obstacle_sampling_near_only(self, T, q, D, k, tau):
tester = test_robot(self)
q_test = self.sampling_withinD(q, D, k, q.get_HeadeePos(),
q.ee1_grappled)
for i in range(k):
q_test_conf = self.assign_config(q_test, i)
# find q_test in the distance of D from q
if (tester.self_collision_test(q_test_conf)):
curNode = str(q)
knNode = str(q_test_conf)
if curNode[-1] == knNode[-1]:
if int(curNode[-1]) == 1:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
T.addVertex(str(q_test_conf))
return q_test_conf
else:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
T.addVertex(str(q_test_conf))
return q_test_conf
import test_robot myTest = test_robot.test_robot()
def PRM_collision_check(self, gaph, numLayer):
# numLayer = 2
# dist_tau = 1.0
dist_tau = self.Setting['collision_tau']
tester = test_robot(self)
verlists = gaph.getVertices()
count = 0
for ver in gaph.vertArr:
# for ver in verlists:
verA = gaph.getVertex(ver)
connects = verA.getConnections()
robA = self.str2robotConfig(verA.getId())
# find whose the farest conn, do more collision check
adapt = []
if verA.checkConnections():
arrA = verA.getAllconnkeylist()
adapt = np.zeros((1, len(arrA)))
tree = KDTree(arrA, leaf_size=2)
myarray = np.asarray([robA.str2list_radians()])
myarray[0][0] = myarray[0][0] * 10000
myarray[0][1] = myarray[0][1] * 10000
dist, ind = tree.query(myarray, k=len(arrA))
# check which one is greater than threshold
for s in range(len(arrA)):
if dist[0][s] > dist_tau:
adapt[0][ind[0][s]] = 1
queue4delete = []
s = 0
for conn in connects:
conn_key = conn
# if adapt[0][s] == 1:
# numLayer_tmp = numLayer*4
# else:
# numLayer_tmp = numLayer
# s+=1
if ((ver, conn_key) not in self.visitedCollide) or (
(conn_key, ver) not in self.visitedCollide):
self.visitedCollide[(ver, conn_key)] = 0
self.visitedCollide[(conn_key, ver)] = 0
# self.visitedCollide.add((ver,conn_key))
# self.visitedCollide.add((conn_key,ver))
verB = gaph.getVertex(conn_key)
robB = self.str2robotConfig(verB.getId())
# idA ='0.5 0.1; 17.11552405 46.16114993 79.48422487 31.67332461; 0.11502017 0.11508678 0.11821618 0.12656738; 1'
# idB ='0.5 0.1; 17.01306426 47.1029291 67.73374325 14.29924812; 0.19926977 0.14879958 0.12901342 0.14967376; 1'
# checkList =[]
# robA = self.str2robotConfig(idA)
# robB = self.str2robotConfig(idB)
# aa = np.asarray(robA.str2list_radians())-np.asarray(robB.str2list_radians())
# saa = np.sum(np.absolute(aa))
# flag_pr = self.check_obstacle_primitive(robA.str2list(),robB.str2list())
# flag = self.collision_check(np.asarray(robA.str2list()),np.asarray(robB.str2list()),numLayer,checkList)
checkList = []
if adapt[0][s] == 1:
flag = False
flag_pr = self.check_obstacle_primitive(
robA.str2list(), robB.str2list())
else:
# # bounding box check, can decrease checking times a lot
flag_pr = False
flag = self.collision_check(
np.asarray(robA.str2list()),
np.asarray(robB.str2list()), numLayer, checkList)
if (not tester.self_bounding_collision_test(robA)) and (
not tester.self_bounding_collision_test(robB)
) and (not flag) and (not flag_pr):
continue
# False-> no collision, True -> have collision
count += 1
# checkList = []
# flag = self.collision_check(np.asarray(robA.str2list()),np.asarray(robB.str2list()),numLayer,checkList)
self.collision_checkList[(ver, conn_key)] = checkList
self.collision_checkList[(conn_key, ver)] = checkList
if flag or flag_pr:
self.visitedCollide[(ver, conn_key)] = 1
self.visitedCollide[(conn_key, ver)] = 1
queue4delete.append(conn_key)
else:
if self.visitedCollide[(
ver,
conn_key)] == 1 or self.visitedCollide[(conn_key,
ver)] == 1:
# if len(self.collision_checkList[(ver,conn_key)]) != 0 or len(self.collision_checkList[(conn_key,ver)]) != 0:
queue4delete.append(conn_key)
for dd in queue4delete:
verA.delete_connect_id(dd)
verB = gaph.getVertex(dd)
verB.delete_connect_id(verA.getId())
print("==== collision violate times ==== ", count)
def PRM_expansionBuildGraph(self, graph, numberOfsampling, tau, eexy,
ee1Flag):
# numberOfsampling = 100
success_limit = 10
D = [tau, 0.1]
# D = [0.4,1e-3]
# tau = 0.5
k = 1
gPoints = self.get_grapple_points()
T = graph
tester = test_robot(self)
successCount = 0
prevRobot = None
loop_count = 0
count_node = 0
while count_node < numberOfsampling:
loop_count += 1
# over success_limit success, change node
if prevRobot is not None and successCount != 0:
m = prevRobot
else:
prVertex = math.floor(random.random() * T.getNumbVertices())
mVertex = T.getVertex(T.getVerticeByInt(prVertex))
# if(not isinstance(mVertex.getId(), str)):
# qq = 1
m = self.str2robotConfig(mVertex.getId())
# m need to become robot
robot_conf = self.sampling_withinD(m, D, k, eexy, ee1Flag)
q = self.assign_config(robot_conf, 0)
# check q is collision or not
if (tester.self_collision_test(q)
) and tester.test_config_distance_tau(m, q, self, tau):
# print(str(q)[:-3])
# f = open("tmp_output.txt", "a")
# f.write(str(q)[:-3] + '\n')
# f.close()
curNode = str(m)
knNode = str(q)
if curNode[-1] == knNode[-1]:
if int(curNode[-1]) == 1:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
count_node += 1
T.addVertex(str(q))
# T.addEdge(str(m),str(q))
successCount += 1
prevRobot = q
else:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
count_node += 1
T.addVertex(str(q))
# T.addEdge(str(m),str(q))
successCount += 1
prevRobot = q
else: # obstacle sampling
successCount = 0
output = self.obstacle_sampling(T, m, q, D, k, tester, tau)
if output is not None:
count_node += 1
successCount += 1
prevRobot = output
if successCount > success_limit:
successCount = 0
def buildGraph(self, graph, numberSamples, knearest, tau, addState, eexy,
ee1Flag):
tester = test_robot(self)
points = self.grapple_points
numberSamples_tmp = numberSamples
D = [tau, 0.1]
count = 0
loop_count = 0
diffAng = self.checkhv_ee(eexy)
while count < numberSamples:
samples = self.sampling_eexy(eexy, numberSamples, ee1Flag, diffAng)
loop_count += 1
addCount = 0
for sp in range(numberSamples_tmp):
addCount += 1
rob = self.assign_config(samples, sp)
if tester.self_collision_test(rob):
graph.addVertex(str(rob))
# points = rob.str2list()[:-1]
# points = rob.points
points = rob.get_position()
myarray = np.asarray(points)
r, c = myarray.shape
myarray = myarray.reshape(1, c * r)
if count == 0:
output = myarray
count += 1
else:
output = np.vstack([output, myarray])
count += 1
if count > numberSamples:
break
else:
# print("add 20 obstacle samples")
self.obstacle_sampling_near_only(graph, rob, D, 1, tau)
if addState == 1:
init = self.get_init_state()
graph.addVertex(str(init))
# myarray = np.asarray(init.str2list()[:-1])
myarray = np.asarray(init.get_position())
r, c = myarray.shape
myarray = myarray.reshape(1, c * r)
output = np.vstack([output, myarray])
goal = self.get_goal_state()
graph.addVertex(str(goal))
# myarray = np.asarray(goal.str2list()[:-1])
myarray = np.asarray(goal.get_position())
r, c = myarray.shape
myarray = myarray.reshape(1, c * r)
output = np.vstack([output, myarray])
r, c = output.shape
tree = KDTree(output, leaf_size=2)
# print(output)
for sp in range(r):
if sp >= r - 2:
knearest = 2
dist, ind = tree.query(output[sp:sp + 1], k=knearest)
curNode = graph.getVerticeByInt(sp)
m = self.str2robotConfig(curNode)
for kn in range(1, knearest):
knNode = graph.getVerticeByInt(ind[0][kn])
q = self.str2robotConfig(knNode)
if (sp < r - 2):
if (tester.test_config_distance(m, q, self, tau)):
if curNode[-1] == knNode[-1]:
if int(curNode[-1]) == 1:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(' ')[
1] == knNode.split(' ')[1]:
graph.addEdge(curNode, knNode)
else:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(' ')[
1] == knNode.split(' ')[1]:
graph.addEdge(curNode, knNode)
else:
if curNode[-1] == knNode[-1]:
if int(curNode[-1]) == 1:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
graph.addEdge(curNode, knNode)
else:
if curNode.split(' ')[0] == knNode.split(
' ')[0] and curNode.split(
' ')[1] == knNode.split(' ')[1]:
graph.addEdge(curNode, knNode)