def interpMove(self,
start,
end,
seconds,
logFile=None,
extraLogInfoFn=None):
'''Moves between start and end over seconds seconds. start
and end may be functions of the time.'''
self.updateClock()
timeStart = self.time
timeEnd = self.time + seconds
ii = 0
tlast = self.time
while self.time < timeEnd:
#print 'time:', self.time
ii += 1
posS = start(self.time) if isinstance(start,
FunctionType) else start
posE = end(self.time) if isinstance(end, FunctionType) else end
print "Start: " + str(posS)
print "End: " + str(posE)
goal = lInterp(self.time, [timeStart, timeEnd], posS, posE)
#startSend = datetime.now()
cmdPos = self.commandPosition(goal)
#deltaTimeSend = datetime.now() - startSend
#print "sent: " + str(deltaTimeSend) #TODO: remove this line after testing
if logFile:
extraInfo = ''
if extraLogInfoFn:
extraInfo = extraLogInfoFn()
print >> logFile, self.time, ' '.join([str(x)
for x in cmdPos]),
#print >>logFile, ' '.join(str(ac.current_position) for ac in self.actuators),
print >> logFile, extraInfo
#volts = ['%d: %s' % (ii,ac.current_voltage) for ii,ac in enumerate(self.actuators)]
#print ' '.join(volts)
#[ac.read_all() for ac in self.actuators]
#positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)]
#print ' '.join(positions)
#print ''.join(['x' if ac.led else ' ' for ac in self.actuators])
#sleep(self.sleep)
#sleep(float(1)/100)
self.updateClock()
secElapsed = self.time - tlast
tosleep = self.sleep - secElapsed
if tosleep > 0:
sleep(tosleep)
self.updateClock()
tlast = self.time
def interpMove(self, start, end, seconds, logFile=None, extraLogInfoFn=None):
'''Moves between start and end over seconds seconds. start
and end may be functions of the time.'''
self.updateClock()
timeStart = self.time
timeEnd = self.time + seconds
ii = 0
tlast = self.time
while self.time < timeEnd:
#print 'time:', self.time
ii += 1
posS = start(self.time) if isinstance(start, FunctionType) else start
posE = end(self.time) if isinstance(end, FunctionType) else end
print "Start: " + str(posS)
print "End: " + str(posE)
goal = lInterp(self.time, [timeStart, timeEnd], posS, posE)
#startSend = datetime.now()
cmdPos = self.commandPosition(goal)
#deltaTimeSend = datetime.now() - startSend
#print "sent: " + str(deltaTimeSend) #TODO: remove this line after testing
if logFile:
extraInfo = ''
if extraLogInfoFn:
extraInfo = extraLogInfoFn()
print >>logFile, self.time, ' '.join([str(x) for x in cmdPos]),
#print >>logFile, ' '.join(str(ac.current_position) for ac in self.actuators),
print >>logFile, extraInfo
#volts = ['%d: %s' % (ii,ac.current_voltage) for ii,ac in enumerate(self.actuators)]
#print ' '.join(volts)
#[ac.read_all() for ac in self.actuators]
#positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)]
#print ' '.join(positions)
#print ''.join(['x' if ac.led else ' ' for ac in self.actuators])
#sleep(self.sleep)
#sleep(float(1)/100)
self.updateClock()
secElapsed = self.time - tlast
tosleep = self.sleep - secElapsed
if tosleep > 0:
sleep(tosleep)
self.updateClock()
tlast = self.time
def repeating_motion(time, intervals, poses):
'''Return position assumign repeated motion between the given
poses where each segment takes a length of time given by
intervals.
'''
assert len(intervals) == len(poses)
time_points = cumsum([0] + intervals)
total_duration = time_points[-1]
relative_time = time % total_duration
for ii in xrange(len(time_points) - 1):
if (relative_time < time_points[ii + 1]):
return lInterp(relative_time,
[time_points[ii], time_points[ii + 1]], poses[ii],
poses[(ii + 1) % len(poses)])
raise Exception('Logic error; should not get here')
def repeating_motion(time, intervals, poses):
'''Return position assumign repeated motion between the given
poses where each segment takes a length of time given by
intervals.
'''
assert len(intervals) == len(poses)
time_points = cumsum([0] + intervals)
total_duration = time_points[-1]
relative_time = time % total_duration
for ii in xrange(len(time_points) - 1):
if (relative_time < time_points[ii+1]):
return lInterp(relative_time,
[time_points[ii], time_points[ii+1]],
poses[ii],
poses[(ii+1)%len(poses)])
raise Exception('Logic error; should not get here')