def TOPPInterface(traj0, W):
Ndim = 2
Adim = 2
discrtimestep = 1e-3
ndiscrsteps = int((traj0.duration + 1e-10) / discrtimestep) + 1
q = np.zeros((Ndim, ndiscrsteps))
qs = np.zeros((Ndim, ndiscrsteps))
qss = np.zeros((Ndim, ndiscrsteps))
a = np.zeros((ndiscrsteps, 2 * Adim))
b = np.zeros((ndiscrsteps, 2 * Adim))
c = np.zeros((ndiscrsteps, 2 * Adim))
F = np.zeros((ndiscrsteps, Ndim))
tvect = arange(0, traj0.duration + discrtimestep, discrtimestep)
umax = np.array((1, 1))
umin = np.array((-1, -1))
Fborder = 0.2
for i in range(ndiscrsteps):
t = i * discrtimestep
q[:, i] = traj0.Eval(t)
qs[:, i] = traj0.Evald(t)
qss[:, i] = traj0.Evaldd(t)
Fi = np.array((0, 0))
if q[0, i] < Fborder:
Fi = np.array((1.5, 0))
I = np.eye(Ndim)
G = np.vstack((I, -I))
h = np.hstack((-Fi - umax, Fi + umin))
a[i, :] = np.dot(G, qs[:, i]).flatten()
b[i, :] = np.dot(G, qss[:, i]).flatten()
c[i, :] = h
vmax = 1e5 * np.ones(Ndim)
topp_inst = TOPP.QuadraticConstraints(traj0, discrtimestep, vmax, list(a),
list(b), list(c))
x = topp_inst.solver
#ret = x.RunComputeProfiles(0,0)
ret = x.RunVIP(0, 0)
if ret == 1:
print "TOPP VIP solution:", x.sdendmin, x.sdendmax
else:
print "TOPP VIP failure"
def getSpeedIntervalAtPoint(self, N):
if (N < 0) or (N > self.Nwaypoints):
print "Critical Point",N,"is out of bounds of trajectory"
sys.exit(1)
if N == 0:
return [0,0]
Wn = self.waypoints[:,0:N]
trajN = Utilities.InterpolateViapoints(Wn)
[a,b,c] = self.GetABC(trajN, self.discrtimestep)
vmax = 1e5*np.ones(self.Ndim)
topp_inst = TOPP.QuadraticConstraints(trajN, self.discrtimestep, vmax, list(a), list(b), list(c))
x = topp_inst.solver
ret = x.RunVIP(0.0, 0.0)
if ret != 1:
semin = 0.0
semax = 0.0
else:
semin = x.sdendmin
semax = x.sdendmax
if semin<=1e-5:
ret2 = x.RunComputeProfiles(0.0, semin)
if ret2 != 1:
#print N,ret,ret2,semin,semax
#np.savetxt('topp/a2',a)
#np.savetxt('topp/b2',b)
#np.savetxt('topp/c2',c)
#tstr = "trajectorystring=\"\"\" %s \"\"\""%(str(trajN))
#with open("topp/traj2", "w") as fh:
# fh.write("%s" % str(trajN))
self.SaveToFile('vip')
sys.exit(0)
return [semin, semax]
def initializeFromSpecifications(self, W):
self.filename = 'images/topp_'+str(params.FILENAME)+'.png'
self.Ndim = W.shape[0]
self.Nwaypoints = W.shape[1]
self.traj0 = Utilities.InterpolateViapoints(W)
self.length = self.traj0.duration
dendpoint = np.linalg.norm(self.traj0.Eval(self.length)-W[:,-1])
if dendpoint > self.TRAJECTORY_ACCURACY_REQUIRED:
print "###############"
print "TOPP INTERFACE ERROR"
print "path duration:",self.length
print "###############"
print "FINAL POINT on piecewise C^2 traj:",self.traj0.Eval(self.length)
print "FINAL WAYPOINT :",W[:,-1]
print "distance between points:",dendpoint
print "###############"
sys.exit(1)
[a,b,c] = self.GetABC(self.traj0, self.discrtimestep)
vmax = 1e5*np.ones(self.Ndim)
self.topp_inst = TOPP.QuadraticConstraints(self.traj0, self.discrtimestep, vmax, list(a), list(b), list(c))
self.waypoints = W
def TOPPInterface(Ndim, trajectorystring, discrtimestep, a, b, c):
vmax = 1e5 * np.ones(Ndim) ## no velocity constraints
topp_inst = TOPP.QuadraticConstraints(trajectorystring, discrtimestep,
vmax, list(a), list(b), list(c))
x = topp_inst.solver
##### compute all possible speeds at end of traj
ret = x.RunVIP(0, 0)
print "discrtimestep", discrtimestep, "TOPP RunVIP (0,0) code:", ret,
if ret == 1:
print "(Success)",
semin = x.sdendmin
semax = x.sdendmax
print "sd_end: [", semin, ",", semax, "]"
##### compute speed profile for some speeds inside sd_end
for speed in np.linspace(semin, semax, 5):
ret = x.RunComputeProfiles(0, speed)
print "discrtimestep", discrtimestep, "TOPP RunComputeProfiles (0,", speed, ") code:", ret,
if ret == 1:
print "(Success)"
else:
print "(Failure)"
def Shortcut(robot,
taumax,
vmax,
lietraj,
maxiter,
expectedduration=-1,
meanduration=0,
upperlimit=-1,
inertia=None,
trackingplot=None):
if trackingplot == 1:
plt.axis([0, maxiter, 0, lietraj.duration])
plt.ion()
plt.show()
ylabel('Trajectory duration (s)')
xlabel('Iteration')
t_sc_start = time.time()
originalduration = lietraj.duration
#return shortcuted traj
if upperlimit < 0:
dur = lietraj.duration
upperlimit = lietraj.duration
else:
dur = upperlimit
attempt = 0
## for shortcutting
integrationtimestep = 1e-2
reparamtimestep = 1e-2
passswitchpointnsteps = 5
discrtimestep = 1e-2
constraintsstring = str(discrtimestep)
constraintsstring += "\n" + ' '.join([str(v) for v in taumax])
if not (inertia is None):
for v in inertia:
constraintsstring += "\n" + ' '.join([str(i) for i in v])
assert (dur > 10.0 * discrtimestep)
ncollision = 0
nnotretimable = 0
nnotshorter = 0
for it in range(maxiter):
if trackingplot == 1:
plt.scatter(it, lietraj.duration)
plt.draw()
if (
expectedduration > 0
): # check, if newlietraj.duration is short enough, stop SHORTCUTING
if (lietraj.duration < expectedduration):
print "\033[1;32mTrajectory's duration is already shorter than expected time --> stop shortcuting\033[0m"
break
if (dur < discrtimestep):
print "[Utils::Shortcut] trajectory duration is less than discrtimestep.\n"
break ## otherwise, this will cause an error in TOPP
## select an interval for shortcutting
t0 = random.rand() * dur
if meanduration == 0:
meanduration = dur - t0
T = random.rand() * min(meanduration, dur - t0)
t1 = t0 + T
while (T < 2.0 * discrtimestep):
t0 = random.rand() * dur
if meanduration == 0:
meanduration = dur - t0
T = random.rand() * min(meanduration, dur - t0)
t1 = t0 + T
if t1 > upperlimit:
t1 = upperlimit
if (t1 < t0):
temp = t0
t0 = t1
t1 = temp
T = t1 - t0
# print "\n\nShortcutting iteration", it + 1
# print t0, t1, t1- t0
# interpolate from t0 to t1
R_beg = lietraj.EvalRotation(t0)
R_end = lietraj.EvalRotation(t1)
omega0 = lietraj.EvalOmega(t0)
omega1 = lietraj.EvalOmega(t1)
shortcuttraj = lie.InterpolateSO3(R_beg, R_end, omega0, omega1, T)
#check feasibility only for the new portion
isincollision = CheckCollisionTraj(robot, shortcuttraj, R_beg,
discrtimestep)
if (not isincollision):
# a,b,c = lie.ComputeSO3Constraints(shortcuttraj, taumax, discrtimestep)
abc = TOPPbindings.RunComputeSO3Constraints(
str(shortcuttraj), constraintsstring) # discrtimestep)
a, b, c = lie.Extractabc(abc)
topp_inst = TOPP.QuadraticConstraints(shortcuttraj, discrtimestep,
vmax, list(a), list(b),
list(c))
x = topp_inst.solver
ret = x.RunComputeProfiles(1, 1)
if (ret == 1):
x.resduration
## check whether the new one has shorter duration
if (x.resduration + 0.01 < T): #skip if not shorter than 0.3 s
x.ReparameterizeTrajectory()
x.WriteResultTrajectory()
TOPPed_shortcuttraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
newlietraj = ReplaceTrajectorySegment(
lietraj, TOPPed_shortcuttraj, t0, t1)
lietraj = newlietraj
dur = lietraj.duration
#print "*******************************************"
print "Success at iteration", it + 1, ":", t0, t1, "Deta_t:", t1 - t0 - x.resduration
attempt += 1
#print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
#print "*******************************************"
else:
# print "Not shorter"
nnotshorter += 1
else:
# print "Not retimable"
nnotretimable += 1
else:
# print "Collision"
ncollision += 1
# print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
print "\033[1;32mT:", nnotretimable, "; S:", nnotshorter, "; C:", ncollision, "; OK:", attempt, "\033[0m"
print "\033[1;32m", originalduration - lietraj.duration, "sec. shorter\033[0m"
t_sc_end = time.time()
print "\033[1;32mRunning time:", t_sc_end - t_sc_start, "sec.\033[0m"
return lietraj
def SE3Shortcut(robot,
taumax,
fmax,
vmax,
se3traj,
Rlist,
maxiter,
expectedduration=-1,
meanduration=0,
upperlimit=-1,
plotdura=None):
if plotdura == 1:
plt.axis([0, maxiter, 0, se3traj.duration])
plt.ion()
plt.show()
ylabel('Trajectory duration (s)')
xlabel('Iteration')
t_sc_start = time.time()
originalduration = se3traj.duration
#return shortcuted traj
if upperlimit < 0:
dur = se3traj.duration
upperlimit = se3traj.duration
else:
dur = upperlimit
attempt = 0
## for shortcutting
integrationtimestep = 1e-2
reparamtimestep = 1e-2
passswitchpointnsteps = 5
discrtimestep = 1e-2
assert (dur > 10.0 * discrtimestep)
ncollision = 0
nnotretimable = 0
nnotshorter = 0
transtraj, rtraj = TransRotTrajFromSE3Traj(se3traj)
lietraj = lie.SplitTraj(Rlist, rtraj)
for it in range(maxiter):
if plotdura == 1:
plt.scatter(it, se3traj.duration)
plt.draw()
#transtraj, rtraj = TransRotTrajFromSE3Traj(se3traj)
#lietraj = lie.SplitTraj2(Rlist, rtraj)
if (
expectedduration > 0
): # check, if newlietraj.duration is short enough, stop SHORTCUTING
if (se3traj.duration < expectedduration):
print "\033[1;32mTrajectory's duration is already shorter than expected time --> stop shortcuting\033[0m"
break
if (dur < discrtimestep):
print "[Utils::Shortcut] trajectory duration is less than discrtimestep.\n"
break ## otherwise, this will cause an error in TOPP
## select an interval for shortcutting
t0 = random.rand() * dur
if meanduration == 0:
meanduration = dur - t0
T = random.rand() * min(meanduration, dur - t0)
t1 = t0 + T
while (T < 2.0 * discrtimestep):
t0 = random.rand() * dur
if meanduration == 0:
meanduration = dur - t0
T = random.rand() * min(meanduration, dur - t0)
t1 = t0 + T
if t1 > upperlimit:
t1 = upperlimit
if (t1 < t0):
temp = t0
t0 = t1
t1 = temp
T = t1 - t0
# print "\n\nShortcutting iteration", it + 1
# print t0, t1, t1- t0
# interpolate from t0 to t1
R_beg = lietraj.EvalRotation(t0)
R_end = lietraj.EvalRotation(t1)
omega0 = lietraj.EvalOmega(t0)
omega1 = lietraj.EvalOmega(t1)
shortcutrtraj = lie.InterpolateSO3(R_beg, R_end, omega0, omega1, T)
t_beg = transtraj.Eval(t0)
t_end = transtraj.Eval(t1)
v_beg = transtraj.Evald(t0)
v_end = transtraj.Evald(t1)
shortcuttranstraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
TrajString3rdDegree(t_beg, t_end, v_beg, v_end, T))
shortcutse3traj = SE3TrajFromTransandSO3(shortcuttranstraj,
shortcutrtraj)
#check feasibility only for the new portion
isincollision = CheckCollisionSE3Traj(robot, shortcuttranstraj,
shortcutrtraj, R_beg,
discrtimestep)
if (not isincollision):
a, b, c = ComputeSE3Constraints(shortcutse3traj, taumax, fmax,
discrtimestep)
topp_inst = TOPP.QuadraticConstraints(shortcutse3traj,
discrtimestep, vmax, list(a),
list(b), list(c))
x = topp_inst.solver
ret = x.RunComputeProfiles(1, 1)
if (ret == 1):
x.resduration
## check whether the new one has shorter duration
if (x.resduration + 0.1 < T): #skip if not shorter than 0.1 s
x.ReparameterizeTrajectory()
x.WriteResultTrajectory()
TOPPed_shortcutse3traj = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
TOPPed_shortcuttranstraj, TOPPed_shortcutrtraj = TransRotTrajFromSE3Traj(
TOPPed_shortcutse3traj)
newlietraj = ReplaceTrajectorySegment(
lietraj, TOPPed_shortcutrtraj, t0, t1)
newtranstraj = ReplaceTransTrajectorySegment(
transtraj, TOPPed_shortcuttranstraj, t0, t1)
#####################################################
newrtraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
TrajStringFromTrajList(newlietraj.trajlist))
newse3traj = SE3TrajFromTransandSO3(newtranstraj, newrtraj)
Rlist = newlietraj.Rlist
rtraj = newrtraj
transtraj = newtranstraj
lietraj = newlietraj
se3traj = newse3traj
dur = se3traj.duration
#print "*******************************************"
print "Success at iteration", it + 1, ":", t0, t1, "Deta_t:", t1 - t0 - x.resduration
attempt += 1
#print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
#print "*******************************************"
else:
# print "Not shorter"
nnotshorter += 1
else:
# print "Not retimable"
nnotretimable += 1
else:
# print "Collision"
ncollision += 1
# print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
print "\033[1;32mT:", nnotretimable, "; S:", nnotshorter, "; C:", ncollision, "; OK:", attempt, "\033[0m"
print "\033[1;32m", originalduration - se3traj.duration, "sec. shorter\033[0m"
t_sc_end = time.time()
print "\033[1;32mRunning time:", t_sc_end - t_sc_start, "sec.\033[0m"
return se3traj, Rlist
inertia = eye(3)
vmax = ones(3)
accelmax = ones(3)
discrtimestep = 1e-3
constraintsstr = str(discrtimestep)
constraintsstr += "\n" + ' '.join([str(a) for a in accelmax])
for v in inertia:
constraintsstr += "\n" + ' '.join([str(i) for i in v])
#When Inertia is an Identity matrix, angular accelerations are the same as torques
t0 = time.time()
abc = TOPPbindings.RunComputeSO3Constraints(trajstr, constraintsstr)
a, b, c = Extractabc(abc)
t1 = time.time()
topp_inst = TOPP.QuadraticConstraints(traj, discrtimestep, vmax, list(a),
list(b), list(c))
x = topp_inst.solver
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
t2 = time.time()
print("Compute a,b,c:", t1 - t0)
print("Run TOPP:", t2 - t1)
print("Total:", t2 - t0)
# Display results
ion()
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
def computeReparametrizationTrajectory(F, R, amin, amax, W, dW, ddW):
Ndim = W.shape[0]
Nwaypoints = W.shape[1]
[trajstr,durationVector] = GetTrajectoryString(W, dW, ddW)
L = np.sum(durationVector)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajstr)
dendpoint = np.linalg.norm(traj0.Eval(L)-W[:,-1])
if dendpoint > 0.01:
print L
print "###############"
print "FINAL POINT on piecewise C^2 traj:",traj0.Eval(L)
print "FINAL WAYPOINT :",W[:,-1]
print "###############"
sys.exit(1)
### compute a,b,c
q = np.zeros((Ndim,Nwaypoints))
qs = np.zeros((Ndim,Nwaypoints))
qss = np.zeros((Ndim,Nwaypoints))
for i in range(0,Nwaypoints):
duration = np.sum(durationVector[0:i])
q[:,i] = traj0.Eval(duration)
qs[:,i] = traj0.Evald(duration)
qss[:,i] = traj0.Evaldd(duration)
I = np.identity(Ndim)
G = np.vstack((I,-I))
a = np.zeros((Nwaypoints, 2*Ndim))
b = np.zeros((Nwaypoints, 2*Ndim))
c = np.zeros((Nwaypoints, 2*Ndim))
for i in range(0,Nwaypoints):
Rmax = np.maximum(np.dot(R[:,:,i],amax),np.dot(R[:,:,i],amin))
Rmin = np.minimum(np.dot(R[:,:,i],amax),np.dot(R[:,:,i],amin))
H1 = F[:,i] - Rmax
H2 = -F[:,i] + Rmin
for j in range(Ndim):
#print "qvol[",j,"]=",np.linalg.norm(-H1[j]-H2[j])
if H2[j] > -H1[j]:
print H2[j],"<= q[",j,"]<=",-H1[j]
sys.exit(1)
c[i,:] = np.hstack((H1,H2)).flatten()
for i in range(0,Nwaypoints):
a[i,:] = np.dot(G,qs[:,i]).flatten()
b[i,:] = np.dot(G,qss[:,i]).flatten()
vmax = 1000*np.ones(Ndim)
durationQ = traj0.duration/(Nwaypoints-1)
#durationQ = 1e-1
#topp_inst = TOPP.QuadraticConstraints(traj0, durationVector[0], vmax, list(a), list(b), list(c))
topp_inst = TOPP.QuadraticConstraints(traj0, durationQ, vmax, list(a), list(b), list(c))
x = topp_inst.solver
#customPlotTrajectory(traj0)
#x.integrationtimestep = 0.001
#x.reparamtimestep = 0.001
#x.extrareps = 10
try:
#print "W=",repr(W[0:2,:])
#print "q=",repr(q[0:2,:])
#print "dW=",repr(dW[0:2,:])
#print "qs=",repr(qs[0:2,:])
#print "ddW=",repr(ddW[0:2,:])
#print "qss=",repr(qss[0:2,:])
#print "a=",repr(np.around(a,decimals=2))
#print "b=",repr(np.around(b,decimals=2))
#print "c=",repr(np.around(c,decimals=2))
#print "d=",repr(durationVector)
#print durationQ*Nwaypoints,"(",durationQ,") VS. ",traj0.duration
#print trajstr
ret = topp_inst.solver.RunComputeProfiles(0,0)
if ret == 4:
#[semin,semax] = topp_inst.AVP(0, 0)
#print "TOPP critical pt:",semin,semax
Nc = topp_inst.solver.GetMVCCriticalPoint()
return Nc
if ret == 1:
print "TOPP: success"
return Nwaypoints
if ret== 0:
print "TOPP: unspecified error"
sys.exit(0)
else:
print "TOPP: ",ret
sys.exit(0)
return None
except Exception as e:
print "TOPP EXCEPTION: ",e
print W
print duration
print durationVector
print traj0.duration
sys.exit(0)
def VisualizeTrajectory(traj0, W, Fx, Fy, offset, PostMakeup=True):
Ndim = W.shape[0]
#Ndim = 2
Adim = 3
#######################################################################
#######################################################################
#discrtimestep = 1.0/(W.shape[1]-1)#0.05
discrtimestep = 0.5 * 1e-3
#discrtimestep = 1e-2
ndiscrsteps = int((traj0.duration + 1e-10) / discrtimestep) + 1
q = np.zeros((Ndim, ndiscrsteps))
qs = np.zeros((Ndim, ndiscrsteps))
qss = np.zeros((Ndim, ndiscrsteps))
a = np.zeros((ndiscrsteps, 2 * Adim))
b = np.zeros((ndiscrsteps, 2 * Adim))
c = np.zeros((ndiscrsteps, 2 * Adim))
F = np.zeros((ndiscrsteps, Ndim))
tvect = arange(0, traj0.duration + discrtimestep, discrtimestep)
for i in range(ndiscrsteps):
t = i * discrtimestep
q[:, i] = traj0.Eval(t)
qs[:, i] = traj0.Evald(t)
qss[:, i] = traj0.Evaldd(t)
if q[0, i] >= 2:
F[i, :] = np.array((Fx, Fy, 0, 0))
Ri = params.GetControlMatrixAtWaypoint(q[:, i])
[G, h] = params.GetControlConstraintMatricesFromControl(Ri, F[i, :])
a[i, :] = np.dot(G, qs[:, i]).flatten()
b[i, :] = np.dot(G, qss[:, i]).flatten()
c[i, :] = h
#print "discrtimestep=",discrtimestep
#PrintNumpy("a",a[:,[0,1,3,4]])
#PrintNumpy("b",b[:,[0,1,3,4]])
#PrintNumpy("c",c[:,[0,1,3,4]])
#print a.shape
#print b.shape
#print c.shape
######################################################
t0 = time.time()
trajectorystring = str(traj0)
vmax = 1e5 * np.ones(Ndim)
topp_inst = TOPP.QuadraticConstraints(traj0, discrtimestep, vmax, list(a),
list(b), list(c))
x = topp_inst.solver
t1 = time.time()
#ret = x.RunComputeProfiles(0,0)
ret = x.RunVIP(0, 0)
print "TOPP:", ret
#print t1-t0
if ret == 1:
x.ReparameterizeTrajectory()
t2 = time.time()
x.WriteResultTrajectory()
msstyle = 'g'
try:
#print x.restrajectorystring
#traj1 = TOPPTrajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)
semin = x.sdendmin
semax = x.sdendmax
print "speed profile [", semin, ",", semax, "]"
#plot(qddvect[:,0], qddvect[:,1]-offset, 'r')
#if PostMakeup:
#tvect = arange(0, traj1.duration + discrtimestep, discrtimestep)
#qddvect = np.array([traj1.Evaldd(t) for t in tvect])
#for i in range(ndiscrsteps):
#plot([q[0,i],q[0,i]+qddvect[i,0]],[q[1,i]-offset,q[1,i]+qddvect[i,1]-offset], '-om', linewidth=2)
if semin > 0:
msstyle = 'm'
except Exception as e:
print "Exception:", e
msstyle = 'k'
sys.exit(0)
pass
else:
msstyle = 'r'
if PostMakeup:
for i in range(ndiscrsteps):
plot([q[0, i], q[0, i] + F[i, 0]],
[q[1, i] - offset, q[1, i] + F[i, 1] - offset],
'-ob',
linewidth=2)
plot(q[0, :],
q[1, :] - offset,
'|' + msstyle,
markersize=40,
markeredgewidth=3)
Npts = 1000
tvect = np.linspace(0, traj0.duration, Npts)
qvect = np.array([traj0.Eval(t) for t in tvect])
plot(qvect[:, 0], qvect[:, 1] - offset, '-' + msstyle)
plot(W[0, :], W[1, :] - offset, 'o' + msstyle, markersize=15)
def getSpeedProfile(RdimIdx, SO3dimIdx, F, R, amin, amax, W, dW, ddW):
Ndim = W.shape[0]
Nwaypoints = W.shape[1]
### ManifoldIDX: 0 -> R-subspace. 1-> SO(2) subspace. 2-> SO(3) subspace
### (ordered three consecutive numbers as yaw/pitch/roll
Rdim = RdimIdx.shape[0]
[Rtrajstr, durationVector] = GetTrajectoryStringR(RdimIdx, W, dW, ddW)
L = np.sum(durationVector)
Rtraj = Trajectory.PiecewisePolynomialTrajectory.FromString(Rtrajstr)
print "FINAL POINT on piecewise C^2 traj:", Rtraj.Eval(L)
print "###############"
### compute a,b,c
qs = np.zeros((Rdim, Nwaypoints))
qss = np.zeros((Rdim, Nwaypoints))
for i in range(0, Nwaypoints):
duration = np.sum(durationVector[0:i])
qs[:, i] = Rtraj.Evald(duration)
qss[:, i] = Rtraj.Evaldd(duration)
print duration, Rtraj.Eval(duration), qs[:, i], qss[:, i]
print "###############"
subplot(3, 1, 1)
Rtraj.Plot(0.001)
subplot(3, 1, 2)
Rtraj.Plotd(0.001)
subplot(3, 1, 3)
Rtraj.Plotdd(0.001)
plt.show()
#sys.exit(0)
I = np.identity(Rdim)
G = np.vstack((I, -I))
ar = np.zeros((Nwaypoints, 2 * Rdim))
br = np.zeros((Nwaypoints, 2 * Rdim))
cr = np.zeros((Nwaypoints, 2 * Rdim))
for i in range(0, Nwaypoints):
Rmax = np.maximum(np.dot(R[RdimIdx, :, i], amax),
np.dot(R[RdimIdx, :, i], amin))
Rmin = np.minimum(np.dot(R[RdimIdx, :, i], amax),
np.dot(R[RdimIdx, :, i], amin))
H1 = F[RdimIdx, i] - Rmax
H2 = -F[RdimIdx, i] + Rmin
#for j in RdimIdx:
# print "qvol[",j,"]=",np.linalg.norm(-H1[j]-H2[j])
# if H2[j] > -H1[j]:
# print H2[j],"<= q[",j,"]<=",-H1[j]
# sys.exit(1)
cr[i, :] = np.hstack((H1, H2)).flatten()
for i in range(0, Nwaypoints):
ar[i, :] = np.dot(G, qs[:, i]).flatten()
br[i, :] = np.dot(G, qss[:, i]).flatten()
# Constraints
t0 = time.time()
[aso3, bso3, cso3] = GetABCFromSO3Manifold(SO3dimIdx, amin, amax, W, dW,
ddW)
a = np.hstack((ar, aso3))
b = np.hstack((br, bso3))
c = np.hstack((cr, cso3))
traj0 = GetTrajectoryStringComplete(W, dW, ddW)
vmax = 1000 * np.ones(Ndim)
############################################################################
### DEBUG: only retime on R manifold subspace
############################################################################
#a = list(ar)
#b = list(br)
#c = list(cr)
#traj0 = Rtraj
#vmax = 1000*np.ones(Rdim)
############################################################################
#topp_inst = TOPP.QuadraticConstraints(traj0, duration, vmax, list(a), list(b), list(c))
print "###########################"
topp_inst = TOPP.QuadraticConstraints(traj0, durationVector[0], vmax,
list(a), list(b), list(c))
x = topp_inst.solver
#x.integrationtimestep = 0.001
#x.reparamtimestep = 0.001
#x.extrareps = 10
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
print "TOPP Output:", ret
if (ret == 4):
print ret, " [ERROR TOPP: MVC hit zero]"
if (ret == 1):
x.ReparameterizeTrajectory()
# Display results
ion()
#x.WriteProfilesList()
#x.WriteSwitchPointsList()
#profileslist = list(TOPPpy.ProfilesFromString(x.resprofilesliststring))
#switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
#TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
x.WriteResultTrajectory()
print x.restrajectorystring
traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
print "Trajectory duration before TOPP: ", traj0.duration
print "Trajectory duration after TOPP: ", traj1.duration
P = []
tstep = traj1.duration / Nwaypoints
subplot(3, 1, 1)
traj1.Plot(0.001)
subplot(3, 1, 2)
traj1.Plotd(0.001)
subplot(3, 1, 3)
traj1.Plotdd(0.001)
plt.show()
t = 0
while t < traj1.duration:
P.append(np.linalg.norm(traj1.Evald(t)))
t = t + tstep
print "profile computed"
#mvcbobrow = profileslist.pop(0)
#mvc = np.array(mvcbobrow[3])
#print mvc
return np.array(P)
return None
verbose=settings.verbose,
loss=settings.loss,
optimizer=settings.optimizer,
epsilon=settings.epsilon,
lrate=settings.lrate)
anet.create_anet()
hex_nn = HexNN(anet,
hex_state,
save_int=settings.save_interval,
file_add=settings.file_add,
sim_increment=settings.sim_increment,
verbose=settings.verbose)
hex_nn.run(settings.simulations, settings.training_games)
elif choice == '2':
players = list()
for anet in settings.anet_files:
if anet.strip().lower() == "none":
players.append(None)
elif anet.split('_')[0] == "Demo":
players.append(ANET.load_model("./Demo_files/" + anet))
else:
players.append(ANET.load_model(anet))
topp = TOPP(players,
g=settings.games,
board_dim=settings.root_board_dim,
epsilon=settings.epsilon)
topp.play_tournament()
elif choice == '4':
script = "../fireworks/./main"
os.system("bash -c '%s'" % script)