def rotate(self,radians):
robot = self.world.robot(0)
object = self.world.rigidObject(0)
robot.setConfig(vis.getItemConfig("robot"))
T = vis.getItemConfig("rigidObject")
object.setTransform(T[:9],T[9:])
self.angle += radians
T = (so3.rotation((0,1,0),radians),[0.0]*3)
Tbase = moving_base.get_moving_base_xform(robot)
moving_base.set_moving_base_xform(robot,*se3.mul(T,Tbase))
Tobj = object.getTransform()
object.setTransform(*se3.mul(T,Tobj))
#update visualization
vis.setItemConfig("robot",robot.getConfig())
vis.setItemConfig("rigidObject",sum(object.getTransform(),[]))
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
coordinates.updateFromWorld()
self.refresh()
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def load_config(self,name):
object = self.world.rigidObject(0)
self.config_name = name
print "Loading from",name+".config,",name+".xform","and",name+".array"
qrob = resource.get(name+".config",type="Config",doedit=False)
Tobj = resource.get(name+".xform",type="RigidTransform",doedit=False)
self.contact_units = resource.get(name+"_units.array",type="IntArray",default=[],doedit=False)
if len(self.contact_units) == 0:
print "COULDNT LOAD "+name+"_units.array, trying "+name+".array"
contact_links = resource.get(name+".array",type="IntArray",default=[],doedit=False)
if len(contact_links) > 0:
robot = self.world.robot(0)
contact_links = [robot.link(l).getName() for l in contact_links]
self.contact_units = []
for i,(link,unit) in enumerate(self.hand.all_units):
if link in contact_links:
self.contact_units.append(i)
print "UNITS",self.contact_units
object.setTransform(*Tobj)
qobj = vis.getItemConfig("rigidObject")
vis.setItemConfig("robot",qrob)
vis.setItemConfig("rigidObject",qobj)
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
coordinates.updateFromWorld()
for e in endeffectors:
f = coordinates.frame(robot.link(e).getName())
fw = coordinates.addFrame(robot.link(e).getName()+"_tgt",robot.link(e).getTransform())
assert f != None
vis.add("ee_"+robot.link(e).getName(),fw)
vis.edit("ee_"+robot.link(e).getName())
obj = coordinates.ik_fixed_objective(f)
eeobjectives.append(obj)
#this tests the cartesian interpolation stuff
print "***** BEGINNING CARTESIAN INTERPOLATION TEST *****"
vis.setWindowTitle("Klamp't Cartesian interpolation test")
vis.pushPlugin(InterpKeyCapture(endeffectors,eeobjectives))
vis.show()
while vis.shown():
coordinates.updateFromWorld()
time.sleep(0.1)
vis.popPlugin()
vis.hide("ghost1")
vis.hide("ghost2")
vis.animate(("world",world.robot(0).getName()),None)
print
print
#this tests the "bump" function stuff
print "***** BEGINNING BUMP FUNCTION TEST *****"
configs = resource.get("cartesian_test"+world.robot(0).getName()+".configs",description="Make a reference trajectory for bump test",world=world)
if configs is None:
print "To test the bump function, you need to create a reference trajectory"
vis.kill()
exit(0)
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
time.sleep(0.01)
"""
print "Doing a dialog..."
vis.setWindowTitle("Dialog test")
print "calling dialog()"
vis.dialog()
print "Doing a split screen program..."
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.show()
while vis.shown():
time.sleep(0.1)
print "Showing a dialog, back to normal..."
vis.setPlugin(None)
vis.dialog()
print "Showing again, back to normal..."
vis.setWindowTitle("Basic visualization test")
vis.show()
while vis.shown():
time.sleep(0.01)
def keyboardfunc(self,c,x,y):
if c == 'h':
print "Help: "
print "- s: save grasp configuration to disk"
print "- l: load grasp configuration from disk"
print "- <: rotate left"
print "- >: rotate right"
print "- [space]: solve for stability"
print "- f: solve for the feasible force volume"
print "- w: solve for the feasible wrench space"
print "- f: solve for the robust force volume"
print "- m: save the current volume to a mesh"
print "- M: load the current volume from a mesh"
print "- 6: calculates the 6D wrench space"
print "- 7: loads the 6D wrench space"
print "- 0: zeros the robot's base transform"
print "- 9: hides the widgets"
if c == 's':
name = raw_input("Enter a name for this configuration > ")
if name.strip() == '':
print "Not saving..."
else:
self.save_config(name)
self.config_name = name
elif c == 'l':
vis.animate("rigidObject",None)
name = raw_input("Enter a name for the configuration to load > ")
if name.strip() == '':
print "Not loading..."
else:
self.load_config(name)
elif c == ',' or c == '<':
vis.animate("rigidObject",None)
self.rotate(math.radians(1))
print "Now at angle",math.degrees(self.angle)
elif c == '.' or c == '>':
vis.animate("rigidObject",None)
self.rotate(-math.radians(1))
print "Now at angle",math.degrees(self.angle)
elif c == ' ':
self.solve(self.contact_units)
elif c == 'f':
if self.wrenchSpace6D != None:
cm = se3.apply(self.world.rigidObject(0).getTransform(),self.world.rigidObject(0).getMass().getCom())
self.wrenchSpace = self.wrenchSpace6D.getSlice(cm).convex_decomposition[0]
self.wrench_space_gl_object = None
else:
self.wrenchSpace = self.calculate_force_volume((0,0,0))
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'w':
self.wrenchSpace = self.calculate_3D_wrench_space()
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'r':
dropout = raw_input("How much to drop out? (default 0) > ")
if dropout.strip() == '':
dropout = 0
else:
dropout = float(dropout.strip())
perturb = raw_input("How much to perturb? (default 0.1) > ")
if perturb.strip() == '':
perturb = 0.1
else:
perturb = float(perturb.strip())
N = raw_input("How many samples? (default 100) > ")
if N.strip() == '':
N = 100
else:
N = int(N.strip())
self.wrenchSpace = self.calculate_robust_force_volume((0,0,0),dropout=dropout,configurationPerturbation=perturb,maxVolumes=N)
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'm':
assert self.wrenchSpace != None
from PyQt4.QtGui import QFileDialog
savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name
name = QFileDialog.getSaveFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)")
g = polytope.hull_to_klampt_geom(self.wrenchSpace)
g.saveFile(str(name))
elif c == 'M':
from PyQt4.QtGui import QFileDialog
savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name
name = QFileDialog.getOpenFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)")
g = Geometry3D()
g.loadFile(str(name))
self.wrenchSpace = polytope.klampt_geom_to_hull(g)
self.wrench_space_gl_object = None
#decomp = stability.approximate_convex_decomposition(g)
#print "Convex decomposition into",len(decomp),"pieces"
print len(polytope.pockets(g)),"pocket faces"
elif c == '6':
N = raw_input("How many samples? (default 100) > ")
if N.strip() == '':
N = 100
else:
N = int(N.strip())
self.wrenchSpace6D = self.calculate_6D_wrench_space(N)
self.wrenchSpace = None
self.wrench_space_gl_object = None
elif c == '7':
self.wrenchSpace6D = self.load_6D_wrench_space()
self.wrenchSpace = None
self.wrench_space_gl_object = None
elif c == '0':
robot = self.world.robot(0)
q = robot.getConfig()
q[0:6] = [0]*6
robot.setConfig(q)
vis.setItemConfig("robot",robot.getConfig())
coordinates.updateFromWorld()
elif c == '9':
vis.edit("robot",False)
vis.edit("rigidObject",False)
#vis.hide("COM")
#object = self.world.rigidObject(0)
#vis.edit("COM")
else:
return False
return True
def motionfunc(self,x,y,dx,dy):
res = GLPluginInterface.motionfunc(self,x,y,dx,dy)
coordinates.updateFromWorld()
object = self.world.rigidObject(0)
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
return res