sim_actuator.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Description:  Simulate ART brake driver actuator
00004 #
00005 #   Copyright (C) 2009 Austin Robot Technology
00006 #
00007 #   License: Modified BSD Software License Agreement
00008 #
00009 # $Id: sim_actuator.py 2 2010-01-17 01:54:03Z jack.oquin $
00010 #
00011 
00012 import sys
00013 import getopt
00014 import os
00015 
00016 import math
00017 import numpy
00018 
00019 #import matplotlib.pyplot as pyplot
00020 #import pylab
00021 
00022 import plot_brake
00023 
00024 # global actuator constants
00025 ticksPerInch = 24000                    # encoder ticks per inch of travel
00026 aMax = 8 * ticksPerInch                 # max acceleration (ticks/s^2)
00027 vMax = 4 * ticksPerInch                 # max velocity (ticks/s)
00028 
00029 # driver cycle interval
00030 cycleInterval = 1.0/20.0                # 20Hz cycle interval
00031 
00032 epsilon = 0.000001                      # small value, nearly zero
00033 
00034 def getAccel(direction):
00035     "Get acceleration constant from direction of movement (-, 0, +)."
00036     if direction == 0.0:
00037         a = 0.0
00038     elif direction > 0.0:
00039         a = aMax
00040     else:
00041         a = -aMax
00042     return a
00043 
00044 class Range:
00045     "Sensor range conversion"
00046 
00047     def __init__(self, off, full):
00048         "Constructor."
00049         self.off = off                  # value with brake off
00050         self.full = full                # value with brake fully on
00051         self.range = full - off         # corresponding range
00052 
00053     def __getitem__(self, pos):
00054         "Range indexing operation."
00055         return (pos * self.range) + self.off
00056 
00057     def pos(self, key):
00058         "Get corresponding position."
00059         return (key - self.off) / self.range
00060 
00061 class Actuator:
00062     "Simulated ART brake actuator."
00063 
00064     def __init__(self, verbose=False):
00065         "Constructor."
00066         self.verbose = verbose
00067 
00068         # initialize sensor ranges
00069         self.encRg = Range(0.0, 50000.0)
00070         self.potRg = Range(4.9, 0.49)
00071         self.prRg = Range(0.85, 4.5)
00072 
00073         self.reset()
00074 
00075     def __str__(self):
00076         "Convert actuator state to printable string."
00077         return ("actuator state: [t: %.3f" % self.t
00078                 + ", x: " + str(self.x)
00079                 + ", v: " + str(self.v)
00080                 + ", a: " + str(self.a) + "]\n"
00081                 + "                [pos: " + str(self.pos)
00082                 + ", pot: " + str(self.pot)
00083                 + ", pr: " + str(self.pr) + "]")
00084 
00085     def move(self, dt):
00086         """Move actuator for dt seconds at constant acceleration.
00087         """
00088         if dt <= epsilon:
00089             raise RuntimeError('invalid move interval: ' + str(dt))
00090         v0 = self.v                     # previous velocity
00091         self.v += dt * self.a           # velocity change
00092         self.x += dt * (self.v+v0)/2.0  # advance by average velocity
00093         self.t += dt                    # advance simulated time
00094         self.update()                   # update sensor values
00095         if self.verbose and abs(self.v) > vMax:
00096             print 'actuator velocity too high:', self.v, '(time: %.3f)' % self.t
00097         #if abs(self.v) > vMax:
00098         #    raise RuntimeError('actuator velocity too high: ' + str(self.v))
00099 
00100     def reset(self):
00101         "Reset actuator state."
00102         self.t = 0                      # time (in seconds)
00103         self.x = 0                      # position (in ticks)
00104         self.v = 0                      # velocity (ticks/s)
00105         self.a = 0                      # acceleration (ticks/s^2)
00106         self.update()                   # update sensor values
00107 
00108     def setAccel(self, direction):
00109         "Set new acceleration constant from direction of movement (-, 0, +)."
00110         self.a = getAccel(direction)
00111 
00112     def update(self):
00113         "Update sensor values."
00114         self.pos = self.encRg.pos(self.x) # position [0.0, 1.0]
00115         self.pot = self.potRg[self.pos]   # potentiometer voltage
00116         self.pr = self.prRg[self.pos]     # pressure sensor voltage
00117 
00118 # make an actuator instance
00119 a = Actuator()
00120 
00121 class Step:
00122     "One step in an actuator movement plan."
00123 
00124     def __init__(self, t=0.0, a=0.0):
00125         "Constructor."
00126         self.t = t                      # end of step time
00127         self.a = getAccel(a)            # constant acceleration
00128 
00129     def __str__(self):
00130         "Convert plan step to printable string."
00131         return ("step: [t: %.3f" % self.t + ", a: " + str(self.a) + "]")
00132 
00133 class Plan:
00134     "An actuator movement plan."
00135 
00136     def __init__(self):
00137         "Constructor."
00138         self.reset()
00139 
00140     def __getitem__(self, i):
00141         "Get i-th Step of Plan."
00142         return self.steps[i]
00143 
00144     def __iter__(self):
00145         "Plan iterator visits each Step."
00146         return self.steps.__iter__()
00147 
00148     def __str__(self):
00149         "Convert next step of plan to printable string (empty if none)."
00150         retval = ""
00151         if len(self.steps) > 0:
00152             retval = 'plan ' + str(self.steps[0])
00153         return retval
00154 
00155     def advance(self, now):
00156         "Advance to next Plan Step, if it is now time."
00157         for s in self.steps:
00158             if s.t > now:
00159                 return
00160             self.steps = self.steps[1:] # remove the first Step
00161 
00162     def append(self, step):
00163         "Add a Step to the Plan."
00164         self.steps.append(step)
00165 
00166     def getAccel(self):
00167         "Get current acceleration value."
00168         if len(self.steps) > 0:
00169             return self.steps[0].a
00170         else:
00171             return 0.0
00172 
00173     def interval(self, now, finish):
00174         """Return interval (in seconds) until end of:
00175                 (1) current plan step,
00176                 (2) current 20Hz driver cycle, or
00177                 (3) finish time.
00178         """
00179         dt = finish - now               # (3) interval to finish
00180         if len(self.steps) > 0:
00181             dt = self.steps[0].t - now  # (1) interval in step
00182 
00183         # (2) interval remaining in current driver cycle
00184         dcycle = math.fmod(now, cycleInterval)
00185         if dcycle <= epsilon:           # now is start of a cycle
00186             dcycle = cycleInterval
00187         if dcycle < dt:
00188             dt = dcycle
00189         if dt <= epsilon:
00190             raise RuntimeError('invalid time interval: ' + str(dt))
00191         return dt
00192 
00193     def reset(self):
00194         "Reset the plan."
00195         self.steps = []
00196 
00197 class Test:
00198     "Actuator simulation test."
00199 
00200     def __init__(self, verbose=False):
00201         "Constructor."
00202         self.verbose = verbose
00203         self.dev = Actuator(verbose)
00204         self.plan = Plan()
00205         if verbose:
00206             print self.dev
00207         self.plt = plot_brake.brakeData('sim-actuator')
00208         self.plt.set_state(self.dev.t, self.dev.pos, self.dev.pot,
00209                            self.dev.x, self.dev.pr)
00210 
00211     def check(self, value, expected, label=''):
00212         "Check test result against expected value."
00213         if (value - expected) > 0.00001:
00214             format = 'test error (sim time %.3f) %s %.6f (expecting %.6f)'
00215             print format % (self.dev.t, label, value, expected)
00216             print self.dev
00217             print self.plan
00218 
00219     def cmd(self, dx):
00220         """Command actuator to do relative move.
00221 
00222         @param dx = encoder ticks to move.
00223         @returns time (in seconds) required to get there.
00224         
00225         This calculation assumes a triangular velocity profile with
00226         constant acceleration |a|, applied first in one direction,
00227         then in the other.
00228 
00229         Erase any previous plan, then move the actuator in two steps:
00230 
00231           (1) accelerate in direction requested;
00232           (2) decelerate to zero velocity
00233         
00234         The real device supports a trapezoidal velocity profile with a
00235         maximum velocity (vMax).  Since it takes a long time to reach
00236         vMax, this model ignores that detail, which does not seem to
00237         matter much in practice.
00238         """
00239         t0 = self.dev.t                 # starting time
00240         x0 = self.dev.x                 # current encoder position
00241         v0 = self.dev.v                 # current actuator velocity
00242         if abs(dx) >= 0.5:              # nonzero move?
00243             a = getAccel(dx)            # accelerate in new direction
00244         elif abs(v0) > epsilon:         # current velocity nonzero?
00245             a = getAccel(-v0)           # cancel current velocity
00246         else:                           # holding in place
00247             self.plan.reset()           # delete any existing plan
00248             return 0.0
00249 
00250         dt_h = -v0 / a                  # time to halt current movement
00251         dx_h = dx - 0.5 * v0 * dt_h     # dx remaining after halt
00252         dt2 = math.sqrt(dx_h/a)         # deceleration time
00253         dt1 = dt2 + dt_h                # time to move towards goal
00254         dt = dt1 + dt2                  # total move duration
00255 
00256         # make a new plan
00257         self.plan.reset()
00258         if dt1 > epsilon:
00259             self.plan.append(Step(t0+dt1, a))
00260         if dt2 > epsilon:
00261             self.plan.append(Step(t0+dt, -a))
00262 
00263         if self.verbose:
00264             print str(self.dev)
00265             print str(self.plan)
00266             print 'cmd(%.f): %.3f' % (dx, t0), dt1, dt2, dt
00267         if dt < 0.0:
00268             raise RuntimeError('invalid move duration: ' + str(dt))
00269 
00270         # record command info for later plotting
00271         self.plt.set_cmd(t0, self.dev.encRg.pos(x0+dx))
00272         return dt
00273 
00274     def plot(self, save=False):
00275         "Plot some test results."
00276         self.plt.plot_position(save)
00277         #self.plt.plot_encoder(save)
00278 
00279     def run(self, duration):
00280         "Run the test for some duration."
00281         t = self.dev.t                  # start time
00282         finish = t + duration           # finish time
00283         if self.verbose:
00284             print 'run time: %.3f, duration %.3f' % (t, duration)
00285         while t < finish:
00286             self.plan.advance(t)
00287             if self.verbose:
00288                 print 'run time: %.3f;' % t, str(self.plan)
00289             self.dev.setAccel(self.plan.getAccel())
00290             self.dev.move(self.plan.interval(t, finish))
00291             t = self.dev.t
00292             self.plt.set_state(t, self.dev.pos, self.dev.pot,
00293                                self.dev.x, self.dev.pr)
00294     
00295 
00296 def usage(progname):
00297     "Print usage message."
00298     print "\n", progname, """[-h] [ <commands.txt> ]
00299 
00300  -h, --help         print this message
00301  -i, --interactive  display plots to terminal (default: save as files)
00302  -n, --no-plot      do not plot results
00303  -s, --save         save plot image files
00304  -v, --verbose      print verbose debug messages
00305 
00306  <commands.txt>\tname of command file to simulate
00307 
00308 If a command file is specified, read brake commands from it, saving
00309 plots of the data.  Otherwise, read brake commands interactively.
00310  """
00311 
00312 # main program -- for either script or interactive use
00313 def main(argv=None):
00314     "Main program, called as a script or interactively."
00315 
00316     if argv is None:
00317         argv = sys.argv                 # use command args
00318 
00319     # extract base name of command, will be '' when imported
00320     progname = os.path.basename(argv[0])
00321     if progname is "":
00322         progname = "sim_actuator.py"
00323 
00324     # process parameters
00325     try:
00326         opts, files = getopt.gnu_getopt(argv[1:], 'hinsv',
00327                                         ('help', 'interactive', 'no-plot'
00328                                          'save', 'verbose'))
00329     except getopt.error, msg:
00330         print msg
00331         print "for help use --help"
00332         return 9
00333 
00334     save = False
00335     plot = True
00336     verbose = False
00337 
00338     for k,v in opts:
00339         if k in ("-h", "--help"):
00340             usage(progname)
00341             return 2
00342         if k in ("-i", "--interactive"):
00343             save = False
00344         if k in ("-n", "--no-plot"):
00345             plot = False
00346         if k in ("-s", "--save"):
00347             save = True
00348         if k in ("-v", "--verbose"):
00349             verbose = True
00350 
00351     test = Test(verbose)
00352 
00353     if len(files) < 1:
00354         print "running pre-defined test sequence"
00355         test.run(0.1)
00356 
00357         # apply full brake
00358         test.run(test.cmd(50000.0))
00359         test.check(test.dev.x, 50000.0, 'encoder')
00360         test.check(test.dev.v, 0.0, 'velocity')
00361 
00362         # request brake off
00363         test.run(test.cmd(-50000.0))
00364         test.check(test.dev.x, 0.0, 'encoder')
00365         test.check(test.dev.v, 0.0, 'velocity')
00366         test.run(0.2)
00367 
00368         # apply full brake, run until one quarter finished
00369         dt = test.cmd(50000.0)
00370         test.run(dt/4.0)
00371 
00372         # stop the brake and hold it at the current position
00373         test.run(test.cmd(0.0))
00374         test.run(0.2)
00375         
00376         # release the brake slightly, then back to full
00377         test.run(test.cmd(-5000.0))
00378         test.run(0.2)
00379         test.run(test.cmd(50000.0 - test.dev.x))
00380         test.run(0.2)
00381         test.check(test.dev.x, 50000.0, 'encoder')
00382         test.check(test.dev.v, 0.0, 'velocity')
00383 
00384         # request brake off, stop part way and hold, then release fully
00385         dt = test.cmd(-50000.0)
00386         test.run(0.3 * dt)
00387         test.run(test.cmd(0.0))
00388         test.run(0.2)
00389         test.run(test.cmd(0.0 - test.dev.x))
00390         test.check(test.dev.x, 0.0, 'encoder')
00391         test.check(test.dev.v, 0.0, 'velocity')
00392 
00393         test.run(test.cmd(25000.0))
00394         test.check(test.dev.x, 25000.0, 'encoder')
00395         test.check(test.dev.v, 0.0, 'velocity')
00396 
00397         # request brake off
00398         test.run(test.cmd(-25000.0))
00399         test.check(test.dev.x, 0.0, 'encoder')
00400         test.check(test.dev.v, 0.0, 'velocity')
00401 
00402         test.run(test.cmd(0.0))
00403         test.check(test.dev.x, 0.0, 'encoder')
00404         test.check(test.dev.v, 0.0, 'velocity')
00405 
00406         dt = test.cmd(4320.0)
00407         test.check(dt, 0.3, 'dt')
00408         test.run(dt)
00409         test.check(test.dev.v, 0.0, 'velocity')
00410 
00411         test.run(0.199)
00412         test.check(test.dev.v, 0.0, 'velocity')
00413 
00414         dt = test.cmd(-4320.0)
00415         test.check(dt, 0.3, 'dt')
00416         test.run(dt)
00417         test.check(test.dev.v, 0.0, 'velocity')
00418 
00419         test.run(0.199)
00420         test.check(test.dev.v, 0.0, 'velocity')
00421 
00422         if plot:
00423             test.plot(save)
00424 
00425     elif len(files) == 1:
00426         print "Simulating " + files[0]
00427         #b.get_bag(files[0])
00428         #b.plot(save)
00429 
00430     else:
00431         print "only one command file can be processed at a time"
00432         usage(progname)
00433         return 9
00434 
00435     return 0
00436 
00437 # when called as a script or via python-send-buffer
00438 if __name__ == "__main__":
00439 
00440     # run main function and exit
00441     rc = 0
00442     try:
00443         rc = main()
00444     except KeyboardInterrupt:
00445         pass
00446         
00447     sys.exit(rc)


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12