def main(args):
# move to starting position
rl = RobotLibrary(args.robot, joystick=False)
rl.move(args.x, args.y, 0.0)
# configure
twister = Twister(args.robot, rci=rl._rci)
twister.dof = 'Rz' # most stable, due to each wheel contributing the same
twister.speed = args.vRz
twister.repeats = args.numRepeats
twister.duration = args.duration
twister.disableVisionLoc = True # the point of this procedure is to compare vision with encoders
twister.accelerationLimit = 2.0 # must prevent wheel slip
# run
twister.run()
class MotionRangeTwister():
def __init__(self, args):
# setup control and state reader
self.rl = RobotLibrary(args.robot, joystick=False)
# get ball and move to starting position
self.homePosition = (args.x, args.y, 0.0)
self.reset()
# other settings
self.dof = args.dof
self.v = None
self.accRange = None
self.setRange() # sensible defaults for v and accRange
# setup twister, reuse connected RCI
self.twister = Twister(args.robot, rci=self.rl._rci)
self.twister.dof = self.dof
self.twister.duration = 1.0
self.twister.sleep = 0.5
self.twister.repeats = args.numRepeats
self.twister.disableVisionLoc = True
def setRange(self):
# default acceleration range and speed setpoint
self.v = 0.5
step = 0.2
if (self.dof == 'Rz'):
self.v = 2.0
step = 0.5
self.accRange = [step * n for n in range(6,20)] # bh_verification
self.accRange = [step*2 * n for n in range(10,20)] # motion_verification
def reset(self):
self.rl.move(*self.homePosition)
def info(self, msg):
# write to RDL eventlog
os.system('frun diagnostics sendEvent INFO "' + msg + '"')
# write to stdout
print(msg, end='')
sys.stdout.flush() # TODO remove, can use print() argument flush=True when migrating to python3
def run(self):
# run
for a in self.accRange:
self.reset()
isRz = (self.dof == 'Rz')
self.info("testing {0} a={1:.2f}{2}2 at v={3:.2f}{2}...".format(self.dof, a, ['m/s', 'rad/s'][isRz], self.v))
self.twister.speed = self.v
self.twister.accelerationLimit = a
self.twister.run()
