def testCartesianDrive(): w = WorldModel() #w.readFile("../../data/tx90scenario0.xml") w.readFile("../../data/robots/jaco.rob") r = w.robot(0) solver = CartesianDriveSolver(r) #set a non-singular configuration q = r.getConfig() q[3] = 0.5 r.setConfig(q) solver.start(q, 6) vis.add("world", w) vis.addPlot("timing") vis.addPlot("info") vis.show() time.sleep(0.1) dt = 0.01 t = 0 while t < 20 and vis.shown(): vis.lock() if t < 2: v = [0, 0, 0.25] elif t < 3: v = [0, 0, -0.1] elif t < 3.2: v = [0, 0, -1] elif t < 8: v = [0, 0, 0] elif t < 10: v = [-1, 0, 0] else: v = [1, 0, 0] if t < 4: w = [0, 0, 0] elif t < 10: w = [0, -0.25, 0] else: w = None t0 = time.time() progress, qnext = solver.drive(q, w, v, dt) t1 = time.time() vis.addText("debug", "Vel %s" % (str(v), )) vis.logPlot("timing", "t", t1 - t0) vis.logPlot("info", "progress", progress) vis.logPlot("info", "adj", solver.driveSpeedAdjustment) r.setConfig(qnext) q = qnext vis.unlock() vis.add("tgt", solver.driveTransforms[0]) t += dt time.sleep(max(0.005 - (t1 - t0), 0)) vis.show(False) vis.clear()
vis.setColor("grid "+str(i),0,1,0,0.5) vis.hideLabel("grid "+str(i)) #vis.add("pc "+str(i),g.pc) #vis.setColor("pc "+str(i),1,1,0,0.5) for i,obs in enumerate(obstaclegeoms): vis.add("envpc "+str(i),obs.pc) vis.setColor("envpc "+str(i),1,1,0,0.5) vis.hideLabel("envpc "+str(i)) settings = None settings = SemiInfiniteOptimizationSettings() #if you use qinit = random-collision-free, you'll want to set this higher settings.max_iters = 5 settings.minimum_constraint_value = -0.02 vis.addPlot("timing") vis.show() oldcps = [] while vis.shown(): vis.lock() t0 = time.time() for path in movableObjects: q = vis.getItemConfig(path) T = (q[:9],q[9:]) for c,p in zip(constraints,pairs): if p[1].getName() == path[1]: c.env.setTransform(T) q0 = robot.getConfig() robot.setConfig(qinit) #qcollfree,trace,cps = geometryopt.optimizeCollFreeRobot(robot,obstacles,constraints=constraints,qinit='random-collision-free',qdes=q0,verbose=VERBOSE,settings=settings,
doedit=True) if len(configs) < 2: print "Didn't add 2 or more milestones, quitting" exit(-1) resource.set("pathtest.configs", configs) traj0 = trajectory.RobotTrajectory(robot, times=range(len(configs)), milestones=configs) #show the path in the visualizer, repeating for 60 seconds traj = trajectory.path_to_trajectory(traj0, speed=1.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "Default values") vis.addPlot("plot") vis.addPlotItem("plot", "robot") #action callbacks def setHalfSpeed(): traj = trajectory.path_to_trajectory(traj0, speed=0.5) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "0.5x speed") def setDoubleSpeed(): traj = trajectory.path_to_trajectory(traj0, speed=2.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj)
def modification_template(world): """Tests a variety of miscellaneous vis functions""" vis.add("world",world) robot = world.robot(0) vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque import random for i in range(10): #set some links to random colors randlink = random.randint(0,robot.numLinks()-1) color = (random.random(),random.random(),random.random()) vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color) #test the on-screen text display vis.addText("text2","Here's some red text") vis.setColor("text2",1,0,0) vis.addText("text3","Here's bigger text") vis.setAttribute("text3","size",24) vis.addText("text4","Transform status") vis.addText("textbottom","Text anchored to bottom of screen",(20,-30)) #test a point pt = [2,5,1] vis.add("some point",pt) #test a rigid transform vis.add("some blinking transform",[so3.identity(),[1,3,0.5]]) vis.edit("some point") #vis.edit("some blinking transform") #vis.edit("coordinates:ATHLETE:ankle roll 3") #test an IKObjective link = world.robot(0).link(world.robot(0).numLinks()-1) #point constraint obj = ik.objective(link,local=[[0,0,0]],world=[pt]) #hinge constraint #obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]]) #transform constraint #obj = ik.objective(link,R=link.getTransform()[0],t=pt) vis.add("ik objective",obj) #enable plotting vis.addPlot('plot') vis.addPlotItem('plot','some point') vis.setPlotDuration('plot',10.0) #run the visualizer, which runs in a separate thread vis.setWindowTitle("Manual animation visualization test") class MyCallback: def __init__(self): self.iteration = 0 def __call__(self): vis.lock() #TODO: you may modify the world here. This line tests a sin wave. pt[2] = 1 + math.sin(self.iteration*0.03) vis.unlock() #changes to the visualization with vis.X functions can done outside the lock if (self.iteration % 100) == 0: if (self.iteration / 100)%2 == 0: vis.hide("some blinking transform") vis.addText("text4","The transform was hidden") vis.logPlotEvent('plot','hide') else: vis.hide("some blinking transform",False) vis.addText("text4","The transform was shown") vis.logPlotEvent('plot','show') #this is another way of changing the point's data without needing a lock/unlock #vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True) #or #vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)]) if self.iteration == 200: vis.addText("text2","Going to hide the text for a second...") if self.iteration == 400: #use this to remove text vis.clearText() if self.iteration == 500: vis.addText("text2","Text added back again") vis.setColor("text2",1,0,0) self.iteration += 1 callback = MyCallback() if not MULTITHREADED: vis.loop(callback=callback,setup=vis.show) else: vis.show() while vis.shown(): callback() time.sleep(0.01) #use this to remove a plot vis.remove("plot") vis.kill()
r.randomizeConfig() elif config is not None: if config.endswith('.config'): q = io.load(config, 'Config') else: q = io.read('Config', config) r.setConfig(q) vis.add("robot", r) for i in range(1, w.numRobots()): vis.add(w.robot(i).getName(), w.robot(i)) for i in range(0, w.numRigidObjects()): vis.add(w.rigidObject(i).getName(), w.rigidObject(i)) for i in range(0, w.numTerrains()): vis.add(w.terrain(i).getName(), w.terrain(i)) vis.addPlot("joints") vis.addPlotItem("joints", "robot") vis.show() d0 = [r.driver(i).getValue() for i in range(r.numDrivers())] qmin, qmax = r.getJointLimits() index = 0 period = 5 period_start = time.time() while vis.shown(): t = time.time() u = (t - period_start) / period driver = r.driver(index) if driver.getType() == 'affine': dmin = float('inf')
#transform constraint #obj = ik.objective(link,R=link.getTransform()[0],t=pt) vis.add("ik objective", obj) vis.edit("some point") #vis.edit("some blinking transform") #vis.edit("coordinates:ATHLETE:ankle roll 3") #test the on-screen text display vis.addText("text2", "Here's some red text") vis.setColor("text2", 1, 0, 0) vis.addText("text3", "Here's bigger text") vis.setAttribute("text3", "size", 24) vis.addText("text4", "Transform status") vis.addText("textbottom", "Text anchored to bottom of screen", (20, -30)) vis.addPlot('plot') vis.addPlotItem('plot', 'some point') vis.setPlotDuration('plot', 10.0) print "Visualization items:" vis.listItems(indent=2) vis.autoFitCamera() print "Starting visualization window..." #run the visualizer, which runs in a separate thread vis.setWindowTitle("Basic visualization test") vis.show() iteration = 0 while vis.shown(): vis.lock()