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)