def plot_profiles(self):
import pylab
self.topp.WriteProfilesList()
self.topp.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(
self.topp.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(
self.topp.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist, switchpointslist)
# TOPPpy.PlotAlphaBeta(topp)
pylab.title("%s phase profile" % type(self).__name__)
pylab.axis([0, 1, 0, 10])
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)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist, switchpointslist, 4)
x.WriteResultTrajectory()
traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
dtplot = 0.01
TOPPpy.PlotKinematics(traj, traj1, dtplot, vmax, accelmax)
input()
amax = 10 * ones(ndof) # Acceleration limits
# Set up the TOPP instance
discrtimestep = 0.01
uselegacy = True
t0 = time.time()
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += "\n" + " ".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" + " ".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)
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)
trajectorystring = TOPPpy.BezierToTrajectoryString(Tv,p0v,p1v,p2v,p3v)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajectorystring)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(traj0, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None,"QuadraticConstraints",constraintstring,trajectorystring);
x.extrareps = 5
ret = x.RunComputeProfiles(0,0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
#TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
#input()
if(ret == 1):
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
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)
trajectorystring = TOPPpy.BezierToTrajectoryString(Tv, p0v, p1v, p2v, p3v)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(
trajectorystring)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += TOPPopenravepy.ComputeTorquesConstraints(
robot, traj0, taumin, taumax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
x.extrareps = 5
ret = x.RunComputeProfiles(0, 0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
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)
trajectorystring = TOPPpy.BezierToTrajectoryString(Tv, p0v, p1v, p2v, p3v)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(
trajectorystring)
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)
x.extrareps = 5
ret = x.RunComputeProfiles(0, 0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
robot.taumin = taumin
robot.taumax = taumax
t1 = time.time()
x = TOPPbindings.TOPPInstance(None,"PolygonConstraints",constraintstring,trajectorystring)
x.integrationtimestep = 1e-3
ret = x.RunComputeProfiles(1,1)
t2 = time.time()
print("Compute Polygon constraints:", t1-t0, "seconds")
print("Note : Compute Polygon is faster with the C++ version, checkout branch tomas-develop")
print("Run TOPP:", t2-t1, "seconds")
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
if(ret == 1):
x.ReparameterizeTrajectory()
x.WriteResultTrajectory()
trajtotal2 = Trajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)
dt=0.01
robot1.SetTransform(T1)
robot2.SetTransform(T2)
traj = trajtotal
for t in arange(0,traj.duration,dt):
q = traj.Eval(t)
robot1.SetDOFValues(q[0:3])
robot2.SetDOFValues(q[3:5])
# Constraints
vmax = float(sys.argv[2]) * np.ones(traj0.dimension)
amax = float(sys.argv[3]) * np.ones(traj0.dimension)
mrr_desired = float(sys.argv[4])
# Set up the TOPP instance
trajectorystring = str(traj0)
discrtimestep = float(sys.argv[5])
ndiscrsteps = int((traj0.duration + 1e-10) / discrtimestep) + 1
TOPPbindings.passswitchpointnsteps = 100
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeMaterialRemovalConstraints(
traj0, amax, mrr_desired, volume_rates, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints", constraintstring,
trajectorystring)
# Run TOPP
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory(0.0005)
x.WriteProfilesList()
x.WriteSwitchPointsList()
x.WriteTSMap()
svalues = np.fromstring(x.tsmapstring, sep='\n')
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)