def plot_trajectories(self, trajectories, joint_names, show_plot=True):
try:
print " Get spline list for each trajectory"
master_spline_list = []
for trajectory in trajectories:
# Generate spline data
spline_list, dT, num_segments, num_joints = traj_utility.define_trajectory_splines(
trajectory)
master_spline_list.append((spline_list, dT))
print " Plot per joint ..."
for jnt, joint_name in enumerate(joint_names):
# define figure and axes
print "Plot trajectories for joint ", joint_name
fig = plt.figure()
ax1 = fig.add_subplot(141)
ax2 = fig.add_subplot(142)
ax3 = fig.add_subplot(143)
ax4 = fig.add_subplot(144)
axes = (ax1, ax2, ax3, ax4)
for ax in axes:
ax.hold(True)
for traj in range(len(trajectories)):
#print " plot joint splines to figure ..."
splines = master_spline_list[traj][0][jnt]
dT = master_spline_list[traj][1]
self.plot_splines(axes, splines, dT,
self.colors[traj % len(self.colors)])
self.plot_knots(axes, splines, dT,
self.colors[traj % len(self.colors)],
self.style[traj % len(self.style)])
fig.suptitle(joint_name)
if (show_plot):
plt.show()
print "Done plotting!"
except Exception as msg:
print "Exception in VigirTrajectoryPlotter.plot_trajectories"
print msg
def plot_trajectories(self, trajectories, joint_names,show_plot=True):
try:
print " Get spline list for each trajectory"
master_spline_list = []
for trajectory in trajectories:
# Generate spline data
spline_list, dT, num_segments, num_joints = traj_utility.define_trajectory_splines(trajectory)
master_spline_list.append( (spline_list, dT) )
print " Plot per joint ..."
for jnt, joint_name in enumerate(joint_names):
# define figure and axes
print "Plot trajectories for joint ", joint_name
fig = plt.figure()
ax1 = fig.add_subplot(141)
ax2 = fig.add_subplot(142)
ax3 = fig.add_subplot(143)
ax4 = fig.add_subplot(144)
axes = (ax1, ax2, ax3, ax4)
for ax in axes:
ax.hold(True)
for traj in range(len(trajectories)):
#print " plot joint splines to figure ..."
splines = master_spline_list[traj][0][jnt]
dT = master_spline_list[traj][1]
self.plot_splines(axes, splines, dT, self.colors[traj%len(self.colors)])
self.plot_knots(axes, splines, dT, self.colors[traj%len(self.colors)], self.style[traj%len(self.style)])
fig.suptitle(joint_name)
if (show_plot):
plt.show()
print "Done plotting!"
except Exception as msg:
print "Exception in VigirTrajectoryPlotter.plot_trajectories"
print msg
def reparameterize(self, old_trajectory):
try:
count = 0
elapsed = 0.0
violation = True # Assume we violate a limit to start
continue_flag = True
start_time = time.clock()
#if len(old_trajectory.points[0].accelerations) > 0:
# print "Assume valid quintic spline - no reparameterization!"
# return old_trajectory
#if len(old_trajectory.points[0].velocities) > 0:
# print "Assume valid cubic spline - no reparameterization!"
# return old_trajectory
new_trajectory = deepcopy(old_trajectory)
if (len(new_trajectory.points[0].velocities) == 0):
#print " Old trajectory does not have velocities - add for smoothing"
for point in new_trajectory.points:
point.velocities = [
0.0 for x in xrange(len(point.positions))
]
if (len(new_trajectory.points[0].accelerations) == 0):
#print " Old trajectory does not have accelerations - add for smoothing"
for point in new_trajectory.points:
point.accelerations = [
0.0 for x in xrange(len(point.positions))
]
if (len(old_trajectory.points[0].velocities) == 0):
# Call the specific reparameterization defined by smoother
new_trajectory = self.piecewise_smoother.reparameterize(
new_trajectory)
else:
# If we have velocities or accelerations, then assume this is a MoveIt! parabolicly smoothed trajectory or native cubic
new_trajectory = self.cubic_smoother.reparameterize(
new_trajectory)
############################################################################################################################
try:
if (self.test_trajectories or self.plot_trajectories):
print "vvvvvvvvvvvvvvvvvvvv Test vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv"
spline_list, dT_new, num_segments2, num_joints = traj_utility.define_trajectory_splines(
new_trajectory)
if (self.test_trajectories):
print "Test trajectory splines ..."
dT = deepcopy(dT_new)
violation = False
for jnt, joint_name in enumerate(
new_trajectory.joint_names):
jnt_violation, dT_new = self.check_limits(
dT, spline_list[jnt], dT_new, joint_name)
#if (jnt_violation):
# print "Limit violation for ",joint_name
violation = violation or jnt_violation
if (violation):
print " At least one joint violation in final trajectory!"
if (self.plot_trajectories):
self.plotter.plot_splines_list(
spline_list,
dT,
new_trajectory.joint_names,
show_plot=False)
print "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"
except Exception as msg:
print "Exception during trajectory testing after smoother"
##print "vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv"
##print new_trajectory
##print "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"
print " ", msg
traj_utility.PrintException()
sys.exit(-1)
############################################################################################################################
return new_trajectory
except Exception as msg:
print "Exception during trajectory reparameterization"
print msg
print "Force piecewise linear commands ..."
new_trajectory = deepcopy(old_trajectory)
for point in new_trajectory.points:
point.velocities = ()
point.accelerations = ()
return new_trajectory
def reparameterize(self, old_trajectory):
try:
count = 0
elapsed = 0.0
violation = True # Assume we violate a limit to start
continue_flag = True
start_time = time.clock()
#if len(old_trajectory.points[0].accelerations) > 0:
# print "Assume valid quintic spline - no reparameterization!"
# return old_trajectory
#if len(old_trajectory.points[0].velocities) > 0:
# print "Assume valid cubic spline - no reparameterization!"
# return old_trajectory
new_trajectory = deepcopy(old_trajectory)
if (len(new_trajectory.points[0].velocities) == 0):
#print " Old trajectory does not have velocities - add for smoothing"
for point in new_trajectory.points:
point.velocities = [0.0 for x in xrange(len(point.positions)) ]
if (len(new_trajectory.points[0].accelerations) == 0):
#print " Old trajectory does not have accelerations - add for smoothing"
for point in new_trajectory.points:
point.accelerations = [0.0 for x in xrange(len(point.positions)) ]
if (len(old_trajectory.points[0].velocities) == 0):
# Call the specific reparameterization defined by smoother
new_trajectory = self.piecewise_smoother.reparameterize(new_trajectory)
else:
# If we have velocities or accelerations, then assume this is a MoveIt! parabolicly smoothed trajectory or native cubic
new_trajectory = self.cubic_smoother.reparameterize(new_trajectory)
############################################################################################################################
try:
if (self.test_trajectories or self.plot_trajectories):
print "vvvvvvvvvvvvvvvvvvvv Test vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv"
spline_list, dT_new, num_segments2, num_joints = traj_utility.define_trajectory_splines(new_trajectory)
if (self.test_trajectories):
print "Test trajectory splines ..."
dT = deepcopy(dT_new)
violation = False
for jnt, joint_name in enumerate(new_trajectory.joint_names):
jnt_violation, dT_new = self.check_limits(dT, spline_list[jnt], dT_new, joint_name)
#if (jnt_violation):
# print "Limit violation for ",joint_name
violation = violation or jnt_violation
if (violation):
print " At least one joint violation in final trajectory!"
if (self.plot_trajectories):
self.plotter.plot_splines_list(spline_list, dT, new_trajectory.joint_names,show_plot=False)
print "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"
except Exception as msg:
print "Exception during trajectory testing after smoother"
##print "vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv"
##print new_trajectory
##print "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"
print " ",msg
traj_utility.PrintException()
sys.exit(-1)
############################################################################################################################
return new_trajectory
except Exception as msg:
print "Exception during trajectory reparameterization"
print msg
print "Force piecewise linear commands ..."
new_trajectory = deepcopy(old_trajectory)
for point in new_trajectory.points:
point.velocities = ()
point.accelerations = ()
return new_trajectory
