def preview_traj(self):
#Assume that robot is in initial position now
T0=self.base.GetTransform()
s=self.robot.CreateRobotStateSaver()
for k in xrange(self.srcdata.shape[0]):
pose_map={key:self.srcdata[k,v] for (key,v) in self.joint_map.items()}
pose_array=array([pose_map[dof] if pose_map.has_key(dof) else 0.0 for dof in xrange(self.robot.GetDOF())])
values=pose_array*self.joint_signs+self.joint_offsets
self.robot.SetDOFValues(values)
T=self.get_transform(T0,self.srcdata[k,0:6])
self.robot.SetTransform(T)
_time.sleep(self.timestep/2.)
if kbhit.kbhit():
kbhit.getch()
_oh.pause()
s.Restore()
def preview_traj(self):
#Assume that robot is in initial position now
T0=self.base.GetTransform()
s=self.robot.CreateRobotStateSaver()
for k in xrange(self.srcdata.shape[0]):
pose_map={key:self.srcdata[k,v] for (key,v) in self.joint_map.items()}
pose_array=array([pose_map[dof] if pose_map.has_key(dof) else 0.0 for dof in xrange(self.robot.GetDOF())])
values=pose_array*self.joint_signs+self.joint_offsets
self.robot.SetDOFValues(values)
T=self.get_transform(T0,self.srcdata[k,0:6])
self.robot.SetTransform(T)
_time.sleep(self.timestep/2.)
if kbhit.kbhit():
kbhit.getch()
_oh.pause()
s.Restore()
def wait_start():
print "Press Enter to abort, starting simulation in ... "
for x in range(5,0,-1):
print '{}...'.format(x)
for k in range(10):
time.sleep(.1)
k=False
if kbhit.kbhit():
k=kbhit.getch()
if k:
return False
print "Starting simulation!"
return True
def wait_start():
print "Press Enter to abort, starting simulation in ... "
for x in range(5, 0, -1):
print '{}...'.format(x)
for k in range(10):
time.sleep(.1)
k = False
if kbhit.kbhit():
k = kbhit.getch()
if k:
return False
print "Starting simulation!"
return True
