Commit 1fd5213a authored by Jayant Khatkar's avatar Jayant Khatkar

add watching a portion of a plan functionality

parent 5f99628f
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment