def multiwindow_template(world): """Tests multiple windows and views.""" vis.add("world",world) vp = vis.getViewport() vp.w,vp.h = 800,800 vis.setViewport(vp) vis.setWindowTitle("vis.spin test: will close in 5 seconds...") vis.spin(5.0) #Now testing ability to re-launch windows vis.setWindowTitle("Shown again. Close me to proceed.") vis.spin(float('inf')) vis.setWindowTitle("Dialog test. Close me to proceed.") vp = vis.getViewport() vp.w,vp.h = 400,600 vis.setViewport(vp) vis.dialog() vp.w,vp.h = 640,480 vis.setViewport(vp) for i in range(3): widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) vis.addPlugin(widgets) vis.setWindowTitle("Split screen test") vis.spin(float('inf')) vis.setPlugin(None) vis.setWindowTitle("Back to normal. Close me to quit.") vis.dialog() vis.kill()
def launch_xform_viewer(robotname): """Launches a very simple program that spawns an object from one of the databases. It launches a visualization of the mvbb decomposition of the object, and corresponding generated poses. It then spawns a hand and tries all different poses to check for collision """ world = WorldModel() robot = make_moving_base_robot(robotname, world) set_moving_base_xform(robot, se3.identity()[0], se3.identity()[1]) xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform", default=se3.identity(), world=world, doedit=False) print "Transform:", xform program = XFormVisualizer(world, xform) vis.setPlugin(program) program.reshape(800, 600) vis.show() # this code manually updates the visualization while vis.shown(): time.sleep(0.01) vis.kill() return
def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None): """Launches a very simple program that spawns a box with dimensions specified in boxes_db. """ world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname, world) xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform", default=se3.identity(), world=world, doedit=False) for box_dims, poses in box_db.db.items(): if world.numRigidObjects() > 0: world.remove(world.rigidObject(0)) obj = make_box(world, box_dims[0], box_dims[1], box_dims[2]) poses_filtered = [] R,t = obj.getTransform() obj.setTransform(R, [0, 0, box_dims[2]/2.]) w_T_o = np.array(se3.homogeneous(obj.getTransform())) p_T_h = np.array(se3.homogeneous(xform)) p_T_h[2][3] += 0.02 for pose in poses: w_T_p = w_T_o.dot(pose) w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h)) if CollisionTestPose(world, robot, obj, w_T_h_des_se3): pose_pp = se3.from_homogeneous(pose) t_pp = pose_pp[1] q_pp = so3.quaternion(pose_pp[0]) q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]] print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims elif w_T_p[2][3] <= 0.: print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table" else: poses_filtered.append(pose) print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses" # embed() # create a hand emulator from the given robot name module = importlib.import_module('plugins.' + robotname) # emulator takes the robot index (0), start link index (6), and start driver index (6) program = FilteredMVBBTesterVisualizer(box_dims, poses_filtered, world, p_T_h, module, box_db, links_to_check) vis.setPlugin(None) vis.setPlugin(program) program.reshape(800, 600) vis.show() # this code manually updates the visualization while vis.shown(): time.sleep(0.1) return
def main(): print("============================================================") print(sys.argv[0] + ": Simulates a robot file and Python controller") if len(sys.argv) <= 1: print("USAGE: simtest.py [world_file] [controller files (.py)]") print("============================================================") if len(sys.argv) <= 1: exit() world = WorldModel() #load up any world items, control modules, or paths control_modules = [] for fn in sys.argv[1:]: path, base = os.path.split(fn) mod_name, file_ext = os.path.splitext(base) if file_ext == '.py' or file_ext == '.pyc': sys.path.append(os.path.abspath(path)) mod = importlib.import_module(mod_name, base) control_modules.append(mod) elif file_ext == '.path': control_modules.append(fn) else: res = world.readFile(fn) if not res: print("Unable to load model " + fn) print("Quitting...") sys.exit(1) viewer = MyGLViewer(world) for i, c in enumerate(control_modules): if isinstance(c, str): sys.path.append(os.path.abspath("../control")) import trajectory_controller #it's a path file, try to load it controller = trajectory_controller.make(world.robot(i), c) else: try: maker = c.make except AttributeError: print("Module", c.__name__, "must have a make() method") print("Quitting...") sys.exit(1) controller = maker(world.robot(i)) viewer.sim.setController(world.robot(i), controller) vis.setWindowTitle("Klamp't Simulation Tester") if SPLIT_SCREEN_TEST: viewer2 = MyGLViewer(world) vis.setPlugin(viewer) vis.addPlugin(viewer2) viewer2.window.broadcast = True vis.show() while vis.shown(): time.sleep(0.01) vis.kill() else: vis.run(viewer)
def multiwindow_template(world): """Tests multiple windows and views.""" vis.add("world", world) vp = vis.getViewport() vp.w, vp.h = 400, 600 vis.setViewport(vp) vis.addText("label1", "This is Window 1", (20, 20)) vis.setWindowTitle("Window 1") vis.show() id1 = vis.getWindow() print("First window ID:", id1) id2 = vis.createWindow("Window 2") vis.add("Lone point", [0, 0, 0]) vis.setViewport(vp) vis.addText("label1", "This is Window 2", (20, 20)) print("Second window ID:", vis.getWindow()) vis.setWindow(id2) vis.spin(float('inf')) #restore back to 1 window, clear the text vis.setWindow(id1) vis.clearText() vp = vis.getViewport() vp.w, vp.h = 800, 800 vis.setViewport(vp) vis.setWindowTitle("vis.spin test: will close in 5 seconds...") vis.spin(5.0) #Now testing ability to re-launch windows vis.setWindowTitle("Shown again. Close me to proceed.") vis.spin(float('inf')) vis.setWindowTitle("Dialog test. Close me to proceed.") vp = vis.getViewport() vp.w, vp.h = 400, 600 vis.setViewport(vp) vis.dialog() vp.w, vp.h = 640, 480 vis.setViewport(vp) for i in range(3): widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) vis.addPlugin(widgets) vis.setWindowTitle("Split screen test") vis.spin(float('inf')) vis.setPlugin(None) vis.setWindowTitle("Back to normal. Close me to quit.") vis.dialog() vis.kill()
def run_ex3(): world = WorldModel() res = world.readFile("ex3_file.xml") if not res: raise RuntimeError("Unable to load world file") vis.add("world", world) vis.setWindowTitle("Pick and place test, use a/b/c/d to select target") vis.pushPlugin(GLPickAndPlacePlugin(world)) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill()
def run_ex2(): world = WorldModel() res = world.readFile("ex2_file.xml") if not res: raise RuntimeError("Unable to load world file") vis.add("world", world) vis.pushPlugin(GLTransferPlanPlugin(world)) vis.setWindowTitle( "Transfer plan test, press r/f to plan with right/forward target") vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill()
def run_ex1(): world = WorldModel() res = world.readFile("ex1_file.xml") if not res: raise RuntimeError("Unable to load world file") vis.add("world", world) vis.setWindowTitle( "Transit plan test, press l/r to plan with left/right arm") vis.pushPlugin(GLTransitPlanPlugin(world)) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill()
def run(self): """Standardized interface for running program""" vp = vis.getViewport() #Square screen #vp.w,vp.h = 800,800 #For saving HD quality movies vp.w, vp.h = 1024, 768 vp.clippingplanes = self.clippingplanes vis.setViewport(vp) #vis.run(program) vis.setPlugin(self) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill()
def run(self): """Standardized interface for running program""" if KLAMPT_VERSION >= 0.7: vp = vis.getViewport() #Square screen #vp.w,vp.h = 800,800 #For saving HD quality movies vp.w, vp.h = 1024, 768 vp.clippingplanes = self.clippingplanes vis.setViewport(vp) #vis.run(program) vis.setPlugin(self) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill() else: #Square screen #self.width,self.height = 800,800 #For saving HD quality movies self.width, self.height = 1024, 768 GLBaseClass.run(self)
def launch_shelf(robotname, objects): """Launches the task 2 program that asks the robot to retrieve some set of objects packed within a shelf. """ world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname, world) box = make_box(world, *box_dims) shelf = make_shelf(world, *shelf_dims) shelf.geometry().translate((0, shelf_offset, shelf_height)) rigid_objects = [] for objectset, objectname in objects: object = make_object(objectset, objectname, world) #TODO: pack in the shelf using x-y translations and z rotations object.setTransform( *se3.mul((so3.identity(), [0, shelf_offset, shelf_height + 0.01]), object.getTransform())) rigid_objects.append(object) xy_jiggle(world, rigid_objects, [shelf], [-0.5 * shelf_dims[0], -0.5 * shelf_dims[1] + shelf_offset], [0.5 * shelf_dims[0], 0.5 * shelf_dims[1] + shelf_offset], 100) doedit = True xform = resource.get("shelf/default_initial_%s.xform" % (robotname, ), description="Initial hand transform", default=robot.link(5).getTransform(), world=world) if not xform: print "User quit the program" return set_moving_base_xform(robot, xform[0], xform[1]) #now the simulation is launched program = GLSimulationProgram(world) sim = program.sim #setup some simulation parameters visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection for l in range(robot.numLinks()): sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink) for l in range(world.numRigidObjects()): sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink) #create a hand emulator from the given robot name module = importlib.import_module('plugins.' + robotname) #emulator takes the robot index (0), start link index (6), and start driver index (6) hand = module.HandEmulator(sim, 0, 6, 6) sim.addEmulator(0, hand) #controlfunc is now attached to control the robot import shelf_controller sim.setController(robot, shelf_controller.make(sim, hand, program.dt)) #the next line latches the current configuration in the PID controller... sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity()) #this code uses the GLSimulationProgram structure, which gives a little more control over the visualization vis.setPlugin(program) program.reshape(800, 600) vis.show() while vis.shown(): time.sleep(0.1) return #this code manually updates the vis vis.add("world", world) vis.show() t0 = time.time() while vis.shown(): vis.lock() sim.simulate(0.01) sim.updateWorld() vis.unlock() t1 = time.time() time.sleep(max(0.01 - (t1 - t0), 0.001)) t0 = t1 return
if not res: raise RuntimeError("Unable to load model "+fn) viewer = MyGLViewer(world) for i,c in enumerate(control_modules): if isinstance(c,str): sys.path.append(os.path.abspath("../control")) import trajectory_controller #it's a path file, try to load it controller = trajectory_controller.make(world.robot(i),c) else: try: maker = c.make except AttributeError: print "Module",c.__name__,"must have a make() method" raise controller = maker(world.robot(i)) viewer.sim.setController(world.robot(i),controller) if SPLIT_SCREEN_TEST: viewer2 = MyGLViewer(world) vis.setPlugin(viewer) vis.addPlugin(viewer2) viewer2.window.broadcast = True vis.show() while vis.shown(): time.sleep(0.01) vis.kill() else: vis.run(viewer)
def launch_shelf(robotname,objects): """Launches the task 2 program that asks the robot to retrieve some set of objects packed within a shelf. """ world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname,world) box = make_box(world,*box_dims) shelf = make_shelf(world,*shelf_dims) shelf.geometry().translate((0,shelf_offset,shelf_height)) rigid_objects = [] for objectset,objectname in objects: object = make_object(objectset,objectname,world) #TODO: pack in the shelf using x-y translations and z rotations object.setTransform(*se3.mul((so3.identity(),[0,shelf_offset,shelf_height + 0.01]),object.getTransform())) rigid_objects.append(object) xy_jiggle(world,rigid_objects,[shelf],[-0.5*shelf_dims[0],-0.5*shelf_dims[1]+shelf_offset],[0.5*shelf_dims[0],0.5*shelf_dims[1]+shelf_offset],100) doedit = True xform = resource.get("shelf/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world) if not xform: print "User quit the program" return set_moving_base_xform(robot,xform[0],xform[1]) #now the simulation is launched program = GLSimulationProgram(world) sim = program.sim #setup some simulation parameters visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection for l in range(robot.numLinks()): sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink) for l in range(world.numRigidObjects()): sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink) #create a hand emulator from the given robot name module = importlib.import_module('plugins.'+robotname) #emulator takes the robot index (0), start link index (6), and start driver index (6) hand = module.HandEmulator(sim,0,6,6) sim.addEmulator(0,hand) #controlfunc is now attached to control the robot import shelf_controller sim.setController(robot,shelf_controller.make(sim,hand,program.dt)) #the next line latches the current configuration in the PID controller... sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity()) #this code uses the GLSimulationProgram structure, which gives a little more control over the visualization vis.setPlugin(program) program.reshape(800,600) vis.show() while vis.shown(): time.sleep(0.1) return #this code manually updates the vis vis.add("world",world) vis.show() t0 = time.time() while vis.shown(): vis.lock() sim.simulate(0.01) sim.updateWorld() vis.unlock() t1 = time.time() time.sleep(max(0.01-(t1-t0),0.001)) t0 = t1 return
def main(): print( "================================================================================" ) print(sys.argv[0] + ": Simulates a robot file and Python controller") if len(sys.argv) <= 1: print( "USAGE: klampt_sim [world_file] [trajectory (.traj/.path) or controller (.py)]" ) print() print( " Try: klampt_sim athlete_plane.xml motions/athlete_flex_opt.path " ) print(" [run from Klampt-examples/data/]") print( "================================================================================" ) if len(sys.argv) <= 1: exit() world = WorldModel() #load up any world items, control modules, or paths control_modules = [] for fn in sys.argv[1:]: path, base = os.path.split(fn) mod_name, file_ext = os.path.splitext(base) if file_ext == '.py' or file_ext == '.pyc': sys.path.append(os.path.abspath(path)) mod = importlib.import_module(mod_name, base) control_modules.append(mod) elif file_ext == '.path': control_modules.append(fn) else: res = world.readFile(fn) if not res: print("Unable to load model " + fn) print("Quitting...") sys.exit(1) viewer = MyGLViewer(world) for i, c in enumerate(control_modules): if isinstance(c, str): from klampt.control.blocks import trajectory_tracking from klampt.model.trajectory import RobotTrajectory from klampt.io import loader traj = loader.load('Trajectory', c) rtraj = RobotTrajectory(world.robot(i), traj.times, traj.milestones) #it's a path file, try to load it controller = trajectory_tracking.TrajectoryPositionController( rtraj) else: try: maker = c.make except AttributeError: print("Module", c.__name__, "must have a make() method") print("Quitting...") sys.exit(1) controller = maker(world.robot(i)) viewer.sim.setController(world.robot(i), controller) vis.setWindowTitle("Klamp't Simulation Tester") if SPLIT_SCREEN_TEST: viewer2 = MyGLViewer(world) vis.setPlugin(viewer) vis.addPlugin(viewer2) viewer2.window.broadcast = True vis.show() while vis.shown(): time.sleep(0.01) vis.kill() else: vis.run(viewer)
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) print "Ending klampt.vis visualization." vis.kill()