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