def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example prints the config when [space] is pressed if c == ' ': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) print("Config:", subconfig) self.subrobots[0].setConfig(subconfig) elif c == 'r': self.subrobots[0].randomizeConfig() self.robotWidget.set(self.robot.getConfig()) elif c == 'p': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) self.subrobots[0].setConfig(self.subrobots[0].fromfull( self.startConfig)) settings = { 'type': "sbl", 'perturbationRadius': 0.5, 'bidirectional': 1, 'shortcut': 1, 'restart': 1, 'restartTermCond': "{foundSolution:1,maxIters:1000}" } plan = robotplanning.planToConfig(self.world, self.subrobots[0], subconfig, movingSubset='all', **settings) plan.planMore(1000) print(plan.getPath()) else: GLWidgetPlugin.keyboardfunc(self, c, x, y)
def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example prints the config when [space] is pressed if c == 'h' or c == '?': print("Controls:") print("- [space]: print the widget's sub-robot configuration") print("- r: randomize the sub-robot configuration") print("- p: plan to the widget's sub-robot configuration") print("- i: test the IK functions") elif c == ' ': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) print("Config:", subconfig) self.subrobots[0].setConfig(subconfig) elif c == 'r': self.subrobots[0].randomizeConfig() self.robotWidget.set(self.robot.getConfig()) elif c == 'p': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) self.subrobots[0].setConfig(self.subrobots[0].fromfull( self.startConfig)) settings = { 'type': "sbl", 'perturbationRadius': 0.5, 'bidirectional': 1, 'shortcut': 1, 'restart': 1, 'restartTermCond': "{foundSolution:1,maxIters:1000}" } plan = robotplanning.planToConfig(self.world, self.subrobots[0], subconfig, movingSubset='all', **settings) plan.planMore(1000) print(plan.getPath()) elif c == 'i': link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1) print("IK solve for ee to go 10cm upward...") obj = ik.objective(link, local=[0, 0, 0], world=vectorops.add( link.getWorldPosition([0, 0, 0]), [0, 0, 0.1])) solver = ik.solver(obj) res = solver.solve() print(" result", res) print(" residual", solver.getResidual()) print(self.robotWidget.set(self.robot.getConfig())) else: GLWidgetPlugin.keyboardfunc(self, c, x, y)
def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example prints the config when [space] is pressed if c == ' ': config = self.robotWidget.get() print "Config:", config self.world.robot(0).setConfig(config) elif c == 's': fn = "widgets_test_world.xml" print "Saving file to", fn self.world.saveFile(fn) else: GLWidgetPlugin.keyboardfunc(self, c, x, y)