def getCriticalPointFromWaypoints(self, env, W, oldNc=0):
self.topp = TOPPInterface(W, env)
Nc = self.topp.getCriticalPoint() - 1
if Nc < 0:
print "return oldNc=", oldNc
Nc = oldNc
return Nc
def getVelocityIntervalWithoutForceField(self, env, W):
self.topp = TOPPInterface(W, env, zeroForce=True)
if self.topp.ReparameterizeTrajectory():
return self.topp.traj0
else:
#### without a force field any path should be executable
#### at near zero speed => catch it if not
print "WARNING1: without force field, TOPP couldn't find a valid \
velocity profile. Path not continuous or system not STLC"
print "discrtimestep=", self.topp.discrtimestep
self.topp.SaveToFile('clc2')
CP = self.topp.getCriticalPoint()
traj = self.topp.traj0
q = np.array(
[traj.Eval(t) for t in np.linspace(0, traj.duration, 1e5)]).T
plot(q[0, :], q[1, :], '-r', linewidth=2)
plot(W[0, :], W[1, :], 'ok', markersize=2)
plot(W[0, CP], W[1, CP], 'og', markersize=10)
plt.show()
sys.exit(1)
def PlotParametrization(self, env):
self.topp = TOPPInterface(W, env)
if self.topp.ReparameterizeTrajectory():
self.topp.PlotTrajectory(env)
print self.topp
else:
print "Trajectory has no ReParameterization"
def getTOPPTrajectoryWithoutForceField(self, env, W):
self.topp = TOPPInterface(W, env, zeroForce=True)
if self.topp.ReparameterizeTrajectory():
return self.topp
else:
self.info()
print "WARNING2: without force field, TOPP couldn't find a valid \
velocity profile. Path not continuous or system not STLC"
print W.shape
print W
sys.exit(1)
