def pathNearNarrowPoint(NarrPt):
# give a narrow point
# return a feasible path through it
for speedi in linspace(3, 18, num=20): # sample speed at the narrow point
statei = NarrPt[0:3] + [-speedi, 0, 0] + NarrPt[3:7] + [0, 0, 0]
for i in range(
0, 1): # sample trajectory at the state with specific speed
iterTotal = 3
delta_t = 0.02
statet = statei
tbackward = -3
tforward = 3
fFind = 0
bFind = 0
cf = []
cb = []
sf = []
sb = []
t1 = 0
t2 = -delta_t
#######################
### forward ###########
#######################
statet = statei
while t1 < tforward:
f = us.sampleFSstill(statet, 0.1)
statet = us.updateState(statet, f, delta_t)
robot.SetActiveDOFValues(statet)
# time.sleep(delta_t)
cf.append(us.S2CQ(statet))
sf.append(statet)
zAngle = us.Q2E(statet[6:10])[1]
speedTotal = abs(statet[3] + statet[4] + statet[5])
if env.CheckCollision(robot) == True:
break
print "zAngle " + str(zAngle) + " speedTotal " + str(
speedTotal)
if abs((zAngle)) < 0.2 and speedTotal < 1:
fFind = 1
break
print "find stable transition point"
t1 = t1 + delta_t
#######################
#### backrward ########
#######################
fFind = 1
if fFind == 1:
statet = statei
while t2 < tforward:
f = us.sampleFSstillBack(statet, 0.1)
statet = us.updateState(statet, f, -delta_t)
robot.SetActiveDOFValues(statet)
# time.sleep(delta_t)
cb.append(us.S2CQ(statet))
sb.append(statet)
zAngle = us.Q2E(statet[6:10])[1]
speedTotal = abs(statet[3] + statet[4] + statet[5])
if env.CheckCollision(robot) == True:
break
print "zAngle " + str(zAngle) + " speedTotal " + str(
speedTotal)
if abs((zAngle)) < 0.4 and speedTotal < 2:
bFind = 1
break
print "find stable transition point"
t2 = t2 - delta_t
#################################
#### Animate the trajectory #####
#################################
# if bFind == 1 and fFind == 1:
ctotal = list(reversed(cb)) + cf
stotal = list(reversed(sb)) + sf
for ct in ctotal:
robot.SetActiveDOFValues(ct)
# time.sleep(delta_t)
if bFind == 1 and fFind == 1:
break
raw_input(
"can not find a path at current speed, press enter to try a higher speed "
)
return [ctotal, stotal]
print "test update state"
# s1 = [1,1,1,1,0,0,0,0.2,0.2,0.2,0.1,0.1,-0.1]
avf = 1.85 * 9.8 / 4
u = [-0.5 * avf, 2 * avf, -0.5 * avf, 3 * avf]
ts = 0.02
t = range(0, 150)
speed = 1
while 1:
speed = speed + 1
# s2 = [0,0,1,speed,speed,0,0.4741598817790379, 0.47942553860420301, 0.0, 0.73846026260412878,0,0,0]
s2 = [0, 0, 1, speed, 0, 0, 1, 0, 0, 0, 0, 0, 0]
robot3.SetActiveDOFValues(us.S2CQ(s2))
# s2 = [0,0,2,8,0,0,1,0,0,0,0,0,0]
c2 = [5, 2, 2, 1, 0, 0, 0]
# waitrobot(robot2)
for tt in t:
# f = list(-np.array(us.sampleFSC(s2,c2,0.5)))
# f = list(np.array(us.sampleFSstill(s2,0.5)))
# f = us.sampleFSstillBack(s2,0.5)
# f = us.sampleFSstill(s2,0.5)
f = us.sampleFSC(s2, c2, 0.01)
# raw_input("step")
# f = us.sampleFCSAP(s2,c2)
# fp = us.sampleFSC(s2,c2)
# f = list(np.array(fa) + np.array(fp))
tbackward = -3
tforward = 3
#######################
### forward ###########
#######################
fiter = 0
fFind = 0
while fiter < iterTotal:
fiter += 1
cf = []
statet = statei
t1 = 0
while t1 < tforward:
f = us.sampleFSstill(statet, 0.5)
statet = us.updateState(statet, f, delta_t)
cf.append(us.S2CQ(statet))
# robot.SetActiveDOFValues(us.S2CQ(statet))
# time.sleep(delta_t)
zAngle = us.Q2E(statet[6:10])[1]
speedTotal = abs(statet[3] + statet[4] + statet[5])
print "zAngle " + str(zAngle) + " speedTotal " + str(
speedTotal)
if abs((zAngle)) < 0.2 and speedTotal < 1:
fFind = 1
break
print "find stable transition point"
t1 = t1 + delta_t
# for ct in cf:
# robot.SetActiveDOFValues(ct)
# time.sleep(delta_t)
delta_t = 0.01
print "attemp index", i
for x in xrange(1, 10):
f = us.sampleF()
t = delta_t
newState = previousStateNode
while t < 1:
nearestIndex = us.nnNodeIndexPathSQ(newState, path, i)
# f = us.sampleFS(newState)
# f = us.sampleFSC(newState,attemptConfig)
f = us.sampleFSC(newState, path[nearestIndex])
newState = us.updateState(newState, f, delta_t)
robot.SetActiveDOFValues(us.S2CQ(newState))
# Check if within range
# l2Distance = us.distCSQ(newState,attemptConfig)
# nearestIndex = us.nnNodeIndexPathSQ(newState,path,i)
l2Distance = us.distCSQ(newState, path[nearestIndex], 0.05)
time.sleep(0.01)
# print l2Distance
if minL2 > l2Distance:
minL2 = l2Distance
if l2Distance < 0.3:
# Check Collision
robot.SetActiveDOFValues(us.S2CQ(newState))
print "L2Distance with in 0.1"