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()
Example #2
0
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)
Example #5
0
 def callback(coordinates=coordinates):
     coordinates.updateFromWorld()
Example #6
0
        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