visualise real plan as graph (#49)

src/utils/plan.py

@@ -4,6 +4,7 @@ import pickle
 from bisect import bisect_left
 from .planning_tools import planTrajectory, JTrajectory
 from .simEnv import SimEnv
+import matplotlib.pyplot as plt
 
 
 def _emptyTraj(joints, arm):
@@ -130,9 +131,9 @@ class Plan:
         return JTrajectory(joint_list, np.arange(0,len(joint_list)/4, 0.25), arm)
 
 
-    def disp(self, t=10):
+    def watch(self, t=10):
         """
-        display the plan sped up
+        watch plan in pybullet GUI (sped up)
         """
         t_max = max([self.cumul_time[r][-1] for r in self.cumul_time])
         speed = t_max/t
@@ -145,6 +146,39 @@ class Plan:
         return
 
 
+    def visulise(self):
+        """
+        Display the plan as a graph showing when execting and when not
+        """
+        print("green vertical line mark start, red vertical linesmark end of contour")
+        fig, ax = plt.subplots()
+        for r in range(self.n):
+            for t in range(1,len(self.trajs[r])):
+                if self.trajs[r][t].contour is not None:
+                    print(t)
+                    ax.plot([self.cumul_time[r][t]- self.trajs[r][t].time[-1],
+                             self.cumul_time[r][t]],
+                            [r,r],
+                            'bo-',
+                            linewidth=2)
+
+                    #ax.plot([combined_schedule.start_times[r][t],
+                    #         combined_schedule.start_times[r][t]],
+                    #        [0,combined_schedule.n-1],
+                    #        'g--',
+                    #        linewidth=0.5)
+
+                    #ax.plot([combined_schedule.end_times[r][t],
+                    #         combined_schedule.end_times[r][t]],
+                    #        [0,combined_schedule.n-1],
+                    #        'r--',
+                    #        linewidth=0.5)
+        plt.xlabel("Time")
+        plt.ylabel("Robots")
+        plt.show()
+        return
+
+
     def save(self, filename):
         """
         pickle this plan