def PlotTraj(W):
traj0 = Utilities.InterpolateViapoints(W)
print "Interpolate Viapoints Default",
M = 5000
q = np.array([traj0.Eval(t) for t in np.linspace(0, traj0.duration, M)]).T
plt.plot(q[0, :], q[1, :], '-r', linewidth=2)
plt.plot(W[0, :], W[1, :], 'ok')
TOPPInterface(traj0, W)
print "Interpolate Viapoints Adjust",
traj0 = InterpolateViapointsCustom(W)
q = np.array([traj0.Eval(t) for t in np.linspace(0, traj0.duration, M)]).T
plt.plot(q[0, :], q[1, :], '-g', linewidth=2)
plt.plot(W[0, :], W[1, :], 'ok')
TOPPInterface(traj0, W)
print "quiver it"
X, Y = np.mgrid[-0.1:0.5:15j, -0.005:0.005:15j]
V = X + Y
U = X + Y
U[X > 0.19] = 0
U[X < 0.19] = 1.5
V = 0.0 * V
speed = np.sqrt(U**2 + V**2)
UN = U / speed
VN = V / speed
plt.quiver(X, Y, UN, VN, color='Teal', headlength=7)
plt.show()
def traj_str_5th_degree(q_beg, q_end, qd_beg, qd_end, qdd_beg, qdd_end,
duration):
"""
Return an interpolated 5th degree polynomial trajectory string.
It is up to 10 decimal places accuracy to guarantee continuity
at trajectory end.
@type q_beg: list
@param q_beg: Initial configuration.
@type q_end: list
@param q_end: Final configuration.
@type qd_beg: list
@param qd_beg: Time derivative of initial configuration.
@type qd_end: list
@param qd_end: Time derivative of final configuration.
@type qdd_beg: list
@param qdd_beg: 2nd order time derivative of initial configuration.
@type qdd_end: list
@param qdd_end: 2nd order time derivative of final configuration.
@type duration: float
@param duration: Time length of the interpolated trajectory.
@rtype: str
@return: The interpolated polynomial trajectory string.
"""
traj_str = ''
ndof = len(q_beg)
traj_str += "%f\n%d" % (duration, ndof)
for k in range(ndof):
a, b, c, d, e, f = Utilities.Interpolate5thDegree(
q_beg[k], q_end[k], qd_beg[k], qd_end[k], qdd_beg[k], qdd_end[k],
duration)
traj_str += "\n%.10f %.10f %.10f %.10f %.10f %.10f" % (f, e, d, c, b,
a)
return traj_str
def InterpolateFree(qbeg, qend, qsbeg, qsend, duration):
pathstring = ''
ndof = len(qbeg)
pathstring += "%f\n%d" % (duration, ndof)
for k in range(ndof):
a, b, c, d = Utilities.Interpolate3rdDegree(qbeg[k], qend[k], qsbeg[k],
qsend[k], duration)
pathstring += "\n%f %f %f %f" % (d, c, b, a)
return Trajectory.PiecewisePolynomialTrajectory.FromString(pathstring)
def TrajString3rdDegree(q_beg, q_end, qs_beg, qs_end, duration):
trajectorystring = ''
ndof = len(q_beg)
trajectorystring += "%f\n%d" % (duration, ndof)
for k in range(ndof):
a, b, c, d = Utilities.Interpolate3rdDegree(q_beg[k], q_end[k],
qs_beg[k], qs_end[k],
duration)
trajectorystring += "\n%f %f %f %f" % (d, c, b, a)
return trajectorystring
def PlotTrajXY(x, y, PostMakeup=False):
Fx = 0.0
#Fy = 2.144
Fy = 1.1
#W = np.array(((0,0,0,0),(1,0,0,0),(2,0,0,0),(x,y,0,0))).T
W = np.array(
((0, 0, 0, 0), (0.2, 0, 0, 0), (0.4, 0, 0, 0), (0.6, 0, 0, 0),
(0.8, 0, 0, 0), (1, 0, 0, 0), (1.2, 0, 0, 0), (1.4, 0, 0, 0),
(1.6, 0, 0, 0), (1.8, 0, 0, 0), (2, 0, 0, 0), (x, y, 0, 0))).T
#W = np.array(((1.6,0,0,0),(1.8,0,0,0),(2,0,0,0),(x,y,0,0))).T
traj0 = Utilities.InterpolateViapoints(W)
#traj0 = InterpolateViapointsLinear(W)
#print "#######################"
#print "trajectorystring=",traj0
#print "#######################"
VisualizeTrajectory(traj0, W, Fx, Fy, PostMakeup=PostMakeup, offset=1.0)
return traj0
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
ntraj = 1000
ndof = 7
ncurve = 1
discrtimestep = 0.005
vmax = 4 * ones(ndof) # Velocity limits
amax = 20 * ones(ndof) # Accel limits
# Test begins here
nfail = 0
nsingulartreateds = 0
ntangenttreateds = 0
for j in range(ntraj):
print(j)
p0a = Utilities.vect2str_mintos(rand(ndof)*2*pi-pi)
p0b = Utilities.vect2str_mintos(rand(ndof)*2*pi-pi)
p1a = Utilities.vect2str_mintos(rand(ndof)*2*pi-pi)
p1b = Utilities.vect2str_mintos(rand(ndof)*2*pi-pi)
s = '%d'%ncurve
s+= '\n1.0 ' + p0a + ' ' + p0b
for k in range(ncurve-1):
a = rand(ndof)*2*pi-pi
b = rand(ndof)*2*pi-pi
c = 2*b-a
pa = Utilities.vect2str(a)
pb = Utilities.vect2str(b)
pc = Utilities.vect2str(c)
s+= ' ' + pa + ' ' + pb + '\n1.0 ' + pb + ' ' + pc
s+= ' ' + p1a + ' ' + p1b
Tv,p0v,p1v,p2v,p3v = TOPPpy.string2p(s)
from __future__ import print_function
import string, time
from pylab import *
from numpy import *
from TOPP import TOPPbindings
from TOPP import TOPPpy
from TOPP import Trajectory
from TOPP import Utilities
# A two-dof path going through 5 viapoints (0,1) - (1,1) - (5,1) - (3,2) - (5,4)
path = array([[-1, 0, 1, 1, 1, 0, -1, -1, -1], [-1, -1, -1, 0, 1, 1, 1, 0, -1],
[1, 1, 1, 1, 1, 1, 1, 1, 1], [0, 0, 0, 0, 0, 0, 0, 0, 0]])
traj0 = Utilities.InterpolateViapoints(path) # Interpolate using splines
print path
# Constraints
vmax = 2 * ones(traj0.dimension) # Velocity limits
amax = 1.5 * ones(traj0.dimension) # Acceleration limits
# Set up the TOPP instance
trajectorystring = str(traj0)
discrtimestep = 0.01
uselegacy = False
t0 = time.time()
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += "\n" + string.join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits", constraintstring,
trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
def generateToppTrajectoryCallback(self, req):
print("Generating TOPP trajectory.")
res = GenerateTrajectoryResponse()
dof = len(req.waypoints.points[0].positions)
n = len(req.waypoints.points)
# If there is not enough waypoints to generate a trajectory return false
if (n <= 1 or dof == 0):
print(
"You must provide at least 2 points to generate a valid trajectory."
)
res.trajectory.success = False
return res
# Generate trajectory
# Path is of dimensions DOF x n_waypoints
path = np.zeros([dof, n])
for i in range(0, n):
for j in range(0, dof):
path[j][i] = req.waypoints.points[i].positions[j]
traj0 = Utilities.InterpolateViapoints(
path) # Interpolate using splines
# Constraints
vmax = zeros(dof) # Velocity limits
amax = zeros(dof) # Acceleration limits
for i in range(0, dof):
vmax[i] = req.waypoints.points[0].velocities[i]
amax[i] = req.waypoints.points[0].accelerations[i]
# Set up the TOPP instance
trajectorystring = str(traj0)
discrtimestep = 0.01
uselegacy = False
t0 = time.time()
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += "\n" + string.join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits",
constraintstring, trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(
traj0, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
# Run TOPP
t1 = time.time()
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
t2 = time.time()
print("Using legacy:", uselegacy)
print("Discretization step:", discrtimestep)
print("Setup TOPP:", t1 - t0)
print("Run TOPP:", t2 - t1)
print("Total:", t2 - t0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(
x.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist, switchpointslist, 4)
x.WriteResultTrajectory()
traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
dtplot = 0.01
if self.plot_flag == True:
ion()
TOPPpy.PlotKinematics(traj0, traj1, dtplot, vmax, amax)
res.trajectory = self.TOPP2JointTrajectory(traj1,
req.sampling_frequency)
res.success = True
return res
def call_TOPP(req):
### Get the request path and prepare for TOPP
t0 = time.time()
print("= Get the request path...")
path_points = req.path
vel_limits = req.vel_limits
acc_limits = req.acc_limits
discrtimestep = req.timestep
# create a path going through viapoints
print("== Create a path going through viapoints...")
dim = len(path_points[0].positions)
ndata = len(path_points)
path_array = np.zeros((dim, ndata))
for i in range(ndata):
path_array[:, i] = np.array(path_points[i].positions)
print("=== Interpolate viapoints...")
#import pdb
#pdb.set_trace()
path = Utilities.InterpolateViapoints(
path_array) # Interpolate using splines
# Constraints
vmax = np.array(vel_limits)
amax = np.array(acc_limits)
### Set up the TOPP instance
print("==== Set up a TOPP instance...")
t1 = time.time()
trajectorystring = str(path)
uselegacy = True # True to use KinematicLimits(vel & acc)
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += "\n" + string.join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits",
constraintstring, trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(
path, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
### Run TOPP
print("===== Run TOPP...")
t2 = time.time()
#import pdb
#pdb.set_trace()
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
### Convert resultant array to plan
print("====== Convert resultant array to plan...")
t3 = time.time()
x.WriteResultTrajectory(
) # be sure to write the result before reading it!!!
traj = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
traj_point = JointTrajectoryPoint()
traj_points = [] #PathToTrajResponse() # just an empty list
t = 0.0
while t < traj.duration:
traj_point.positions = traj.Eval(t).tolist()
traj_point.velocities = traj.Evald(t).tolist()
traj_point.accelerations = traj.Evaldd(t).tolist()
traj_point.time_from_start = rospy.Duration(t)
traj_points.append(copy.deepcopy(traj_point))
t += discrtimestep
# the last point
traj_point.positions = traj.Eval(traj.duration).tolist()
traj_point.velocities = traj.Evald(traj.duration).tolist()
traj_point.accelerations = traj.Evaldd(traj.duration).tolist()
traj_point.time_from_start = rospy.Duration(traj.duration)
traj_points.append(copy.deepcopy(traj_point))
### Display statistics
t4 = time.time()
print(">>>>> Statistics of TOPP <<<<<")
print("Dimension of path point: " + str(path_array.shape[0]))
print("Number of data points: " + str(path_array.shape[1]))
print("Using legacy: " + str(uselegacy))
print("Discretization step: " + str(discrtimestep) + " s")
print("Obtain request path and get prepared: " + str(t1 - t0) + " s")
print("Setup TOPP: " + str(t2 - t1) + " s")
print("Run TOPP: " + str(t3 - t2) + " s")
print("Convert resultant array to plan: " + str(t4 - t3) + " s")
print("Total: " + str(t4 - t0) + " s")
print("Trajectory duration before TOPP: " + str(path.duration) + " s")
print("Trajectory duration after TOPP: " + str(traj.duration) + " s")
print(">>>>> End <<<<<")
### Return result
res = PathToTrajResponse()
res.traj = traj_points
return res
ncurve = 1
discrtimestep = 0.01
vmax = zeros(ndof)
taumin = zeros(ndof)
taumax = zeros(ndof)
vmax[0:7] = vel_lim[0:7] # Velocity limits
taumin[0:7] = -robot.GetDOFMaxTorque()[0:7] # Torque limits
taumax[0:7] = robot.GetDOFMaxTorque()[0:7] # Torque limits
# Test begins here
nfail = 0
nsingulartreateds = 0
ntangenttreateds = 0
for j in range(ntraj):
print(j)
p0a = Utilities.vect2str_mintos(rand(ndof) * 2 * pi - pi)
p0b = Utilities.vect2str_mintos(rand(ndof) * 2 * pi - pi)
p1a = Utilities.vect2str_mintos(rand(ndof) * 2 * pi - pi)
p1b = Utilities.vect2str_mintos(rand(ndof) * 2 * pi - pi)
s = '%d' % ncurve
s += '\n1.0 ' + p0a + ' ' + p0b
for k in range(ncurve - 1):
a = rand(ndof) * 2 * pi - pi
b = rand(ndof) * 2 * pi - pi
c = 2 * b - a
pa = Utilities.vect2str(a)
pb = Utilities.vect2str(b)
pc = Utilities.vect2str(c)
s += ' ' + pa + ' ' + pb + '\n1.0 ' + pb + ' ' + pc
s += ' ' + p1a + ' ' + p1b
Tv, p0v, p1v, p2v, p3v = TOPPpy.string2p(s)
dof_lim=robot.GetDOFLimits()
vel_lim=robot.GetDOFVelocityLimits()
robot.SetDOFLimits(-10*ones(ndof),10*ones(ndof)) # Overrides robot joint limits for TOPP computations
robot.SetDOFVelocityLimits(100*vel_lim) # Override robot velocity limits for TOPP computations
# Trajectory
q0 = zeros(ndof)
q1 = zeros(ndof)
qd0 = ones(ndof)
qd1 = -ones(ndof)
q0[0:7] = [-2,0.5,1,3,-3,-2,-2]
q1[0:7] = [2,-0.5,-1,-1,1,1,1]
T = 1.5
trajectorystring = "%f\n%d"%(T,ndof)
for i in range(ndof):
a,b,c,d = Utilities.Interpolate3rdDegree(q0[i],q1[i],qd0[i],qd1[i],T)
trajectorystring += "\n%f %f %f %f"%(d,c,b,a)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajectorystring)
# Constraints
vmax = zeros(ndof)
taumin = zeros(ndof)
taumax = zeros(ndof)
vmax[0:7] = vel_lim[0:7] # Velocity limits
taumin[0:7] = -robot.GetDOFMaxTorque()[0:7] # Torque limits
taumax[0:7] = robot.GetDOFMaxTorque()[0:7] # Torque limits
# Set up the TOPP problem
discrtimestep = 0.005
uselegacy = False
t0 = time.time()
# You should have received a copy of the GNU Lesser General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
import string, time
from pylab import *
from numpy import *
from TOPP import TOPPbindings
from TOPP import TOPPpy
from TOPP import Trajectory
from TOPP import Utilities
# A two-dof path going through 5 viapoints (0,1) - (1,1) - (5,1) - (3,2) - (5,4)
data = np.loadtxt(sys.argv[1]).transpose()
path = data[0:3, :]
volumes = data[-1, :]
traj0 = Utilities.InterpolateViapoints(path) # Interpolate using splines
volume_rates = Utilities.InterpolateVolumeRates(volumes,
path) # Interpolate using
# PChip
# Display results
plot_dt = 0.001
ion()
figure_index = 0
from pylab import figure, clf, hold, plot, gca, axis, title, xlabel, ylabel, cycler
figure(figure_index)
volume_rates.Plot(plot_dt)
xlabel('$s$')
ylabel('Volume($V$) and DV/Ds')
figure_index = figure_index + 1
