def __init__(self, controller = None, command_Queue = None, physical=False):
global myHelper
try:
self.file = open('data.json', 'rw')
self.pathDictionary = json.load(self.file)
self.file.close()
except:
self.pathDictionary = dict()
self.leftPath = []
self.rightPath = []
self.leftSaved = False
self.rightSaved = False
self.currentLimb = 'both'
self.rate = -1
self.degreeChange = 1
self.command_Queue = None
simworld = None
world = None
self.controller = None
self.command_Queue = command_Queue
self.visualizer = None
self.controller = None
self.simType = 'virtual'
if physical:
self.simType = 'physical'
if controller != None:
#assuming I'm being given a picking controller
self.controller = controller
simworld = self.controller.simworld
self.simworld = simworld
world = self.controller.world
sim = robotsim.Simulator(simworld)
#assuming visualizer was already created
else:
self.setupController(physical)
self.fake_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
def setupController(self, physical=False):
simworld = self.makeWorld()
planworld = self.makeWorld()
visualizer = None
sim = None
if not physical:
#virtual simulation
sim = robotsim.Simulator(simworld)
low_level_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
self.controller = PickingController(simworld, planworld, low_level_controller)
myHelper = Helper(self)
visualizer = CustomGLViewer.CustomGLViewer(simworld,planworld, low_level_controller, sim, helper = myHelper)
self.command_Queue = visualizer.command_queue
else:
#actually connected to the robot
motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./")
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.1)
q = motion.robot.getKlamptSensedPosition()
simworld.robot(0).setConfig(q)
planworld.robot(0).setConfig(q)
low_level_controller = PhysicalLowLevelController(simworld.robot(0))
self.controller = PickingController(simworld, planworld, low_level_controller)
myHelper = Helper(self)
visualizer = CustomGLViewer.CustomGLViewer(simworld,planworld, low_level_controller, helper=myHelper)
self.command_Queue = visualizer.command_queue
sim = robotsim.Simulator(simworld)
self.simworld = simworld
self.fake_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
self.visualizer = visualizer
visualizer.run()
class Recorder:
def __init__(self, controller = None, command_Queue = None, physical=False):
global myHelper
try:
self.file = open('data.json', 'rw')
self.pathDictionary = json.load(self.file)
self.file.close()
except:
self.pathDictionary = dict()
self.leftPath = []
self.rightPath = []
self.leftSaved = False
self.rightSaved = False
self.currentLimb = 'both'
self.rate = -1
self.degreeChange = 1
self.command_Queue = None
simworld = None
world = None
self.controller = None
self.command_Queue = command_Queue
self.visualizer = None
self.controller = None
self.simType = 'virtual'
if physical:
self.simType = 'physical'
if controller != None:
#assuming I'm being given a picking controller
self.controller = controller
simworld = self.controller.simworld
self.simworld = simworld
world = self.controller.world
sim = robotsim.Simulator(simworld)
#assuming visualizer was already created
else:
self.setupController(physical)
self.fake_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
def setupController(self, physical=False):
simworld = self.makeWorld()
planworld = self.makeWorld()
visualizer = None
sim = None
if not physical:
#virtual simulation
sim = robotsim.Simulator(simworld)
low_level_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
self.controller = PickingController(simworld, planworld, low_level_controller)
myHelper = Helper(self)
visualizer = CustomGLViewer.CustomGLViewer(simworld,planworld, low_level_controller, sim, helper = myHelper)
self.command_Queue = visualizer.command_queue
else:
#actually connected to the robot
motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./")
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.1)
q = motion.robot.getKlamptSensedPosition()
simworld.robot(0).setConfig(q)
planworld.robot(0).setConfig(q)
low_level_controller = PhysicalLowLevelController(simworld.robot(0))
self.controller = PickingController(simworld, planworld, low_level_controller)
myHelper = Helper(self)
visualizer = CustomGLViewer.CustomGLViewer(simworld,planworld, low_level_controller, helper=myHelper)
self.command_Queue = visualizer.command_queue
sim = robotsim.Simulator(simworld)
self.simworld = simworld
self.fake_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
self.visualizer = visualizer
visualizer.run()
def run(self):
print 'Beginning recorder functionalities.'
while(1):
method = self.command_Queue.get().lower()
print '\nCommand is ', method, '\n=================================='
if method is not None:
if method == 'h':
#self.printHelp()
pass
elif method == 's':
self.savePath()
elif method == 't' or method == 'run':
self.testPath()
elif method == 'a':
self.appendMilestone()
elif method == 'u':
self.changePath()
elif method == 'v':
self.changeLimb()
elif method == 'c':
self.commandRobot()
elif method == 'l':
self.loadPath()
elif method == 'f':
self.changeHz()
elif method == 'r':
self.reversePath()
elif method == 'x':
self.clearPaths()
elif method == 'p':
self.printPaths()
elif method == 'm':
self.reset()
elif method == 'w':
print 'Choose camera index'
index = self.getNumber()
self.calibrateCamera(index=index)
elif method == 'b':
print 'Choose box index'
index = self.getNumber()
self.calibrateBox(index=index)
elif method == 'g':
self.runPhysicalPath()
elif method == 'q':
if not self.checkSave(): continue
print 'Ending recorder functionalities'
break
else:
print 'Error did not understand command: ', method
print '\n===================================='
self.printHelp()
return
def checkSave(self):
if self.currentLimb == 'both':
if self.leftSaved == False and self.rightSaved == False:
return self.checkSavedHelper('both')
elif self.leftSaved == False:
return self.checkSavedHelper('left')
elif self.rightSaved == False:
return self.checkSavedHelper('right')
elif self.currentLimb == 'right':
return self.checkSavedHelper('right')
elif self.currentLimb == 'left':
return self.checkSavedHelper('left')
else:
return False
def checkSavedHelper(self, str_input=None):
options = ['y', 'n']
if str_input == None: return False
print 'You are about to overwrite ' + str_input + ' paths is that ok?'
answer = self.getName(options = options)
if answer == 'y':
self.updateSaved()
return True
else:
return False
def updateSaved(self):
if self.currentLimb == 'both':
self.leftSaved = False
self.rightSaved = False
elif self.currentLimb == 'right':
self.rightSaved = False
elif self.currentLimb == 'left':
self.leftSaved = False
def makeWorld(self):
"""Produces a world with only the Baxter, table, partitions, and ground plane in it."""
world = robotsim.WorldModel()
print "Loading simplified Baxter model..."
world.loadElement(os.path.join(model_dir,KLAMPT_MODEL))
#print "Loading shelf model..."
#world.loadElement(os.path.join(model_dir,"Amazon_Picking_Shelf.STL"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
leftWall = world.loadElement(os.path.join(model_dir, 'plane1.env'))
rightWall = world.loadElement(os.path.join(model_dir, 'plane2.env'))
table = world.loadElement(os.path.join(model_dir, 'plane3.env'))
print world.numTerrains(), ' is the number of terrains loaded'
#shift the Baxter up a bit (95cm)
Rbase,tbase = world.robot(0).link(0).getParentTransform()
world.robot(0).link(0).setParentTransform(Rbase,(0,0,0.95))
world.robot(0).setConfig(world.robot(0).getConfig())
factor = .1
wallfactor = .25
wall1Transform = ([wallfactor,0,0,0,0,wallfactor,0,wallfactor,0],[1.8,.7,.8])
wall2Transform = ([wallfactor,0,0,0,0,wallfactor,0,wallfactor,0],[1.8,-.7,.8])
tableTransform = ([factor, 0, 0, 0, factor, 0, 0, 0, factor],[1, 0, .75])
global TERRAIN_CALIBRATION
TERRAIN_CALIBRATION = [0]*world.numTerrains()
TERRAIN_CALIBRATION[0] = ([1,0,0,0,1,0,0,0,1],[0,0,0])
TERRAIN_CALIBRATION[1] = wall1Transform
TERRAIN_CALIBRATION[2] = wall2Transform
TERRAIN_CALIBRATION[3] = tableTransform
#world.terrain(1).geometry().transform(*wall1Transform)
#world.terrain(2).geometry().transform(*wall2Transform)
world.terrain(3).geometry().transform(*tableTransform)
#initialize the shelf xform for the visualizer and object
#ground_truth_shelf_xform = se3.mul(Trel,reorient)
return world
def savePath(self):
name = self.getName()
entry = None
if self.currentLimb == 'left':
entry = [self.currentLimb, ['left', self.leftPath], ['right', []] ]
elif self.currentLimb == 'right':
entry = [self.currentLimb, ['left', []], ['right', self.rightPath ] ]
else:
entry = [self.currentLimb, ['left', self.leftPath], ['right', self.rightPath] ]
#json.dumps({name: entry}, self.file)
self.pathDictionary[name] = entry
with open('data.json', 'w') as outfile:
json.dump(self.pathDictionary, outfile, sort_keys = True, indent=4)
# with open('data.json', 'w') as file:
# json.dump([key for key in self.pathDictionary : self.pathDictionary[key] for key in self.pathDictionary], file, indent=4)
self.leftSaved = True
self.rightSaved = True
print 'Saved'
def testPath(self, reverse = False):
global LEFT_ARM_INDICES
global RIGHT_ARM_INDICES
currentSetup = self.controller.controller.getSensedConfig()
startConfig = [v for v in currentSetup]
if self.leftPath != []:
for i in range(len(LEFT_ARM_INDICES)):
startConfig[LEFT_ARM_INDICES[i]] = self.leftPath[0][i]
if self.rightPath != []:
for i in range(len(RIGHT_ARM_INDICES)):
startConfig[RIGHT_ARM_INDICES[i]] = self.rightPath[0][i]
#self.fake_controller.sim.getWorld().robot(0).setConfig(startConfig)
#self.fake_controller.setVelocity([1]*61,0.1)
self.fake_controller.setLinear(startConfig, .01)
if self.currentLimb == 'left':
for milestone in self.leftPath:
self.fake_controller.appendMilestoneLeft(milestone, dt=.2)
elif self.currentLimb == 'right':
for milestone in self.rightPath:
self.fake_controller.appendMilestoneRight(milestone, dt=.2)
elif self.currentLimb == 'both':
for milestone in self.rightPath:
self.fake_controller.appendMilestoneRight(milestone, dt =.2)
for milestone in self.leftPath:
self.fake_controller.appendMilestoneLeft(milestone, dt=.2)
self.visualizer.addController(self.fake_controller, [0,0,1,.5])
# currentSetup = self.controller.controller.getSensedConfig()
# startConfig = [v for v in currentSetup]
# if self.leftPath != []:
# for i in range(len(LEFT_ARM_INDICES)):
# startConfig[LEFT_ARM_INDICES[i]] = self.leftPath[0][i]
# if self.rightPath != []:
# for i in range(len(RIGHT_ARM_INDICES)):
# startConfig[RIGHT_ARM_INDICES[i]] = self.rightPath[0][i]
# self.fake_controller.setLinear(startConfig, .01)
# totalLen = max(len(self.leftPath), len(self.rightPath))
# if totalLen == 0: totalLen = 1
# while len(self.leftPath) < totalLen:
# if self.leftPath == []:
# self.leftPath.append([startConfig[v] for v in LEFT_ARM_INDICES])
# self.leftPath.append(self.leftPath[-1])
# while len(self.rightPath) < totalLen:
# if self.rightPath == []:
# self.rightPath.append([startConfig[v] for v in RIGHT_ARM_INDICES])
# self.rightPath.append(self.rightPath[-1])
# for i in range(totalLen-1):
# config = [v for v in startConfig]
# for j in range(len(LEFT_ARM_INDICES)):
# config[LEFT_ARM_INDICES[j]] = self.leftPath[i][j]
# for j in range(len(RIGHT_ARM_INDICES)):
# config[RIGHT_ARM_INDICES[j]] = self.rightPath[i][j]
# self.fake_controller.controller.addCubic(config, 1)
print 'Test Concluded'
return
def runPhysicalPath(self):
if self.simType == 'physical':
for milestone in self.leftPath:
self.controller.controller.appendMilestoneLeft(milestone)
for milestone in self.rightPath:
self.controller.controller.appendMilestoneRight(milestone)
def loadPath(self):
if not self.checkSave(): return
print 'Options are:'
for key in self.pathDictionary:
print key
load_name = self.getName(options = [key for key in self.pathDictionary])
if load_name in self.pathDictionary:
self.leftPath = self.pathDictionary[load_name][1][1]
self.rightPath = self.pathDictionary[load_name][2][1]
print 'Loaded'
self.currentLimb = self.pathDictionary[load_name][0]
self.saveLeft = True
self.saveRight = True
else:
print 'Sorry, name not recognized.'
return
def changePath(self, method=None):
if self.rate > 0:
self.recordPath()
else:
print ''
print 'Replace (p), Insert (i), Remove (x), or Append (a)?'
method = self.command_Queue.get().lower()
if method == 'p':
self.replaceMilestone()
elif method == 'i':
self.insertMilestone()
elif method == 'x':
self.removeMilestone()
elif method == 'a':
self.appendMilestone()
else:
print 'Unknown command. Returning to main loop'
return
def changeLimb(self):
#TODO check saved
print ("What type of limb would you like to record?")
print ("Options: Left (l), Right (r), Both (b), Cancel (x) ")
limbResponse = self.getName().lower()
leftResponses = ['l', 'left']
rightResponses = ['r', 'right']
bothResponses = ['b', 'both']
cancelResponses = ['x', 'cancel']
if limbResponse in cancelResponses:
print 'Cancelling'
return
elif limbResponse in leftResponses:
print 'Changing to left limb'
if not self.checkSave():
print 'Canceled'
return
self.currentLimb = LEFT
elif limbResponse in rightResponses:
print 'Changing to right limb'
if not self.checkSave():
print 'Canceled'
return
self.currentLimb = RIGHT
elif limbResponse in bothResponses:
print 'Changing to both limbs'
if not self.checkSave():
print 'Canceled'
return
self.currentLimb = BOTH
def commandRobot(self):
#make it so various numbers allow you to change how the robot behaves
if self.currentLimb == BOTH:
print 'Error, won\'t adjust both arms at once'
return
jointCommands = ['1','!','2','@','3','#','4','$','5','%','6','^','7','&']
print 'Current incremental change is ', self.degreeChange, ' degrees'
while(1):
movement = self.command_Queue.get().lower()
if movement is not None:
if movement in jointCommands:
self.moveRobot(movement)
elif movement == 'h':
self.printRobotHelp()
elif movement == 'c':
self.changeDegrees()
elif movement == 'q':
print 'Ending command of robot'
break
else:
print 'Error did not understand command: ', movement
self.printRobotHelp()
def moveRobot(self, movement=None):
print 'Moving robot'
global LEFT_ARM_INDICES
global RIGHT_ARM_INDICES
joint_ones = ['1', '!']
joint_twos = ['2', '@']
joint_threes = ['3', '#']
joint_fours = ['4', '$']
joint_fives = ['5', '%']
joint_sixes = ['6', '^']
joint_sevens = ['7', '&']
plus = ['1','2','3','4','5','6','7']
minus = ['!','@','#','$','%','^','&']
index = 0
factor = 0
adjustment = [0,0,0,0,0,0,0]
if movement in joint_ones:
index = 0
elif movement in joint_twos:
index = 1
elif movement in joint_threes:
index = 2
elif movement in joint_fours:
index = 3
elif movement in joint_fives:
index = 4
elif movement in joint_sixes:
index = 5
elif movement in joint_sevens:
index = 6
if movement in plus:
factor = 1
elif movement in minus:
factor = -1
adjustment[index] = factor*self.degreeChange*math.pi/180.0
currentSetup = self.controller.controller.getCommandedConfig()
if self.currentLimb == LEFT:
leftMilestone = [currentSetup[v] for v in LEFT_ARM_INDICES]
leftMilestone = vectorops.add(leftMilestone, adjustment)
adjustedMilestone = [currentSetup[i] for i in range(len(currentSetup))]
for i in range(len(LEFT_ARM_INDICES)):
adjustedMilestone[LEFT_ARM_INDICES[i]] = leftMilestone[i]
self.controller.controller.setLinear(adjustedMilestone, .1)
elif self.currentLimb == RIGHT:
rightMilestone = [currentSetup[v] for v in RIGHT_ARM_INDICES]
rightMilestone = vectorops.add(rightMilestone, adjustment)
adjustedMilestone = [currentSetup[i] for i in range(len(currentSetup))]
for i in range(len(RIGHT_ARM_INDICES)):
adjustedMilestone[RIGHT_ARM_INDICES[i]] = rightMilestone[i]
self.controller.controller.setLinear(adjustedMilestone, .1)
return
def changeDegrees(self):
entry = raw_input('Enter a number in degrees')
self.degreeChange = float(entry)
def changeHz(self):
rate = raw_input("Enter a rate ")
self.rate = float(rate)
return
def recordPath(self):
#TODO - test and make sure it works fine
fn = PATH_DIR + FILE_NAME
if self.simType != 'physical':
print 'Error, physical simulation is not set up'
return
if self.currentLimb != 'both':
print 'Warning, record path was initially meant to record both arms at the same time - is your system set up for only recording',
print self.currentLimb + ' arm? (y/n)'
response = raw_input()
if response == 'y':
pass
else:
print 'Not y, returning'
return
f = open(fn, 'w')
try:
print "Beginning capture loop, press Ctrl+C to quit"
t0 = time.time()
while True:
t = time.time() - t0
if t < 1e-3: t = 0
if switch == 'sensed':
if self.curentLimb == 'both':
q = motion.robot.left_limb.sensedPosition()+motion.robot.right_limb.sensedPosition()
elif self.currentLimb == 'right':
q = motion.robot.right_limb.sensedPosition()
elif self.currentLimb == 'left':
q = motion.robot.left_limb.sensedPosition()
self.updateSaved()
else:
if self.curentLimb == 'both':
q = motion.robot.left_limb.commandedPosition()+motion.robot.right_limb.commandedPosition()
elif self.currentLimb == 'right':
q = motion.robot.right_limb.commandedPosition()
elif self.currentLimb == 'left':
q = motion.robot.left_limb.commandedPosition()
self.updateSaved()
if q != lastq:
if lastq is not None:
f.write(str(lastt) + '\t' + str(len(lastq)) + '\t'+ ' '.join(str(v) for v in lastq)+'\n')
f.write(str(t) + '\t' + str(len(q)) + '\t'+ ' '.join(str(v) for v in q)+'\n')
lastt = t
lastq = q
if rate <= 0:
raw_input("Press enter to capture > ")
else:
time.sleep(1.0/self.rate)
except KeyboardInterrupt:
f.close()
print "Saved %g seconds of motion. Exiting."%(time.time()-t0,)
pass
def insertMilestone(self):
global LEFT_ARM_INDICES
global RIGHT_ARM_INDICES
print 'Inserting'
print 'Enter an index'
config = self.controller.controller.getSensedConfig()
leftArm = [config[v] for v in LEFT_ARM_INDICES]
rightArm = [config[v] for v in RIGHT_ARM_INDICES]
index = self.getNumber()
if index == 'q':
return
if self.currentLimb == 'left':
if len(self.leftPath) > index:
self.leftPath.insert(index, [a for a in leftArm])
self.updateSaved()
else:
print 'Error, index outside of list bounds'
elif self.currentLimb == 'right':
if len(self.rightPath) > index:
self.rightPath.insert(index, [a for a in rightArm])
self.updateSaved()
else:
print 'Error, index outside of list bounds'
elif self.currentLimb == 'both':
if len(self.leftPath) > index:
self.leftPath.insert(index, [a for a in leftArm])
self.updateSaved()
else:
print 'Error, index outside of left list bounds'
if len(self.rightPath) > index:
self.rightPath.insert(index, [a for a in rightArm])
self.updateSaved()
else:
print 'Error, index outside of right list bounds'
return
def appendMilestone(self):
#updates saving
global LEFT_ARM_INDICES
global RIGHT_ARM_INDICES
print 'Appending'
config = self.controller.controller.getSensedConfig()
leftArm = [config[v] for v in LEFT_ARM_INDICES]
rightArm = [config[v] for v in RIGHT_ARM_INDICES]
if self.currentLimb == 'left':
self.leftPath.append([a for a in leftArm])
elif self.currentLimb == 'right':
self.rightPath.append([a for a in rightArm])
elif self.currentLimb == 'both':
self.leftPath.append([a for a in leftArm])
self.rightPath.append([a for a in rightArm])
self.updateSaved()
return
def removeMilestone(self):
#updates saving
print 'Removing'
print 'Enter an index'
index = self.getNumber()
if index == 'q':
return
if self.currentLimb == 'left':
if len(self.leftPath) > index:
self.leftPath.pop(index)
self.updateSaved()
else:
print 'Error, index outside of list bounds'
elif self.currentLimb == 'right':
if len(self.rightPath) > index:
self.rightPath.pop(index)
self.updateSaved()
else:
print 'Error, index outside of list bounds'
elif self.currentLimb == 'both':
if len(self.leftPath) > index:
self.leftPath.pop(index)
self.updateSaved()
else:
print 'Error, index outside of left list bounds'
if len(self.rightPath) > index:
self.rightPath.pop(index)
self.updateSaved()
else:
print 'Error, index outside of right list bounds'
return
def replaceMilestone(self):
#updates saving
global LEFT_ARM_INDICES
global RIGHT_ARM_INDICES
print 'Replacing'
print 'Enter an index'
#could take out the leftarm/rightarm step
config = self.controller.controller.getSensedConfig()
leftArm = [config[v] for v in LEFT_ARM_INDICES]
rightArm = [config[v] for v in RIGHT_ARM_INDICES]
index = self.getNumber()
if index == 'q':
return
if self.currentLimb == 'left':
if len(self.leftPath) > index:
self.leftPath[index] = [a for a in leftArm]
self.updateSaved()
else:
print 'Error, index outside of list bounds'
elif self.currentLimb == 'right':
if len(self.rightPath) > index:
self.rightPath[index] = [a for a in rightArm]
self.updateSaved()
else:
print 'Error, index outside of list bounds'
elif self.curentLimb == 'both':
if len(self.leftPath) > index:
self.leftPath[index] = [a for a in leftArm]
self.updateSaved()
else:
print 'Error, index outside of left list bounds'
if len(self.rightPath) > index:
self.rightPath[index] = [a for a in rightArm]
self.updateSaved()
else:
print 'Error, index outside of right list bounds'
def reset(self):
self.currentLimb = 'both'
self.clearPaths()
load_name = 'Q_DEFAULT_LOAD'
milestone_ldefault = self.pathDictionary[load_name][1][1]
milestone_rdefault = self.pathDictionary[load_name][2][1]
print 'left default milestone ' + milestone_ldefault
print 'right default milestone ' + milestone_rdefault
self.controller.controller.appendMilestoneLeft(milestone_ldefault[0], 1)
self.controller.controller.appendMilestoneRight(milestone_rdefault[0], 1)
time.sleep(.1)
self.leftSaved = True
self.rightSaved = True
print 'Done resetting'
def resetMotionQueue(self):
if self.simType == 'physical':
motion.shutdown()
setupController(physical=True)
else:
print 'Error, sim type is not physical'
return
def getNumber(self):
number = ''
numVals = ['1','2','3','4','5','6','7','8','9','0']
while(1):
digit = self.command_Queue.get().lower()
if digit == chr(13):
try:
intNum = int(number)
return intNum
except:
print 'Error, number cannot be evaluated'
return 'q'
elif digit is not None:
if digit in numVals:
number += digit
os.system('clear')
print number
elif digit == '\x08' or digit == '\x7f':
# backspace or delete
number = number[:-1]
os.system('clear')
print number
else:
print 'Error, value not recognized'
print 'Press Enter to submit your entry or quit'
def getName(self, options=None):
name = ''
while(1):
letter = self.command_Queue.get()
if letter == chr(13):
return name
elif letter == chr(9):
#tab
myOptions = []
for one_option in options:
if one_option.startswith(name):
myOptions.append(one_option)
if myOptions == []:
pass
else:
beg = len(name)
end = len(myOptions[0])
escape = False
for i in xrange(beg, end+1):
if not escape:
testString = myOptions[0][:i]
for option in myOptions:
if option.startswith(testString):
pass
else:
name = myOptions[0][:i-1]
escape = True
break
elif letter is not None:
if letter in string.printable:
name += letter
elif letter == '\x08' or letter == '\x7f':
# backspace or delete
name = name[:-1]
else:
print 'Error, value not recognized'
print 'Press Enter to submit your entry or quit'
os.system('clear')
if options is not None:
print 'Options are:'
for one_option in options:
if name in one_option:
print one_option
print '\n' + name
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):
# interpolate path linearly between two endpoints
if path == None:
print "sending empty path"
return False
for smoothIter in range(maxSmoothIters):
# path = path
smoothePath = [0]*(len(path)*2-1)
for i in range(len(path)-1):
smoothePath[i*2] = path[i]
smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
smoothePath[-1] = path[-1]
path = smoothePath
while i <endIndex-1:
# print i, endIndex
q = path[i]
qNext = path[i+1]
dt = vectorops.distance(q,qNext)
# smooth trajectory by interpolating between two consecutive configurations
# if the distance between the two is big
# Note: might want to make dt bigger for arm movements (only 7 configurations vs 52)
if dt>0.1:
qInterp = vectorops.div(vectorops.add(q, qNext), 2)
path.insert(i+1, qInterp)
endIndex +=1
continue
else:
i += 1
waitForMove()
if counter%internalSpeed == 0 or INCREMENTAL:
if limb == 'left':
#CONTROLLER.appendMilestoneLeft(q)
pass
elif limb == 'right':
#CONTROLLER.appendMilestoneRight(q)
pass
else:
#print 'milestone #', i, q
#CONTROLLER.appendMilestone(q)
pass
counter +=1
if limb == 'left':
#CONTROLLER.appendMilestoneLeft(path[-1])
pass
elif limb == 'right':
#CONTROLLER.appendMilestoneRight(path[-1])
pass
else:
pass
#print 'last milestone', path[-1]
#CONTROLLER.appendMilestone(path[-1])
# print 'Done with moving'
def clearPaths(self):
if not self.checkSave(): return
self.leftPath = []
self.rightPath = []
return
def printPaths(self):
print 'Left path is: ', self.leftPath
print 'Right path is: ', self.rightPath
def reversePath(self):
if self.currentLimb == 'left':
self.leftPath = [v for v in self.leftPath[::-1]]
elif self.currentLimb == 'right':
self.rightPath = [v for v in self.rightPath[::-1]]
elif self.curentLimb == 'both':
self.leftPath = [v for v in self.leftPath[::-1]]
self.rightPath = [v for v in self.rightPath[::-1]]
def calibrateBox(self, index=0):
global BOX_COORDS
while(True):
try:
print 'Options: x1, x2, y1, y2, z1, z2'
input_var = raw_input("Wire Box: Enter coordinate and distance to change separated by a comma: ").split(',');
#translational transformation
local_coords = BOX_COORDS[index]
if(input_var[0] == "x1" ):
local_coords[0][0] = local_coords[0][0] + float(input_var[1])
elif(input_var[0] == "y1" ):
local_coords[0][1] = local_coords[0][1] + float(input_var[1])
elif(input_var[0] == "z1" ):
local_coords[0][2] = local_coords[0][2] + float(input_var[1])
#rotational transformations
elif(input_var[0] == "x2" ):
local_coords[1][0] = local_coords[1][0] + float(input_var[1])
elif(input_var[0] == "y2" ):
local_coords[1][1] = local_coords[1][1] + float(input_var[1])
elif(input_var[0] == "z2" ):
local_coords[1][2] = local_coords[1][2] + float(input_var[1])
elif(input_var[0] == "q"):
break
BOX_COORDS[index] = local_coords
except:
print "input error\n"
#print error.strerror
time.sleep(0.1);
if self.visualizer != None:
self.takePicture(index)
self.visualizer.updateBox(index, BOX_COORDS[index])
print 'local box ', index, ' coords are ', BOX_COORDS[index]
def calibrateCamera(self, index=0):
global CAMERA_TRANSFORM
global REAL_CAMERA
if REAL_CAMERA:
if len(CAMERA_TRANSFORM) < index:
return False
while(True):
try:
input_var = raw_input("Camera: Enter joint and angle to change to separated by a comma: ").split(',');
#translational transformation
calibrateR = CAMERA_TRANSFORM[index][0]
calibrateT = CAMERA_TRANSFORM[index][1]
if(input_var[0] == "x" ):
calibrateT[0] = calibrateT[0] + float(input_var[1])
elif(input_var[0] == "y" ):
calibrateT[1] = calibrateT[1] + float(input_var[1])
elif(input_var[0] == "z" ):
calibrateT[2] = calibrateT[2] + float(input_var[1])
#rotational transformations
elif(input_var[0] == "xr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([1, 0, 0], float(input_var[1])))
elif(input_var[0] == "yr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([0, 1, 0], float(input_var[1])))
elif(input_var[0] == "zr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([0, 0, 1], float(input_var[1])))
elif(input_var[0] == "q"):
break
except:
print "input error\n"
#print error.strerror
time.sleep(0.1);
CAMERA_TRANSFORM[index] = (calibrateR, calibrateT)
self.takePicture(index)
print 'local camera ', index, ' transform is ', CAMERA_TRANSFORM[index]
def takePicture(self, index=0):
global perceiver
current_cloud = perceiver.get_current_point_cloud(*self.getCameraToWorldXform(index=index), tolist=True, index=index)
if self.visualizer != None:
self.visualizer.updatePoints(current_cloud)
else:
raise NotImplemented
#self.controller.
def getCameraToWorldXform(self, linkName=None, index=0):
'''
get current transformation (R, t) of camera in world frame.
'''
global CAMERA_TRANSFORM
if len(CAMERA_TRANSFORM) < index:
print 'Error, camera index does not exist'
return ([0,0,0,0,0,0,0,0,0],[0,0,0])
if linkName != None:
return se3.mul(self.simworld.robot(0).link(linkName).getTransform(), CAMERA_TRANSFORM[index])
else:
return CAMERA_TRANSFORM[index]
def printHelp(self):
print 'H: Help (show this text)'
print 'Q: Quit (return to standard controller)'
print 'S: Save current path'
print 'T: Test/Simulate current path'
print 'A: Append current milestone to end of path'
print 'U: Update path manually (i.e. insert/replace milestones)'
print 'L: Load path from file'
print 'V: Switch limb to record'
print 'F: Change recording frequency'
print 'R: Reverse current stored path'
print 'X: Clear/Reset paths'
print 'P: Print current paths'
print 'M: Reset to default config'
print 'C: Command robot to move'
print 'W: Calibrate Camera'
print 'B: Adjust wire boxes'
return
def printRobotHelp(self):
print 'H: Help (show this text)'
print '1/!: Move joint 1'
print '2/@: Move joint 2'
print '3/#: Move joint 3'
print '4/$: Move joint 4'
print '5/%: Move joint 5'
print '6/^: Move joint 6'
print '7/&: Move joint 7'
print 'C: Change the amount by which a joint rotates (degress)'
print 'Q: Quit (return to main loop)'
