Exemplo n.º 1
0
    def initServos(self):
        # Set your serial port accordingly.
        if os.name == "posix":
            possibilities = ['dev/ttyACM'
                             ] + ['/dev/ttyACM' + str(i) for i in range(25)]
            portName = None
            for pos in possibilities:
                if os.path.exists(pos):
                    portName = pos
            if portName is None:
                raise Exception('Could not find any of %s' %
                                repr(possibilities))
        else:
            portName = "COM9"

        serial = SerialStream(port=portName, baudrate=self.baudRate, timeout=1)
        self.net = DynamixelNetwork(serial)

        #print 'Prescan...'
        #print self.net.get_dynamixels()

        print "Scanning for Dynamixels...",
        self.net.scan(min(self.expectedIds), max(self.expectedIds))

        self.actuators = []
        self.actuatorIds = []

        for dyn in self.net.get_dynamixels():
            print dyn.id,
            self.actuatorIds.append(dyn.id)
            self.actuators.append(self.net[dyn.id])
        print "...Done"

        if len(self.actuators) != self.nServos and not self.silentNetFail:
            raise RobotFailure(
                'Expected to find %d servos on network, but only got %d (%s)' %
                (self.nServos, len(self.actuators), repr(self.actuatorIds)))

        for actuator in self.actuators:
            #actuator.moving_speed = 90
            #actuator.synchronized = True
            #actuator.torque_enable = True
            #actuator.torque_limit = 1000
            #actuator.max_torque = 1000

            actuator.moving_speed = 250
            actuator.synchronized = True
            actuator.torque_enable = True
            actuator.torque_limit = 1000
            actuator.max_torque = 1000
            actuator.ccw_compliance_margin = 3
            actuator.cw_compliance_margin = 3

        self.net.synchronize()

        #print 'options are:'
        #for op in dir(self.actuators[0]):
        #    print '  ', op
        #for ac in self.actuators:
        #    print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load
        #    ac.read_all()

        self.currentPos = None
        self.resetClock()
Exemplo n.º 2
0
    def initServos(self):
        # Set your serial port accordingly.
        if os.name == "posix":
            possibilities = ['dev/ttyACM'] + ['/dev/ttyACM' + str(i) for i in range(25)]
            portName = None
            for pos in possibilities:
                if os.path.exists(pos):
                    portName = pos
            if portName is None:
                raise Exception('Could not find any of %s' % repr(possibilities))
        else:
            portName = "COM9"

        serial = SerialStream(port = portName,
                                        baudrate = self.baudRate,
                                        timeout = 1)
        self.net = DynamixelNetwork(serial)

        #print 'Prescan...'
        #print self.net.get_dynamixels()

        print "Scanning for Dynamixels...",
        self.net.scan(min(self.expectedIds), max(self.expectedIds))

        self.actuators   = []
        self.actuatorIds = []

        for dyn in self.net.get_dynamixels():
            print dyn.id,
            self.actuatorIds.append(dyn.id)
            self.actuators.append(self.net[dyn.id])
        print "...Done"

        if len(self.actuators) != self.nServos and not self.silentNetFail:
            raise RobotFailure('Expected to find %d servos on network, but only got %d (%s)'
                               % (self.nServos, len(self.actuators), repr(self.actuatorIds)))

        for actuator in self.actuators:
            #actuator.moving_speed = 90
            #actuator.synchronized = True
            #actuator.torque_enable = True
            #actuator.torque_limit = 1000
            #actuator.max_torque = 1000

            actuator.moving_speed = 250
            actuator.synchronized = True
            actuator.torque_enable = True
            actuator.torque_limit = 1000
            actuator.max_torque = 1000
            actuator.ccw_compliance_margin = 3
            actuator.cw_compliance_margin  = 3

        self.net.synchronize()

        #print 'options are:'
        #for op in dir(self.actuators[0]):
        #    print '  ', op
        #for ac in self.actuators:
        #    print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load
        #    ac.read_all()

        self.currentPos = None
        self.resetClock()
Exemplo n.º 3
0
class RobotPi():
    ''''''
    def __init__(self,
                 silentNetFail=False,
                 expectedIds=None,
                 commandRate=40,
                 loud=False,
                 skipInit=False):
        '''Initialize the robot.
        
        Keyword arguments:

        silentNetworkFail -- Whether or not to fail silently if the
                             network does not find all the dynamixel
                             servos.

        nServos -- How many servos are connected to the robot,
                   i.e. how many to expect to find on the network.

        commandRate -- Rate at which the motors should be commanded,
                   in Hertz.  Default: 40.
        '''

        # The number of Dynamixels on the bus.
        self.expectedIds = expectedIds if expectedIds is not None else range(8)
        self.nServos = len(self.expectedIds)
        self.silentNetFail = silentNetFail

        self.sleep = 1. / float(commandRate)
        self.loud = loud

        #if self.nServos != 9:
        #    pass
        #    #raise Exception('Unfortunately, the Robot class currently assumes 9 servos.')

        # Default baud rate of the USB2Dynamixel device.
        self.baudRate = 1000000

        if not skipInit:
            self.initServos()

    def initServos(self):
        # Set your serial port accordingly.
        if os.name == "posix":
            possibilities = ['dev/ttyACM'
                             ] + ['/dev/ttyACM' + str(i) for i in range(25)]
            portName = None
            for pos in possibilities:
                if os.path.exists(pos):
                    portName = pos
            if portName is None:
                raise Exception('Could not find any of %s' %
                                repr(possibilities))
        else:
            portName = "COM9"

        serial = SerialStream(port=portName, baudrate=self.baudRate, timeout=1)
        self.net = DynamixelNetwork(serial)

        #print 'Prescan...'
        #print self.net.get_dynamixels()

        print "Scanning for Dynamixels...",
        self.net.scan(min(self.expectedIds), max(self.expectedIds))

        self.actuators = []
        self.actuatorIds = []

        for dyn in self.net.get_dynamixels():
            print dyn.id,
            self.actuatorIds.append(dyn.id)
            self.actuators.append(self.net[dyn.id])
        print "...Done"

        if len(self.actuators) != self.nServos and not self.silentNetFail:
            raise RobotFailure(
                'Expected to find %d servos on network, but only got %d (%s)' %
                (self.nServos, len(self.actuators), repr(self.actuatorIds)))

        for actuator in self.actuators:
            #actuator.moving_speed = 90
            #actuator.synchronized = True
            #actuator.torque_enable = True
            #actuator.torque_limit = 1000
            #actuator.max_torque = 1000

            actuator.moving_speed = 250
            actuator.synchronized = True
            actuator.torque_enable = True
            actuator.torque_limit = 1000
            actuator.max_torque = 1000
            actuator.ccw_compliance_margin = 3
            actuator.cw_compliance_margin = 3

        self.net.synchronize()

        #print 'options are:'
        #for op in dir(self.actuators[0]):
        #    print '  ', op
        #for ac in self.actuators:
        #    print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load
        #    ac.read_all()

        self.currentPos = None
        self.resetClock()

    def run(self,
            motionFunction,
            runSeconds=10,
            resetFirst=True,
            interpBegin=0,
            interpEnd=0,
            timeScale=1,
            logFile=None,
            extraLogInfoFn=None):
        '''Run the robot with a given motion generating function.

        Positional arguments:
        
        motionFunction -- Function used to generate the desired motor
                          positions.  This function must take a single
                          argument -- time, in seconds -- and must
                          return the desired length 9 vector of motor
                          positions.  The current implementation
                          expects that this function will be
                          deterministic.
        
        Keyword arguments:

        runSeconds -- How many seconds to run for.  This is in
                      addition to the time added for interpBegin and
                      interpEnd, if any.  Default: 10

        resetFirst -- Begin each run by resetting the robot to its
                      base position, currently implemented as a
                      transition from CURRENT -> POS_FLAT ->
                      POS_READY.  Default: True

        interpBegin -- Number of seconds over which to interpolate
                      from current position to commanded positions.
                      If this is not None, the robot will spend the
                      first interpBegin seconds interpolating from its
                      current position to that specified by
                      motionFunction.  This should probably be used
                      for motion models which do not return POS_READY
                      at time 0.  Affected by timeScale. Default: None

        interpEnd -- Same as interpBegin, but at the end of motion.
                      If interpEnd is not None, interpolation is
                      performed from final commanded position to
                      POS_READY, over the given number of
                      seconds. Affected by timeScale.  Default: None
                      
        timeScale -- Factor by which time should be scaled during this
                      run, higher is slower. Default: 1
                      
        logFile -- File to log time/positions to, should already be
                      opened. Default: None

        extraLogInfoFn -- Function to call and append info to every
                      line the log file. Should return a
                      string. Default: None
        '''

        #net, actuators = initialize()

        #def run(self, motionFunction, runSeconds = 10, resetFirst = True
        #    interpBegin = 0, interpEnd = 0):

        if self.loud:
            print 'Starting motion.'

        failures = self.pingAll()
        if failures:
            self.initServos()
            failures = self.pingAll()
            if failures:
                raise RobotFailure(
                    'Could not communicate with servos %s at beginning of run.'
                    % repr(failures))

        self.resetClock()
        self.currentPos = self.readCurrentPosition()

        if logFile:
            #print >>logFile, '# time, servo goal positions (9), servo actual positions (9), robot location (x, y, age)'
            print >> logFile, '# time, servo goal positions (9), robot location (x, y, age)'

        # Reset the robot position, if desired
        if resetFirst:
            self.interpMove(self.readCurrentPosition(), POS_FLAT, 3)
            print "POS_FLAT: " + str(POS_FLAT)
            print "POS_READY: " + str(POS_READY)
            self.interpMove(POS_FLAT, POS_READY, 3)
            #self.interpMove(POS_READY, POS_HALFSTAND, 4)
            self.currentPos = POS_READY
            self.resetClock()

        # Begin with a segment smoothly interpolated between the
        # current position and the motion model.
        if interpBegin is not None:
            self.interpMove(self.currentPos,
                            scaleTime(motionFunction, timeScale),
                            interpBegin * timeScale, logFile, extraLogInfoFn)
            self.currentPos = motionFunction(self.time)

        # Main motion segment
        self.interpMove(scaleTime(motionFunction, timeScale),
                        scaleTime(motionFunction, timeScale),
                        runSeconds * timeScale, logFile, extraLogInfoFn)
        self.currentPos = motionFunction(self.time)

        # End with a segment smoothly interpolated between the
        # motion model and a ready position.
        if interpEnd is not None:
            self.interpMove(scaleTime(motionFunction, timeScale), POS_READY,
                            interpEnd * timeScale, logFile, extraLogInfoFn)

        failures = self.pingAll()
        if failures:
            # give it a second chance
            sleep(1)
            failures = self.pingAll()
            if failures:
                raise RobotFailure('Servos %s may have died during run.' %
                                   repr(failures))

    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

#
#        currentPos = None
#
#        if resetFirst:
#            currentPos = self.currentPostion()
#            time0 = datetime.datetime.now()
#            seconds = 0
#
#            while seconds < 10:
#                timeDiff = datetime.datetime.now() - time0
#                seconds  = timeDiff.seconds + timeDiff.microseconds/1e6
#
#                if seconds < 3:
#                    goal = lInterp(seconds, [0, 3], startingPos, POS_FLAT)
#                elif seconds < 6:
#                    goal = lInterp(seconds, [3, 6], POS_FLAT, POS_READY)
#                elif seconds < 10:
#                    goal = lInterp(seconds, [6, 10], POS_READY, POS_HALFSTAND)
#                else:
#                    break
#
#                self.commandPosition(goal)
#                sleep(self.sleep)
#
#            currentPos = POS_HALFSTAND
#
#        if interpBegin is not None:
#            if currentPos is None:
#                currentPos = self.currentPostion()
#
#            time0 = datetime.datetime.now()
#            seconds = 0
#
#            while seconds < interpBegin:
#                timeDiff = datetime.datetime.now() - time0
#                seconds  = timeDiff.seconds + timeDiff.microseconds/1e6
#
#                goal = lInterp(seconds, [0, interpBegin], currentPos, motionFunction(seconds))
#
#                self.commandPosition(goal)
#                time.sleep(self.sleep)
#

    def resetClock(self):
        '''Resets the robot time to zero'''
        self.time0 = datetime.now()
        self.time = 0.0

    def updateClock(self):
        '''Updates the Robots clock to the current time'''
        timeDiff = datetime.now() - self.time0
        self.time = timeDiff.seconds + timeDiff.microseconds / 1e6

    def readyPosition(self, persist=False):
        if persist:
            self.resetClock()
            while self.time < 2.0:
                self.commandPosition(POS_READY)
                sleep(.1)
                self.updateClock()
        else:
            self.commandPosition(POS_READY)
            sleep(2)

    def commandPosition(self, position, crop=True, cropWarning=False):
        '''Command the given position

        commandPosition will command the robot to move its servos to
        the given position vector.  This vector is cropped to
        the physical limits of the robot and converted to integer

        Positional arguments:
        position -- A length 9 vector of desired positions.

        Keyword arguments:
        cropWarning -- Whether or not to print a warning if the
                       positions are cropped.  Default: False.
        '''

        if len(position) != self.nServos:
            raise Exception(
                'Expected postion vector of length %d, got %s instead' %
                (self.nServos, repr(position)))

        if crop:
            goalPosition = self.cropPosition([int(xx) for xx in position],
                                             cropWarning)
        else:
            goalPosition = [int(xx) for xx in position]

        if self.loud:
            posstr = ', '.join(['%4d' % xx for xx in goalPosition])
            print '%.2fs -> %s' % (self.time, posstr)

        for ii, actuator in enumerate(self.actuators):
            actuator.goal_position = goalPosition[ii]
        self.net.synchronize()

        #[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]) + '  ' ,#XXX:THIS IS THE SLOW PART!!!
        #print ' '.join(['%.1f' % ac.current_voltage for ac in self.actuators])#XXX:OR THIS

        return goalPosition

    def cropPosition(self, position, cropWarning=False):
        '''Crops the given positions to their appropriate min/max values.
        
        Requires a vector of length 9 to be sure the IDs are in the
        assumed order.'''

        if len(position) != self.nServos:
            raise Exception('cropPosition expects a vector of length %d' %
                            self.nServos)

        ret = copy(position)
        for ii in [0, 2, 4, 6]:
            ret[ii] = max(MIN_INNER, min(MAX_INNER, ret[ii]))
            ret[ii + 1] = max(MIN_OUTER, min(MAX_OUTER, ret[ii + 1]))

        if cropWarning and ret != position:
            print 'Warning: cropped %s to %s' % (repr(position), repr(ret))

        return ret

    def readCurrentPosition(self):
        ret = []
        if len(self.actuators) != self.nServos:
            raise RobotFailure('Lost some servos, now we only have %d' %
                               len(self.actuators))
        for ac in self.actuators:
            #ac.read_all()
            #ret.append(ac.cache[dynamixel.defs.REGISTER['CurrentPosition']])
            ret.append(ac.current_position)
            #sleep(.001)
        return ret

    def pingAll(self):
        failures = []
        for ii in self.actuatorIds:
            result = self.net.ping(ii)
            if result is False:
                failures.append(ii)
        return failures

    def printStatus(self):
        pos = self.readCurrentPosition()
        print 'Positions:', ' '.join(
            ['%d:%d' % (ii, pp) for ii, pp in enumerate(pos)])

    def shimmy(self):
        '''Moves through a set of checks and makes sure the robot is
        still moving.'''

        self.commandPosition(POS_READY)
        sleep(.8)

        success = True
        success &= self.checkMove(POS_READY, POS_CHECK_1)
        success &= self.checkMove(POS_CHECK_1, POS_CHECK_2)
        success &= self.checkMove(POS_CHECK_2, POS_CHECK_3)
        success &= self.checkMove(POS_CHECK_3, POS_CHECK_2)
        success &= self.checkMove(POS_CHECK_2, POS_CHECK_1)
        success &= self.checkMove(POS_CHECK_1, POS_READY)

        return success

    def checkMove(self, aa, bb):
        aa = array(aa)
        bb = array(bb)
        self.commandPosition(aa)

        posAA = array(self.readCurrentPosition())

        self.commandPosition(bb)

        sleep(.4)
        posBB = array(self.readCurrentPosition())

        success = True
        success &= all(abs((aa - bb) - (posAA - posBB)) < 50)
        success &= all(abs(aa - posAA) < 50)
        success &= all(abs(bb - posBB) < 50)

        if not success and False:
            print 'shimmy errors'
            print(aa - bb) - (posAA - posBB)
            print aa - posAA
            print bb - posBB
        return success
Exemplo n.º 4
0
class RobotPi():
    ''''''

    def __init__(self, silentNetFail = False, expectedIds = None, commandRate = 40,
                 loud = False, skipInit = False):
        '''Initialize the robot.
        
        Keyword arguments:

        silentNetworkFail -- Whether or not to fail silently if the
                             network does not find all the dynamixel
                             servos.

        nServos -- How many servos are connected to the robot,
                   i.e. how many to expect to find on the network.

        commandRate -- Rate at which the motors should be commanded,
                   in Hertz.  Default: 40.
        '''

        # The number of Dynamixels on the bus.
        self.expectedIds   = expectedIds if expectedIds is not None else range(8)
        self.nServos       = len(self.expectedIds)
        self.silentNetFail = silentNetFail

        self.sleep = 1. / float(commandRate)
        self.loud  = loud

        #if self.nServos != 9:
        #    pass
        #    #raise Exception('Unfortunately, the Robot class currently assumes 9 servos.')

        # Default baud rate of the USB2Dynamixel device.
        self.baudRate = 1000000

        if not skipInit:
            self.initServos()


    def initServos(self):
        # Set your serial port accordingly.
        if os.name == "posix":
            possibilities = ['dev/ttyACM'] + ['/dev/ttyACM' + str(i) for i in range(25)]
            portName = None
            for pos in possibilities:
                if os.path.exists(pos):
                    portName = pos
            if portName is None:
                raise Exception('Could not find any of %s' % repr(possibilities))
        else:
            portName = "COM9"

        serial = SerialStream(port = portName,
                                        baudrate = self.baudRate,
                                        timeout = 1)
        self.net = DynamixelNetwork(serial)

        #print 'Prescan...'
        #print self.net.get_dynamixels()

        print "Scanning for Dynamixels...",
        self.net.scan(min(self.expectedIds), max(self.expectedIds))

        self.actuators   = []
        self.actuatorIds = []

        for dyn in self.net.get_dynamixels():
            print dyn.id,
            self.actuatorIds.append(dyn.id)
            self.actuators.append(self.net[dyn.id])
        print "...Done"

        if len(self.actuators) != self.nServos and not self.silentNetFail:
            raise RobotFailure('Expected to find %d servos on network, but only got %d (%s)'
                               % (self.nServos, len(self.actuators), repr(self.actuatorIds)))

        for actuator in self.actuators:
            #actuator.moving_speed = 90
            #actuator.synchronized = True
            #actuator.torque_enable = True
            #actuator.torque_limit = 1000
            #actuator.max_torque = 1000

            actuator.moving_speed = 250
            actuator.synchronized = True
            actuator.torque_enable = True
            actuator.torque_limit = 1000
            actuator.max_torque = 1000
            actuator.ccw_compliance_margin = 3
            actuator.cw_compliance_margin  = 3

        self.net.synchronize()

        #print 'options are:'
        #for op in dir(self.actuators[0]):
        #    print '  ', op
        #for ac in self.actuators:
        #    print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load
        #    ac.read_all()

        self.currentPos = None
        self.resetClock()

        
    def run(self, motionFunction, runSeconds = 10, resetFirst = True,
            interpBegin = 0, interpEnd = 0, timeScale = 1, logFile = None,
            extraLogInfoFn = None):
        '''Run the robot with a given motion generating function.

        Positional arguments:
        
        motionFunction -- Function used to generate the desired motor
                          positions.  This function must take a single
                          argument -- time, in seconds -- and must
                          return the desired length 9 vector of motor
                          positions.  The current implementation
                          expects that this function will be
                          deterministic.
        
        Keyword arguments:

        runSeconds -- How many seconds to run for.  This is in
                      addition to the time added for interpBegin and
                      interpEnd, if any.  Default: 10

        resetFirst -- Begin each run by resetting the robot to its
                      base position, currently implemented as a
                      transition from CURRENT -> POS_FLAT ->
                      POS_READY.  Default: True

        interpBegin -- Number of seconds over which to interpolate
                      from current position to commanded positions.
                      If this is not None, the robot will spend the
                      first interpBegin seconds interpolating from its
                      current position to that specified by
                      motionFunction.  This should probably be used
                      for motion models which do not return POS_READY
                      at time 0.  Affected by timeScale. Default: None

        interpEnd -- Same as interpBegin, but at the end of motion.
                      If interpEnd is not None, interpolation is
                      performed from final commanded position to
                      POS_READY, over the given number of
                      seconds. Affected by timeScale.  Default: None
                      
        timeScale -- Factor by which time should be scaled during this
                      run, higher is slower. Default: 1
                      
        logFile -- File to log time/positions to, should already be
                      opened. Default: None

        extraLogInfoFn -- Function to call and append info to every
                      line the log file. Should return a
                      string. Default: None
        '''

        #net, actuators = initialize()

        #def run(self, motionFunction, runSeconds = 10, resetFirst = True
        #    interpBegin = 0, interpEnd = 0):

        if self.loud:
            print 'Starting motion.'

        failures = self.pingAll()
        if failures:
            self.initServos()
            failures = self.pingAll()
            if failures:
                raise RobotFailure('Could not communicate with servos %s at beginning of run.' % repr(failures))

        self.resetClock()
        self.currentPos = self.readCurrentPosition()

        if logFile:
            #print >>logFile, '# time, servo goal positions (9), servo actual positions (9), robot location (x, y, age)'
            print >>logFile, '# time, servo goal positions (9), robot location (x, y, age)'

        # Reset the robot position, if desired
        if resetFirst:
            self.interpMove(self.readCurrentPosition(), POS_FLAT, 3)
            print "POS_FLAT: " + str(POS_FLAT)
            print "POS_READY: " + str(POS_READY)
            self.interpMove(POS_FLAT, POS_READY, 3)
            #self.interpMove(POS_READY, POS_HALFSTAND, 4)
            self.currentPos = POS_READY
            self.resetClock()

        # Begin with a segment smoothly interpolated between the
        # current position and the motion model.
        if interpBegin is not None:
            self.interpMove(self.currentPos,
                            scaleTime(motionFunction, timeScale),
                            interpBegin * timeScale,
                            logFile, extraLogInfoFn)
            self.currentPos = motionFunction(self.time)

        # Main motion segment
        self.interpMove(scaleTime(motionFunction, timeScale),
                        scaleTime(motionFunction, timeScale),
                        runSeconds * timeScale,
                        logFile, extraLogInfoFn)
        self.currentPos = motionFunction(self.time)

        # End with a segment smoothly interpolated between the
        # motion model and a ready position.
        if interpEnd is not None:
            self.interpMove(scaleTime(motionFunction, timeScale),
                            POS_READY,
                            interpEnd * timeScale,
                            logFile, extraLogInfoFn)

        failures = self.pingAll()
        if failures:
            # give it a second chance
            sleep(1)
            failures = self.pingAll()
            if failures:
                raise RobotFailure('Servos %s may have died during run.' % repr(failures))

        
    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

        
#
#        currentPos = None
#
#        if resetFirst:
#            currentPos = self.currentPostion()
#            time0 = datetime.datetime.now()
#            seconds = 0
#            
#            while seconds < 10:
#                timeDiff = datetime.datetime.now() - time0
#                seconds  = timeDiff.seconds + timeDiff.microseconds/1e6
#
#                if seconds < 3:
#                    goal = lInterp(seconds, [0, 3], startingPos, POS_FLAT)
#                elif seconds < 6:
#                    goal = lInterp(seconds, [3, 6], POS_FLAT, POS_READY)
#                elif seconds < 10:
#                    goal = lInterp(seconds, [6, 10], POS_READY, POS_HALFSTAND)
#                else:
#                    break
#
#                self.commandPosition(goal)
#                sleep(self.sleep)
#                
#            currentPos = POS_HALFSTAND
#
#        if interpBegin is not None:
#            if currentPos is None:
#                currentPos = self.currentPostion()
#
#            time0 = datetime.datetime.now()
#            seconds = 0
#            
#            while seconds < interpBegin:
#                timeDiff = datetime.datetime.now() - time0
#                seconds  = timeDiff.seconds + timeDiff.microseconds/1e6
#
#                goal = lInterp(seconds, [0, interpBegin], currentPos, motionFunction(seconds))
#
#                self.commandPosition(goal)
#                time.sleep(self.sleep)
#
    def resetClock(self):
        '''Resets the robot time to zero'''
        self.time0 = datetime.now()
        self.time  = 0.0

    def updateClock(self):
        '''Updates the Robots clock to the current time'''
        timeDiff  = datetime.now() - self.time0
        self.time = timeDiff.seconds + timeDiff.microseconds/1e6

    def readyPosition(self, persist = False):
        if persist:
            self.resetClock()
            while self.time < 2.0:
                self.commandPosition(POS_READY)
                sleep(.1)
                self.updateClock()
        else:
            self.commandPosition(POS_READY)
            sleep(2)

    def commandPosition(self, position, crop = True, cropWarning = False):
        '''Command the given position

        commandPosition will command the robot to move its servos to
        the given position vector.  This vector is cropped to
        the physical limits of the robot and converted to integer

        Positional arguments:
        position -- A length 9 vector of desired positions.

        Keyword arguments:
        cropWarning -- Whether or not to print a warning if the
                       positions are cropped.  Default: False.
        '''

        if len(position) != self.nServos:
            raise Exception('Expected postion vector of length %d, got %s instead'
                            % (self.nServos, repr(position)))

        if crop:
            goalPosition = self.cropPosition([int(xx) for xx in position], cropWarning)
        else:
            goalPosition = [int(xx) for xx in position]

        if self.loud:
            posstr = ', '.join(['%4d' % xx for xx in goalPosition])
            print '%.2fs -> %s' % (self.time, posstr)
        
        for ii,actuator in enumerate(self.actuators):
            actuator.goal_position = goalPosition[ii]
        self.net.synchronize()
        
        #[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]) + '  ' ,#XXX:THIS IS THE SLOW PART!!!
        #print ' '.join(['%.1f' % ac.current_voltage for ac in self.actuators])#XXX:OR THIS

        return goalPosition
    

    def cropPosition(self, position, cropWarning = False):
        '''Crops the given positions to their appropriate min/max values.
        
        Requires a vector of length 9 to be sure the IDs are in the
        assumed order.'''

        if len(position) != self.nServos:
            raise Exception('cropPosition expects a vector of length %d' % self.nServos)

        ret = copy(position)
        for ii in [0, 2, 4, 6]:
            ret[ii]   = max(MIN_INNER, min(MAX_INNER, ret[ii]))
            ret[ii+1] = max(MIN_OUTER, min(MAX_OUTER, ret[ii+1]))

        if cropWarning and ret != position:
            print 'Warning: cropped %s to %s' % (repr(position), repr(ret))
            
        return ret

    def readCurrentPosition(self):
        ret = []
        if len(self.actuators) != self.nServos:
            raise RobotFailure('Lost some servos, now we only have %d' % len(self.actuators))
        for ac in self.actuators:
            #ac.read_all()
            #ret.append(ac.cache[dynamixel.defs.REGISTER['CurrentPosition']])
            ret.append(ac.current_position)
            #sleep(.001)
        return ret

    def pingAll(self):
        failures = []
        for ii in self.actuatorIds:
            result = self.net.ping(ii)
            if result is False:
                failures.append(ii)
        return failures

    def printStatus(self):
        pos = self.readCurrentPosition()
        print 'Positions:', ' '.join(['%d:%d' % (ii,pp) for ii,pp in enumerate(pos)])


    def shimmy(self):
        '''Moves through a set of checks and makes sure the robot is
        still moving.'''

        self.commandPosition(POS_READY)
        sleep(.8)

        success = True
        success &= self.checkMove(POS_READY,   POS_CHECK_1)
        success &= self.checkMove(POS_CHECK_1, POS_CHECK_2)
        success &= self.checkMove(POS_CHECK_2, POS_CHECK_3)
        success &= self.checkMove(POS_CHECK_3, POS_CHECK_2)
        success &= self.checkMove(POS_CHECK_2, POS_CHECK_1)
        success &= self.checkMove(POS_CHECK_1, POS_READY)

        return success

    def checkMove(self, aa, bb):
        aa = array(aa)
        bb = array(bb)
        self.commandPosition(aa)

        posAA = array(self.readCurrentPosition())

        self.commandPosition(bb)

        sleep(.4)
        posBB = array(self.readCurrentPosition())

        success = True
        success &= all( abs((aa-bb) - (posAA-posBB)) < 50)
        success &= all( abs(aa - posAA) < 50)
        success &= all( abs(bb - posBB) < 50)

        if not success and False:
            print 'shimmy errors'
            print (aa-bb) - (posAA-posBB)
            print aa - posAA
            print bb - posBB
        return success