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]
for x in xrange(1,1000): stdev = 2.5 average = 5; f1 = abs(np.random.normal(average, stdev)); f2 = abs(np.random.normal(average, stdev)); f3 = abs(np.random.normal(average, stdev)); f4 = abs(np.random.normal(average, stdev)); f = [f1,f2,f3,f4] delta_t = 0.02 t = delta_t #print "sampled force ", f newState = previousStateNode while t < 1: newState = us.updateState(newState, f, delta_t) # Check if within range #print "Time", t, "New State", newState #with env: newState_Euler = euler_from_quaternion([newState[6],newState[7],newState[8],newState[9]]) attemptConfig_Euler = euler_from_quaternion([attemptConfig[6],attemptConfig[7],attemptConfig[8],attemptConfig[9]]) l2Distance = calculateL2([newState[0], newState[1], newState[2], newState_Euler[0], newState_Euler[1], newState_Euler[2], newState_Euler[3] ], [attemptConfig[0], attemptConfig[1], attemptConfig[2], attemptConfig_Euler[0], attemptConfig_Euler[1], attemptConfig_Euler[2], attemptConfig_Euler[3] ]) #print "New x y z", newState[0],newState[1],newState[2] print l2Distance if minL2 > l2Distance: minL2 = l2Distance if l2Distance < 0.1: # Check Collision
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))
s2 = us.updateState(s2, f, ts)
print 'position : ', s2[0:3]
print 'rotation : ', us.Q2E(s2[6:10]), ' speed: ', s2[3:6]
print
# x1 = array(s2[0:3])
# v1 = array(s2[3:6])
# Q1 = array(s2[6:10])
# W1 = array(s2[10:13])
# E1 = tf.euler_from_quaternion(Q1)
# C = list(x1)+list(Q1)
robot2.SetActiveDOFValues(c2)
robot.SetActiveDOFValues(us.S2CQ(s2))
time.sleep(ts)
# ts = 0.005
cb = []
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)
# waitrobot(robot);
waitrobot(robot)
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, 100)
while 1:
s2 = [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
for tt in t:
s2 = us.updateState(s2, u, ts)
x1 = array(s2[0:3])
v1 = array(s2[3:6])
Q1 = array(s2[6:10])
W1 = array(s2[10:13])
E1 = tf.euler_from_quaternion(Q1)
C = list(x1) + list(Q1)
robot.SetActiveDOFValues(C)
time.sleep(0.02)
# traj = RaveCreateTrajectory(env,'');
# config = robot.GetActiveConfigurationSpecification('linear');
# config.AddDeltaTimeGroup();