def shutdown(u_input=None): if u_input: u_input.shutdown() try: if vis.shown(): Logger.log("Exiting when vis window closed", "") while vis.shown(): time.sleep(0.01) Logger.log("Shutting down", "FAIL") vis.kill() except TypeError: Logger.log("Shutting down", "FAIL") try: sys.exit(0) except SystemExit: os._exit(0)
def animate_trajectories(trajs, times, endWaitTime=5.0, speed=0.2): global world active = 0 for i in range(len(trajs)): vis.add( "traj" + str(i), trajs[i].getLinkTrajectory( END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION)) vis.hide("traj" + str(i)) vis.hideLabel("traj" + str(i)) vis.show() t0 = time.time() if len(times) == 1: tnext = float('inf') else: tnext = times[active + 1] while vis.shown(): t = time.time() if t - t0 > tnext: nextactive = (active + 1) % len(times) vis.hide("traj" + str(active)) vis.hide("traj" + str(nextactive), False) active = nextactive if nextactive + 1 == len(times): tnext += endWaitTime else: tnext += times[active + 1] - times[active] vis.lock() world.robot(0).setConfig(trajs[active].eval((t - t0) * speed, endBehavior='loop')) vis.unlock() time.sleep(0.01) print("Done.")
def play_with_trajectory(traj, configs=[3]): vis.add("trajectory", traj) names = [] for i, x in enumerate(traj.milestones): if i in configs: print("Editing", i) names.append("milestone " + str(i)) vis.add(names[-1], x[:]) vis.edit(names[-1]) #vis.addPlot("distance") vis.show() while vis.shown(): vis.lock() t0 = time.time() updated = False for name in names: index = int(name.split()[1]) qi = vis.getItemConfig(name) if qi != traj.milestones[index]: traj.milestones[index] = qi updated = True if updated: vis.add("trajectory", traj) xnew = trajcache.trajectoryToState(traj) ctest2.setx(xnew) res = ctest2.minvalue(xtraj) print(res) ctest2.clearx() vis.unlock() t1 = time.time() #vis.logPlot("timing","opt",t1-t0) time.sleep(max(0.001, 0.025 - (t1 - t0)))
def edit_template(world): """Shows how to pop up a visualization window with a world in which the robot configuration and a transform can be edited""" #add the world to the visualizer vis.add("world",world) xform = se3.identity() vis.add("transform",xform) robotPath = ("world",world.robot(0).getName()) #compound item reference: refers to robot 0 in the world vis.edit(robotPath) vis.edit("transform") #This prints how to get references to items in the visualizer print("Visualization items:") vis.listItems(indent=2) vis.setWindowTitle("Visualization editing test") if not MULTITHREADED: vis.loop(setup=vis.show) else: vis.show() while vis.shown(): vis.lock() #TODO: you may modify the world here. vis.unlock() time.sleep(0.01) print("Resulting configuration",vis.getItemConfig(robotPath)) print("Resulting transform (config)",vis.getItemConfig("transform")) # this is a vector describing the item parameters xform = list(xform) #convert se3 element from tuple to list config.setConfig(xform,vis.getItemConfig("transform")) print("Resulting transform (se3)",xform) #quit the visualization thread nicely vis.kill()
def plugin_template(world): """Demonstrates the GLPluginInterface functionality""" #create a subclass of GLPluginInterface plugin = MyGLPlugin(world) vis.pushPlugin(plugin) #put the plugin on top of the standard visualization functionality. #vis.setPlugin(plugin) #use this to completely replace the standard visualization functionality with your own. vis.add("world",world) vis.setWindowTitle("GLPluginInterface template") #run the visualizer if not MULTITHREADED: def callback(plugin=plugin): if plugin.quit: vis.show(False) vis.loop(callback=callback,setup=vis.show) else: #if plugin.quit is True vis.show() while vis.shown() and not plugin.quit: vis.lock() #TODO: you may modify the world here vis.unlock() #changes to the visualization must be done outside the lock time.sleep(0.01) if plugin.quit: #if you want to do other stuff after the window quits, the window needs to be hidden vis.show(False) print("Waiting for 2 s...") time.sleep(2.0) #quit the visualization thread nicely 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 Robot_Config_Plot(world, DOF, state_ref, contact_link_dictionary, convex_edges_list, delta_t=0.5): # This function is used to plot the robot motion # The optimized solution is used to plot the robot motion and the contact forces # Initialize the robot motion viewer robot_viewer = MyGLPlugin(world) # Here it is to unpack the robot optimized solution into a certain sets of the lists vis.pushPlugin(robot_viewer) vis.add("world", world) vis.show() sim_robot = world.robot(0) sim_robot.setConfig(state_ref[0:DOF]) contact_link_list = contact_link_dictionary.keys() while vis.shown(): # This is the main plot program vis.lock() sim_robot.setConfig(state_ref[0:DOF]) # Convex_Edges_Plot(sim_robot, convex_edges_list, vis) # Robot_COM_Plot(sim_robot, vis) vis.unlock() time.sleep(delta_t)
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 vis_interaction_test(world): """Run some tests of visualization module interacting with the resource module""" print("Showing robot in modal dialog box") vis.add("robot", world.robot(0)) vis.add("ee", world.robot(0).link(11).getTransform()) vis.dialog() config = resource.get("resourcetest1.config", description="Should show up without a hitch...", doedit=True, editor='visual', world=world) import time if MULTITHREADED: print( "Showing threaded visualization (this will fail on GLUT or Mac OS)" ) vis.show() for i in range(3): vis.lock() q = world.robot(0).getConfig() q[9] = 3.0 world.robot(0).setConfig(q) vis.unlock() time.sleep(1.0) if not vis.shown(): break vis.lock() q = world.robot(0).getConfig() q[9] = -1.0 world.robot(0).setConfig(q) vis.unlock() time.sleep(1.0) if not vis.shown(): break print("Hiding visualization window") vis.show(False) else: print("Showing single-threaded visualization for 5s") vis.spin(5.0) config = resource.get("resourcetest1.config", description="Should show up without a hitch...", doedit=True, editor='visual', world=world)
def run(self): while vis.shown(): vis.lock() vis.unlock() # changes to the visualization must be done outside the lock vis.clearText() print "Ending klampt.vis visualization."
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 PIP_Config_Plot(world, DOF, state_ref, pips_list, CPFlag, delta_t=0.5): # This function is used to plot the robot motion # The optimized solution is used to plot the robot motion and the contact forces # Initialize the robot motion viewer robot_viewer = MyGLPlugin(world) # Here it is to unpack the robot optimized solution into a certain sets of the lists vis.pushPlugin(robot_viewer) vis.add("world", world) vis.show() sim_robot = world.robot(0) sim_robot.setConfig(state_ref[0:DOF]) InfeasiFlag = 0 while vis.shown(): # This is the main plot program vis.lock() sim_robot.setConfig(state_ref[0:DOF]) PIPs_Number = len(pips_list[0]) COM_Pos = sim_robot.getCom() EdgeAList = pips_list[0] EdgeBList = pips_list[1] EdgeCOMList = pips_list[2] EdgexList = pips_list[3] EdgeyList = pips_list[4] EdgezList = pips_list[5] for i in range(0, PIPs_Number): EdgeA = EdgeAList[i] EdgeB = EdgeBList[i] EdgeCOM = EdgeCOMList[i] Edgey = EdgexList[i] Edgez = EdgeyList[i] Edgex = EdgezList[i] PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos, vis) Robot_COM_Plot(sim_robot, vis) if CPFlag is 1: try: h = ConvexHull(EdgeAList) except: InfeasiFlag = 1 if InfeasiFlag is 0 and CPFlag is 1: h = ConvexHull(EdgeAList) hrender = draw_hull.PrettyHullRenderer(h) vis.add("blah", h) vis.setDrawFunc("blah", my_draw_hull) vis.unlock() time.sleep(delta_t)
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 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 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()
def Robot_Config_Plot(world, DOF, config_init): robot_viewer = MyGLPlugin(world) vis.pushPlugin(robot_viewer) vis.add("world", world) vis.show() # Here we would like to read point cloud for visualization of planning. # 1. All Reachable Points # IdealReachableContacts_data = ContactDataLoader("IdealReachableContact") # # 2. Active Reachable Points # ReachableContacts_data = ContactDataLoader("ReachableContacts") # # 3. Contact Free Points # CollisionFreeContacts_data = ContactDataLoader("CollisionFreeContacts") # # 4. Supportive Points # SupportiveContacts_data = ContactDataLoader("SupportiveContacts") OptimalContact_data = ContactDataLoader("OptimalContact") OptimalContactWeights_data = ContactDataLoader("OptimalContactWeights") OptimalContact_data, OptimalContactWeights_data = ContactDataRefine( OptimalContact_data, OptimalContactWeights_data) # # 6. # TransitionPoints_data = ContactDataLoader("TransitionPoints") # import ipdb; ipdb.set_trace() # 7. InitialTransitionPoints_data = ContactDataLoader("InitialPathWayPoints") # 8. ShiftedTransitionPoints_data = ContactDataLoader("ShiftedPathWayPoints") # 9. # FineShiftedPathWayPoints_data = ContactDataLoader("FineShiftedPathWayPoints") # # ReducedOptimalContact_data = ContactDataLoader("ReducedOptimalContact") ContactChoice = ShiftedTransitionPoints_data # ContactChoice = SupportiveContacts_data SimRobot = world.robot(0) SimRobot.setConfig(config_init) while vis.shown(): # This is the main plot program vis.lock() SimRobot.setConfig(config_init) WeightedContactDataPlot(vis, OptimalContact_data, OptimalContactWeights_data) ContactDataPlot(vis, ContactChoice) vis.unlock() time.sleep(0.1) import ipdb ipdb.set_trace() WeightedContactDataUnPlot(vis, OptimalContact_data) ContactDataUnplot(vis, ContactChoice)
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 animation_template(world): """Shows how to animate a robot.""" #first, build a trajectory with 10 random configurations robot = world.robot(0) times = range(10) milestones = [] for t in times: robot.randomizeConfig() milestones.append(robot.getConfig()) traj = trajectory.RobotTrajectory(robot, times, milestones) vis.add("world", world) robotPath = ("world", world.robot(0).getName() ) #compound item reference: refers to robot 0 in the world #we're also going to visualize the end effector trajectory #eetraj = traj.getLinkTrajectory(robot.numLinks()-1,0.05) #vis.add("end effector trajectory",eetraj) #uncomment this to automatically visualize the end effector trajectory vis.add("robot trajectory", traj) vis.setAttribute("robot trajectory", "endeffectors", [13, 20]) vis.setWindowTitle("Animation test") MANUAL_ANIMATION = False if not MANUAL_ANIMATION: #automatic animation, just call vis.animate vis.animate(robotPath, traj) if not MULTITHREADED: #need to set up references to function-local variables manually, and the easiest way is to use a default argument def callback(robot=robot): if MANUAL_ANIMATION: #with manual animation, you just set the robot's configuration based on the current time. t = vis.animationTime() q = traj.eval(t, endBehavior='loop') robot.setConfig(q) pass vis.loop(callback=callback, setup=vis.show) else: vis.show() while vis.shown(): vis.lock() if MANUAL_ANIMATION: #with manual animation, you just set the robot's configuration based on the current time. t = vis.animationTime() q = traj.eval(t, endBehavior='loop') robot.setConfig(q) vis.unlock() time.sleep(0.01) #quit the visualization thread nicely vis.kill()
def update_simulation(world, sim): # this code manually updates the visualization 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 basic_template(world): """Shows how to pop up a visualization window with a world""" #add the world to the visualizer vis.add("world", world) #adding a point vis.add("point", [1, 1, 1]) vis.setColor("point", 0, 1, 0) vis.setAttribute("point", "size", 5.0) #adding lines is currently not super convenient because a list of lists is treated as #a Configs object... this is a workaround to force the vis module to treat it as a polyline. vis.add("line", trajectory.Trajectory([0, 1], [[0, 0, 0], [1, 1, 1]])) vis.setAttribute("line", "width", 1.0) sphere = klampt.GeometricPrimitive() sphere.setSphere([1.5, 1, 1], 0.2) vis.add("sphere", sphere) vis.setColor("sphere", 0, 0, 1, 0.5) box = klampt.GeometricPrimitive() box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2]) g = klampt.Geometry3D(box) vis.add("box", g) vis.setColor("box", 0, 0, 1, 0.5) vis.setWindowTitle("Basic visualization test") if not MULTITHREADED: print("Running vis loop in single-threaded mode with vis.loop()") #single-threaded code def callback(): #TODO: you may modify the world here. pass vis.loop(setup=vis.show, callback=callback) else: print("Running vis loop in multithreaded mode") #multithreaded code vis.show() while vis.shown(): vis.lock() #TODO: you may modify the world here. Do not modify the internal state of any #visualization items outside of the lock vis.unlock() #outside of the lock you can use any vis.X functions, including vis.setItemConfig() #to modify the state of objects time.sleep(0.01) #quit the visualization thread nicely vis.kill()
def main(): #creates a world and loads all the items on the command line world = WorldModel() res = world.readFile("./HRP2_Robot.xml") if not res: raise RuntimeError("Unable to load model") robot = world.robot(0) # coordinates.setWorldModel(world) plugin = MyGLPlugin(world) vis.pushPlugin(plugin) #add the world to the visualizer vis.add("world", world) vis.add("robot", world.robot(0)) vis.show() # ipdb.set_trace() Robotstate_Traj, Contact_Force_Traj = Traj_Loader() h = 0.0068 # 0.0043: vert # 0.0033: flat playspeed = 2.5 norm = 500 while vis.shown(): # This is the main plot program for i in range(0, Robotstate_Traj.shape[1]): vis.lock() Robotstate_Traj_i = Robotstate_Traj[:, i] Robotstate_Traj_Full_i = Dimension_Recovery(Robotstate_Traj_i) # Now it is the plot of the contact force at the contact extremities robot.setConfig(Robotstate_Traj_Full_i) Contact_Force_Traj_i = Contact_Force_Traj[:, i] left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec( robot, Contact_Force_Traj_i, norm) vis.add("left foot force", Trajectory([0, 1], [left_ft, left_ft_end])) vis.add("right foot force", Trajectory([0, 1], [rght_ft, rght_ft_end])) vis.add("left hand force", Trajectory([0, 1], [left_hd, left_hd_end])) vis.add("right hand force", Trajectory([0, 1], [rght_hd, rght_hd_end])) COMPos_start = robot.getCom() COMPos_end = COMPos_start COMPos_end[2] = COMPos_end[2] + 100 vis.add("Center of Mass", Trajectory([0, 1], [COMPos_start, COMPos_end])) # ipdb.set_trace() vis.unlock() time.sleep(h * playspeed)
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 visualize(self): world = robotsim.WorldModel() vis.add("world", world) vis.add("coordinates", coordinates.manager()) vis.setWindowTitle("PointCloud World") vp = vis.getViewport() vp.w, vp.h = 800, 800 vis.setViewport(vp) # vis.autoFitCamera() vis.show() vis.lock() vis.unlock() while vis.shown(): time.sleep(0.01) 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 visualize_pc_in_klampt_vis(self, pcloud_fname): title = pcloud_fname + " klampt world" vis_window_id = vis.createWindow(title) vis.setWindowTitle(title) world = WorldModel() vis.add("world", world) pcd = o3d.io.read_point_cloud(pcloud_fname) print(pcd) pc_xyzs = np.asarray(pcd.points) pc_xyzs_as_list = pc_xyzs.flatten().astype("float") # pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0]) pcloud = PointCloud() pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list) print(pcloud.numPoints()) vis.add("pcloud", pcloud) # vis.setColor("pcloud", 0, 0, 1, a=1) # vis.setAttribute("p1", "size", 5.0) box = klampt.GeometricPrimitive() box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2]) g = klampt.Geometry3D(box) vis.add("box", g) vis.setColor("box", 0, 0, 1, 0.5) coordinates.setWorldModel(world) vis.add("coordinates", coordinates.manager()) vis.show() while vis.shown(): vis.lock() vis.unlock() time.sleep(0.01) vis.kill()
def Robot_Traj_Plot(world, DOF, state_traj, contact_link_dictionary, delta_t=0.5): # Initialize the robot motion viewer robot_viewer = MyGLPlugin(world) # Here it is to unpack the robot optimized solution into a certain sets of the lists vis.pushPlugin(robot_viewer) vis.add("world", world) vis.show() sim_robot = world.robot(0) contact_link_list = contact_link_dictionary.keys() while vis.shown(): # This is the main plot program for config_i in state_traj: vis.lock() sim_robot.setConfig(config_i[0:DOF]) # Robot_COM_Plot(sim_robot, vis) vis.unlock() time.sleep(delta_t)
from sensor_msgs.msg import CameraInfo def oncamerainfo(ci): vp = vis.getViewport() kros.from_CameraInfo(ci, vp) vis.setViewport(vp) camera_sub = rospy.Subscriber('camera_info', CameraInfo, oncamerainfo, queue_size=10) visworld = w.copy() vis.add("world", visworld) vis.show() while not rospy.is_shutdown() and vis.shown(): if TEST_TF_LISTENER: kros.listen_tf(listener, w) vis.lock() for i in range(w.robot(0).numLinks()): visworld.robot(0).link(i).setTransform( *w.robot(0).link(i).getTransform()) vis.unlock() else: vis.lock() visworld.robot(0).setConfig(w.robot(0).getConfig()) vis.unlock() rospy.sleep(0.01)
def idle(self): if not self.running: return if self.world.numRigidObjects() > 0: self.obj = self.world.rigidObject(0) else: return if not self.is_simulating: if len(self.poses) > 0: self.curr_pose = self.poses.pop(0) print "\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "Simulating Next Pose Grasp" print "Dims:\n", self.box_dims print "Pose:\n", self.curr_pose print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!" print "!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n" else: print "Done testing all", len( self.poses), "poses for object", self.box_dims print "Quitting" self.running = False vis.show(hidden=True) return w_T_o_se3 = se3.from_homogeneous(self.w_T_o) self.obj.setTransform(w_T_o_se3[0], w_T_o_se3[1]) w_T_h_des_se3 = se3.from_homogeneous( self.w_T_o.dot(self.curr_pose).dot(self.p_T_h)) self.robot.setConfig(self.q_0) set_moving_base_xform(self.robot, w_T_h_des_se3[0], w_T_h_des_se3[1]) if self.sim is None: self.sim = SimpleSimulator(self.world) self.hand = self.module.HandEmulator(self.sim, 0, 6, 6) self.sim.addEmulator(0, self.hand) # the next line latches the current configuration in the PID controller... self.sim.controller(0).setPIDCommand( self.robot.getConfig(), vectorops.mul(self.robot.getVelocity(), 0.0)) obj_b = self.sim.body(self.obj) obj_b.setVelocity([0., 0., 0.], [0., 0., 0.]) # setup the preshrink visPreshrink = False # turn this to true if you want to see the "shrunken" models used for collision detection for l in range(self.robot.numLinks()): self.sim.body( self.robot.link(l)).setCollisionPreshrink(visPreshrink) for l in range(self.world.numRigidObjects()): self.sim.body(self.world.rigidObject( l)).setCollisionPreshrink(visPreshrink) self.object_com_z_0 = getObjectGlobalCom(self.obj)[2] self.object_fell = False self.t_0 = self.sim.getTime() self.is_simulating = True if self.is_simulating: t_lift = 1.3 # when to lift d_lift = 1.0 # duration # print "t:", self.sim.getTime() - self.t_0 object_com_z = getObjectGlobalCom(self.obj)[2] w_T_h_curr_se3 = get_moving_base_xform(self.robot) w_T_h_des_se3 = se3.from_homogeneous( self.w_T_o.dot(self.curr_pose).dot(self.p_T_h)) if self.sim.getTime() - self.t_0 == 0: # print "Closing hand" self.hand.setCommand([1.0]) elif (self.sim.getTime() - self.t_0) >= t_lift and ( self.sim.getTime() - self.t_0) <= t_lift + d_lift: # print "Lifting" t_i = w_T_h_des_se3[1] t_f = vectorops.add(t_i, (0, 0, 0.2)) u = np.min((self.sim.getTime() - self.t_0 - t_lift, 1.)) send_moving_base_xform_PID(self.sim.controller(0), w_T_h_des_se3[0], vectorops.interpolate(t_i, t_f, u)) if (self.sim.getTime() - self.t_0 ) >= t_lift: # wait for a lift before checking if object fell d_hand = w_T_h_curr_se3[1][2] - w_T_h_des_se3[1][2] d_com = object_com_z - self.object_com_z_0 if d_hand - d_com > 0.1: self.object_fell = True print "!!!!!!!!!!!!!!!!!!" print "Grasp Unsuccessful" print "!!!!!!!!!!!!!!!!!!" self.sim.simulate(0.01) self.sim.updateWorld() if not vis.shown() or (self.sim.getTime() - self.t_0) >= 2.5 or self.object_fell: if vis.shown(): # simulation stopped because it was successful print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" print "Saving grasp, status:", "failure" if self.object_fell else "success" print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\n" w_T_h_curr = np.array(se3.homogeneous(w_T_h_curr_se3)) w_T_o_curr = np.array( se3.homogeneous(self.obj.getTransform())) h_T_o = np.linalg.inv(w_T_h_curr).dot(w_T_o_curr) if self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs: q_grasp = [ float('nan') ] * self.db.n_dofs if self.object_fell else self.hand.getConfiguration( ) elif self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs + self.hand.m_dofs: q_grasp = [ float('nan') ] * self.db.n_dofs if self.object_fell else self.hand.getFullConfiguration( ) else: raise Exception( 'Error: unexcpeted number of joints for hand') c_p, c_f = getObjectPhalanxMeanContactPoint( self.sim, self.obj, self.robot, self.links_to_check) try: self.db.save_simulation(self.box_dims, self.curr_pose, h_T_o, q_grasp, c_p, c_f) except: print "\nXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" print "X Error while calling save_simulation X" print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" traceback.print_exc() print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX\n" self.is_simulating = False self.sim = None self.robot.setConfig(self.q_0)
def launch_balls(robotname,num_balls=10): """Launches a very simple program that simulates a robot grasping an object from one of the databases. It first allows a user to position the robot's free-floating base in a GUI. Then, it sets up a simulation with those initial conditions, and launches a visualization. The controller closes the hand, and then lifts the hand upward. The output of the robot's tactile sensors are printed to the console. """ world = WorldModel() world.loadElement("data/terrains/plane.env") maxlayer = 16 numlayers = int(math.ceil(float(num_balls)/float(maxlayer))) for layer in xrange(numlayers): if layer + 1 == numlayers: ballsperlayer = num_balls%maxlayer else: ballsperlayer = maxlayer w = int(math.ceil(math.sqrt(ballsperlayer))) h = int(math.ceil(float(ballsperlayer)/float(w))) for i in xrange(ballsperlayer): bid = world.loadElement("data/objects/sphere_10cm.obj") if bid < 0: raise RuntimeError("data/objects/sphere_10cm.obj couldn't be loaded") ball = world.rigidObject(world.numRigidObjects()-1) R = so3.identity() x = i % w y = i / w x = (x - (w-1)*0.5)*box_dims[0]*0.7/(w-1) y = (y - (h-1)*0.5)*box_dims[1]*0.7/(h-1) t = [x,y,0.08 + layer*0.11] t[0] += random.uniform(-0.005,0.005) t[1] += random.uniform(-0.005,0.005) ball.setTransform(R,t) robot = make_moving_base_robot(robotname,world) box = make_box(world,*box_dims) box2 = make_box(world,*box_dims) box2.geometry().translate((0.7,0,0)) xform = resource.get("balls/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=True) if not xform: print "User quit the program" return #this sets the initial condition for the simulation 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) #A StateMachineController instance is now attached to control the robot import balls_controller sim.setController(robot,balls_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) vis.show() while vis.shown(): time.sleep(0.1) return """ #this code manually updates the visualization 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 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
Tb = [euler_angle_to_rotation(eb),[1,0,1]] def update_interpolation(u): #linear interpolation with euler angles e = interpolate_euler_angles(ea,eb,u) t = interpolate_linear(Ta[1],Tb[1],u) return (euler_angle_to_rotation(e),t) if __name__ == "__main__": #draw the world reference frame, the start and the goal, and the interpolated frame vis.add("world",se3.identity()) vis.add("start",Ta) vis.add("end",Tb) vis.add("interpolated",Ta) vis.setAttribute("world","length",0.25) vis.setAttribute("interpolated","fancy",True) vis.setAttribute("interpolated","width",0.03) vis.setAttribute("interpolated","length",1.0) period = 3.0 t0 = time.time() vis.show() while vis.shown(): #repeat the interpolation every 3 seconds u = ((time.time()-t0)%period)/period T = update_interpolation(u) vis.setItemConfig("interpolated",T) time.sleep(0.01) vis.kill()
def launch_simple(robotname,object_set,objectname,use_box=False): """Launches a very simple program that simulates a robot grasping an object from one of the databases. It first allows a user to position the robot's free-floating base in a GUI. Then, it sets up a simulation with those initial conditions, and launches a visualization. The controller closes the hand, and then lifts the hand upward. The output of the robot's tactile sensors are printed to the console. If use_box is True, then the test object is placed inside a box. """ world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname,world) object = make_object(object_set,objectname,world) if use_box: box = make_box(world,*box_dims) object.setTransform(*se3.mul((so3.identity(),[0,0,0.01]),object.getTransform())) doedit = True xform = resource.get("%s/default_initial_%s.xform"%(object_set,robotname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world) set_moving_base_xform(robot,xform[0],xform[1]) xform = resource.get("%s/initial_%s_%s.xform"%(object_set,robotname,objectname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=False) if xform: set_moving_base_xform(robot,xform[0],xform[1]) xform = resource.get("%s/initial_%s_%s.xform"%(object_set,robotname,objectname),description="Initial hand transform",default=robot.link(5).getTransform(),world=world,doedit=doedit) if not xform: print "User quit the program" return #this sets the initial condition for the simulation 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) #the result of simple_controller.make() is now attached to control the robot import simple_controller sim.setController(robot,simple_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 """ program.simulate = True vis.setPlugin(program) vis.show() while vis.shown(): time.sleep(0.1) return """ #this code manually updates the visualization 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