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()
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)
# 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(traj0, traj1, dtplot, vmax, amax)
input()
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])
time.sleep(dt)
TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
TOPPpy.PlotKinematics(trajtotal,trajtotal2,dt,robot.vmax)
Bimanual.PlotTorques(robot,trajtotal,trajtotal2,dt)
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
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)
TOPPpy.PlotProfiles(profileslist, switchpointslist, figure_index)
x.WriteResultTrajectory()
traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
TOPPpy.PlotTSMap(traj1, svalues, figure_index + 1)
TOPPpy.PlotKinematics(traj1, traj1, plot_dt, vmax, amax, figure_index + 2)
TOPPpy.PlotMRR(traj1, volume_rates, svalues, plot_dt, [mrr_desired], 5)
np.savetxt(sys.argv[6], np.asarray([traj0.duration, traj1.duration]))
input()
