class MyGLViewer(GLWidgetPlugin): def __init__(self, world): GLWidgetPlugin.__init__(self) self.world = world self.robot = world.robot(0) self.subrobots = [] for i in range(6): self.subrobots.append( SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6)))) (res, val) = editors.run( editors.SelectionEditor("subrobot", self.subrobots[0]._links, description="sub robot links", world=world, robot=self.robot)) if res: print("Return value", val) self.subrobots[0]._links = val self.startConfig = self.robot.getConfig() self.robotWidget = RobotPoser(world.robot(0)) self.robotWidget.setActiveDofs(self.subrobots[0]._links) self.addWidget(self.robotWidget) 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)
class MyGLViewer(GLWidgetPlugin): def __init__(self, world): GLWidgetPlugin.__init__(self) self.world = world self.robot = world.robot(0) self.subrobots = [] for i in range(6): self.subrobots.append( SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6)))) (res, val) = editors.run( editors.SelectionEditor("subrobot", self.subrobots[0]._links, description="sub robot links", world=world, robot=self.robot)) if res: print("Return value", val) self.subrobots[0]._links = val self.startConfig = self.robot.getConfig() self.robotWidget = RobotPoser(world.robot(0)) self.robotWidget.setActiveDofs(self.subrobots[0]._links) self.addWidget(self.robotWidget) 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)