add watching a portion of a plan functionality

src/utils/plan.py

@@ -131,17 +131,19 @@ class Plan:
         return JTrajectory(joint_list, np.arange(0,len(joint_list)/4, 0.25), arm)
 
 
-    def watch(self, t=10):
+    def watch(self, t=10, start=0, end=1):
         """
         watch plan in pybullet GUI (sped up)
         """
         frame_rate = 60
-        t_max = max([self.cumul_time[r][-1] for r in self.cumul_time])
-        speed = t_max/t
+        t_total = max([self.cumul_time[r][-1] for r in self.cumul_time])
+        t_max = t_total*end
+        t_min = t_total*start
+        speed = (t_max-t_min)/t
         tsteps = int(t*frame_rate)
         for t in range(tsteps):
             for r in range(self.n):
-                j = self.jointsAtTime(t*speed/frame_rate, r)
+                j = self.jointsAtTime(t_min + t*speed/frame_rate, r)
                 self.cc.setJoints(r,j)
             time.sleep(1/frame_rate)
         return