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()
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
