def __init__(self, endeffectors, constraints, traj): vis.GLPluginInterface.__init__(self) self.endeffectors = endeffectors self.constraints = constraints self.robot = self.constraints[0].robot self.traj = traj self.refConfig = self.robot.getConfig() vis.add("ghost1", self.refConfig) vis.setColor("ghost1", 0, 1, 0, 0.5) #vis.hide("ghost1",False) def bumpTrajectory(): relative_xforms = [] robot.setConfig(self.refConfig) for e in self.endeffectors: xform = vis.getItemConfig("ee_" + robot.link(e).getName()) T = (xform[:9], xform[9:]) T0 = robot.link(e).getTransform() Trel = se3.mul(se3.inv(T0), T) print "Relative transform of", e, "is", Trel relative_xforms.append(Trel) bumpTraj = cartesian_trajectory.cartesian_bump(self.robot, self.traj, self.constraints, relative_xforms, ee_relative=True, closest=True) assert bumpTraj != None vis.animate(("world", world.robot(0).getName()), bumpTraj) self.refresh() self.add_action(bumpTrajectory, "Bump trajectory", 'b')
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 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 callback(robot_controller=robot_controller, drawing_controller=drawing_controller, storage=[sim_world, planning_world, sim, controller_vis]): start_clock = time.time() dt = 1.0 / robot_controller.controlRate() #advance the controller robot_controller.startStep() if robot_controller.status() == 'ok': drawing_controller.advance(dt) vis.addText("Status", drawing_controller.state, (10, 20)) robot_controller.endStep() #update the visualization sim_robot.setConfig( robot_controller.configToKlampt(robot_controller.sensedPosition())) Tee = sim_robot.link(ee_link).getTransform() vis.add( "pen axis", Trajectory([0, 1], [ se3.apply(Tee, ee_local_pos), se3.apply(Tee, vectorops.madd(ee_local_pos, ee_local_axis, lifth)) ]), color=(0, 1, 0, 1)) controller_vis.update() cur_clock = time.time() duration = cur_clock - start_clock time.sleep(max(0, dt - duration))
def run_cartesian(world, paths): sim_world = world sim_robot = world.robot(0) vis.add("world", world) planning_world = world.copy() planning_robot = planning_world.robot(0) sim = Simulator(sim_world) robot_controller = RobotInterfaceCompleter( KinematicSimControlInterface(sim_robot)) #TODO: Uncomment this if you'd like to test in the physics simulation #robot_controller = RobotInterfaceCompleter(SimPositionControlInterface(sim.controller(0),sim)) if not robot_controller.initialize(): raise RuntimeError("Can't connect to robot controller") ee_link = 'EndEffector_Link' ee_local_pos = (0.15, 0, 0) ee_local_axis = (1, 0, 0) lifth = 0.05 drawing_controller = DrawingController(robot_controller, planning_robot, planning_robot.getConfig(), paths, ee_link, ee_local_pos, ee_local_axis, (0, 0, 1), lifth) controller_vis = RobotInterfacetoVis(robot_controller) #note: this "storage" argument is only necessary for jupyter to keep these around and not destroy them once main() returns def callback(robot_controller=robot_controller, drawing_controller=drawing_controller, storage=[sim_world, planning_world, sim, controller_vis]): start_clock = time.time() dt = 1.0 / robot_controller.controlRate() #advance the controller robot_controller.startStep() if robot_controller.status() == 'ok': drawing_controller.advance(dt) vis.addText("Status", drawing_controller.state, (10, 20)) robot_controller.endStep() #update the visualization sim_robot.setConfig( robot_controller.configToKlampt(robot_controller.sensedPosition())) Tee = sim_robot.link(ee_link).getTransform() vis.add( "pen axis", Trajectory([0, 1], [ se3.apply(Tee, ee_local_pos), se3.apply(Tee, vectorops.madd(ee_local_pos, ee_local_axis, lifth)) ]), color=(0, 1, 0, 1)) controller_vis.update() cur_clock = time.time() duration = cur_clock - start_clock time.sleep(max(0, dt - duration)) vis.loop(callback=callback)
def sampling_9(serial_num): world_name = "flatworld" world_file = "../../data/robot_model_files/worlds/" + world_name + ".xml" robot_file = "../../data/robot_model_files/robots/irb120_icp.rob" world = get_world(world_file, robot_file, visualize_robot=True) robot = world.robot(0) est_config_list = cal_est_config_list(robot, serial_num) path = collision_checking(world, est_config_list, 0.001) if path is False: sys.exit("The calibration path exist self-collision.") else: print("The calibration path is collision free.") vis.clear() vis.add("world", world) for i in range(len(est_config_list)): vis.add("estimated configure" + str(i), est_config_list[i]) vis.setColor("estimated configure" + str(i), 1.0, 0.0, 0.1 * i, 0.5) vis.spin(float('inf')) df = pd.DataFrame(np.asarray(est_config_list)[:, 7:13] * (180. / np.pi)) df.to_csv("../../data/robot_configuration_files/configuration_record_" + serial_num + ".csv", header=False, index=False) time.sleep(1.) est_config_list = est_config_list[0:] controller = SampleController(est_config_list, robot, '192.168.1.178') controller.start()
def initialize(self): vis.add("instructions1", "Right-click to get the list of intersecting items") vis.add("instructions2", "Press q, Ctrl+q, or select Quit from the menu to quit") vis.GLPluginInterface.initialize(self) return True
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 main(): world, robot, link_index, geom_index, obs_index = create_world() q0 = [0, -2, 0] qf = [3, 1, 3.14] config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, q0=q0, qf=qf, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create guess and solve n_grid = 10 guess = np.linspace(q0, qf, n_grid) rst = trajopt.optimize(guess) print(rst) traj = rst['sol'] # show the results vis.add('world', world) robot.setConfig(traj[-1]) for i, qi in enumerate(traj[:-1]): name = 'ghost%d' % i vis.add(name, qi) vis.setAttribute(name, 'type', 'Config') vis.setColor(name, 0, 1, 0, 0.4) vis.spin(100)
def sphere_grid_test(N=5, staggered=True): from klampt import vis from klampt.model import trajectory if staggered: G = sphere_staggered_grid(N) else: G = sphere_grid(N) for n in G.nodes(): x = G.node[n]['params'] vis.add(str(n), x) vis.hideLabel(str(n)) #draw edges? minDist = float('inf') maxDist = 0.0 for i, j in G.edges(): xi = G.node[i]['params'] xj = G.node[j]['params'] vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj])) vis.hideLabel(str(i) + '-' + str(j)) dist = vectorops.distance(xi, xj) if dist > maxDist: maxDist = dist print "Max dispersion at", i, j, ":", dist if dist < minDist: minDist = dist print "Number of points:", G.number_of_nodes() print "Min/max dispersion:", minDist, maxDist vis.run()
def convert(value_new,cmap_new,feature_new=None,lighting_new=None): global value,cmap,feature,lighting,w if value_new is None or value_new == value: if cmap_new is None or cmap == cmap_new: if feature_new is None or feature == feature_new: if lighting_new is None or feature == lighting_new: return if cmap_new is not None: cmap = cmap_new if value_new is not None: value = value_new if feature_new is not None: feature = feature_new if lighting_new is not None: lighting = lighting_new if lighting_new == 'none': lighting = None if value is None: value = 'z' if cmap is None: cmap = 'viridis' a_app = colorize.colorize(w.rigidObject(0),value,cmap,feature,lighting=lighting) #a_app.drawGL(a) #if not value.startswith('n'): colorize.colorize(a_pc,value,cmap,lighting=lighting) vis.remove("A") vis.add("A",a_app) vis.remove("B") vis.add("B",a_pc) vis.dirty("B")
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 qt_template(world): """Runs a custom Qt frame around a visualization window""" if not glinit.available('PyQt5'): print( "PyQt5 is not available on your system, try sudo apt-get install python-qt5" ) return #Qt objects must be explicitly deleted for some reason in PyQt5... g_mainwindow = None #All Qt functions must be called in the vis thread. #To hook into that thread, you will need to pass a window creation function into vis.customUI. def makefunc(gl_backend): global g_mainwindow g_mainwindow = MyQtMainWindow(gl_backend) return g_mainwindow vis.customUI(makefunc) vis.add("world", world) vis.setWindowTitle("Klamp't Qt test") vis.spin(float('inf')) vis.kill() #Safe cleanup of all Qt objects created in makefunc. #If you don't have this, PyQt5 complains about object destructors being called from the wrong thread del g_mainwindow
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 RobotCOMPlot(SimRobot, vis): COMPos_start = SimRobot.getCom() COMPos_end = COMPos_start[:] COMPos_end[2] = COMPos_end[2] - 7.50 vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end])) vis.hideLabel("COM", True) vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute("COM", 'width', 5.0)
def set_remaining_milestones(self, remaining, G_list): self.remaining = remaining self.remaining_milestones = self.milestones[list(remaining)] self.fraction = len(remaining) / self.milestones.shape[0] self.create_internal_subgraphs(G_list) # if(self.fraction < 0.35): vis.add('remaining_milestones', [i[:self.robot.numLinks()] for i in self.remaining_milestones], color=[1, 0, 0, 0.5])
def reset(): vis.lock() robot.setConfig(q0) base_link.setParentTransform( *se3.mul(se3.inv(parent_xform), base_xform0)) vis.unlock() vis.add("base_xform", base_xform0) vis.edit("base_xform") vis.setItemConfig("gripper", grob.getConfig())
def COM2IntersectionPlot(i, vis, COM_Pos, Intersection): # This function is used to plot PIP from COM to intersections Edge_Index = str(i) vis.add("PIPEdgefromCOM:" + Edge_Index, Trajectory([0, 1], [COM_Pos, Intersection])) vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True) vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def add_hm_to_klampt_vis(self, pcd_fname): pc_xyzs, non_floor_points_xyz = self.parse_hm(pcd_fname) pc_xyzs_as_list = pc_xyzs.flatten().astype("float") pcloud = PointCloud() pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list) vis.add("pcloud", pcloud) r, g, b, a = PCloudParser.PCLOUD_COLOR vis.setColor("pcloud", r, g, b, a)
def shift_object(amt): global cur_object,cur_grasp,shown_grasps,db vis.remove(db.objects[cur_object]) cur_object += amt if cur_object >= len(db.objects): cur_object = 0 elif cur_object < 0: cur_object = len(db.objects)-1 vis.add(db.objects[cur_object],w.rigidObject(cur_object)) shift_grasp(None)
def convert(geom,type,label): global a,b,atypes,btype if label=='A': vis.add(label,atypes[type]) vis.setColor(label,1,0,0,0.5) a = atypes[type] else: vis.add(label,btypes[type]) vis.setColor(label,0,1,0,0.5) b = btypes[type]
def get_paths_svg(fn,add_to_vis=True): trajs,attrs = svgimport.svg_import_trajectories(fn,center=True,want_attributes=True) if add_to_vis: for i,(traj,attr) in enumerate(zip(trajs,attrs)): name = attr.get('name',"path %d"%i) vis.add(name,traj) for a,v in attr.items(): if a != 'name': vis.setAttribute(name,a,v) return trajs
def runPlanner(runfunc, name): global lastPlan res = runfunc() if res: if lastPlan: vis.remove(lastPlan) vis.add(name, res) vis.setColor(name, 1, 0, 0) vis.animate(("world", robot.getName()), res) lastPlan = name
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 goToWidget(): #first, set all constraints so they are fit at the robot's last solved configuration, and get the #current workspace coordinates vis.setItemConfig("ghost1", self.goalConfig) self.robot.setConfig(self.goalConfig) for e, d in zip(self.endeffectors, self.constraints): d.matchDestination(*self.robot.link(e).getTransform()) wcur = config.getConfig(self.constraints) wdest = [] for e in self.endeffectors: xform = vis.getItemConfig("ee_" + robot.link(e).getName()) wdest += xform print "Current workspace coords", wcur print "Dest workspace coords", wdest #Now interpolate self.robot.setConfig(self.goalConfig) traj1 = cartesian_trajectory.cartesian_interpolate_linear( robot, wcur, wdest, self.constraints, delta=1e-2, maximize=False) self.robot.setConfig(self.goalConfig) traj2 = cartesian_trajectory.cartesian_interpolate_bisect( robot, wcur, wdest, self.constraints, delta=1e-2) self.robot.setConfig(self.goalConfig) #traj3 = cartesian_trajectory.cartesian_path_interpolate(robot,[wcur,wdest],self.constraints,delta=1e-2,method='any',maximize=False) traj3 = None print "Method1 Method2 Method3:" print " ", (traj1 != None), (traj2 != None), (traj3 != None) if traj1: traj = traj1 elif traj2: traj = traj2 elif traj3: traj = traj3 else: traj = cartesian_trajectory.cartesian_interpolate_linear( robot, wcur, wdest, self.constraints, delta=1e-2, maximize=True) if traj: print "Result has", len(traj.milestones), "milestones" self.goalConfig = traj.milestones[-1] vis.setItemConfig(("world", world.robot(0).getName()), self.goalConfig) vis.animate(("world", world.robot(0).getName()), traj, speed=0.2, endBehavior='loop') vis.setItemConfig("ghost2", traj.milestones[-1]) vis.add("ee_trajectory", traj) self.refresh()
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 ConvexEdgesPlot(SimRobot, convex_edges_list, vis): Convex_Edges_Number = len(convex_edges_list) / 2 COMPos = SimRobot.getCom() for i in range(0, Convex_Edges_Number): EdgeA = convex_edges_list[2 * i] EdgeB = convex_edges_list[2 * i + 1] # Three edges to be added: A->B, A -> COM, B-> COM Edge_Index = str(i) vis.add("Edge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB])) vis.hideLabel("Edge:" + Edge_Index, True) vis.setAttribute("Edge:" + Edge_Index, 'width', 5.0)
def visualize_line(p1, p2, name="default_line", add_z=0, color=None, hm=None): if len(p1) < 3: p1 = [p1[0], p1[1], 0] if len(p2) < 3: p2 = [p2[0], p2[1], 0] p1 = [p1[0], p1[1], p1[2] + add_z] p2 = [p2[0], p2[1], p2[2] + add_z] traj = trajectory.Trajectory(milestones=[p1[0:3], p2[0:3]]) vis.add(name, traj) if color: VisUtils.set_color(name, color)
def convert(geom, type, label): global a, b, atypes, btypes, Ta, Tb if label == 'A': vis.add(label, atypes[type]) vis.setColor(label, 1, 0, 0, 0.5) a = atypes[type] a.setCurrentTransform(*Ta) else: vis.add(label, btypes[type]) vis.setColor(label, 0, 1, 0, 0.5) b = btypes[type] b.setCurrentTransform(*Tb)
def remesh(geom, label): global a, b, Ta, Tb if label == 'A': a = a.convert(a.type(), 0.06) vis.add(label, a) vis.setColor(label, 1, 0, 0, 0.5) a.setCurrentTransform(*Ta) elif label == 'B': b = b.convert(b.type(), 0.06) vis.add(label, b) vis.setColor(label, 0, 1, 0, 0.5) b.setCurrentTransform(*Tb)
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True, visualize=False,verbose=0): """For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable. Args: world (WorldModel): the world containing the objects and obstacles container: the container RigidObject / Terrain in world into which objects should be spawned. Assumed axis-aligned. objects (list of RigidObject): a list of RigidObjects in the world, at arbitrary locations. They are placed in order. container_wall_thickness (float, optional): a margin subtracted from the container's outer dimensions into which the objects are spawned. randomize_orientation (bool or str, optional): if True, the orientation of the objects are completely randomized. If 'z', only the z orientation is randomized. If False or None, the orientation is unchanged visualize (bool, optional): if True, pops up a visualization window to show the progress of the pile verbose (int, optional): if > 0, prints progress of the pile. Side effect: the positions of objects in world are modified Returns: (tuple): (world,sim), containing - world (WorldModel): the original world - sim (Simulator): the Simulator instance at the state used to obtain the stable placement of the objects. Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy() to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances. """ container_outer_bb = _get_bound(container) container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3)) spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:]) collision_margin = 0.0025 if visualize: from klampt import vis from klampt.model import config import time oldwindow = vis.getWindow() newwindow = vis.createWindow("make_object_pile dynamic visualization") vis.setWindow(newwindow) vis.show() visworld = world.copy() vis.add("world",visworld) sim = Simulator(world) sim.setSetting("maxContacts","20") sim.setSetting("adaptiveTimeStepping","0") Tfar = (so3.identity(),[0,0,-100000]) for object in objects: R,t = object.getTransform() object.setTransform(R,Tfar[1]) sim.body(object).setTransform(*Tfar) sim.body(object).enable(False) if verbose: print("Spawn area",spawn_area) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() for index in range(len(objects)): #always spawn above the current height of the pile if index > 0: objects_bound = _get_bound(objects[:index]) if verbose: print("Existing objects bound:",objects_bound) zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2]) spawn_area[0][2] += zshift spawn_area[1][2] += zshift object = objects[index] obb = _get_bound(object) zmin = obb[0][2] R0,t0 = object.getTransform() feasible = False for sample in range(1000): R,t = R0[:],t0[:] if randomize_orientation == True: R = so3.sample() t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin object.setTransform(R,t) xy_randomize(object,spawn_area[0],spawn_area[1]) if verbose: print("Sampled position of",object.getName(),object.getTransform()[1]) if not randomize_orientation: _,t = object.getTransform() object.setTransform(R,t) #object spawned, now settle sobject = sim.body(object) sobject.enable(True) sobject.setTransform(*object.getTransform()) res = sim.checkObjectOverlap() if len(res[0]) == 0: feasible = True #get it low as possible without overlapping R,t = object.getTransform() for lower in range(100): sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01])) res = sim.checkObjectOverlap() if len(res[0]) != 0: if verbose: print("Terminated lowering at",lower,"cm lower") sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01])) res = sim.checkObjectOverlap() break sim.updateWorld() break if not feasible: if verbose: print("Failed to place object",object.getName()) return None if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(0.1) if verbose: print("Beginning to simulate") #start letting everything fall for firstfall in range(10): sim.simulate(0.01) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(0.01) maxT = 5.0 dt = 0.01 t = 0.0 wdamping = -0.01 vdamping = -0.1 while t < maxT: settled = True for object in objects: sobject = sim.body(object) w,v = sobject.getVelocity() sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping)) if vectorops.norm(w) + vectorops.norm(v) > 1e-4: #settled settled=False break if settled: break if visualize: t0 = time.time() sim.simulate(dt) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(max(0.0,dt-(time.time()-t0))) t += dt if visualize: vis.show(False) vis.setWindow(oldwindow) return (world,sim)
print "CSpace stats:" spacestats = space.getStats() for k in sorted(spacestats.keys()): print " ",k,":",spacestats[k] print "Planner stats:" planstats = planner.getStats() for k in sorted(planstats.keys()): print " ",k,":",planstats[k] if path: #save planned milestone path to disk print "Saving to my_plan.configs" resource.set("my_plan.configs",path,"Configs") #visualize path as a Trajectory resource from klampt.model.trajectory import RobotTrajectory traj = RobotTrajectory(robot,range(len(path)),path) #resource.edit("Planned trajectory",traj,world=world) #visualize path in the vis module from klampt import vis vis.add("world",world) vis.animate(("world",robot.getName()),path) vis.add("trajectory",traj) vis.spin(float('inf')) #play nice with garbage collection planner.space.close() planner.close()
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
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
print 2,":",traj.eval(2) print 3,":",traj.eval(3) print 4,":",traj.eval(4) print 5,":",traj.eval(5) print 6,":",traj.eval(6) #print some interpolated points print 0.5,":",traj.eval(0.5) print 2.5,":",traj.eval(2.5) #print some stuff after the end of trajectory print 7,":",traj.eval(6) print 100.3,":",traj.eval(100.3) print -2,":",traj.eval(-2) from klampt import vis vis.add("point",[0,0,0]) vis.animate("point",traj) vis.add("traj",traj) #vis.spin(float('inf')) #show the window until you close it traj2 = trajectory.HermiteTrajectory() traj2.makeSpline(traj) vis.animate("point",traj2) vis.hide("traj") vis.add("traj2",traj2.configTrajectory().discretize(0.1)) #vis.spin(float('inf')) traj_timed = trajectory.path_to_trajectory(traj,vmax=2,amax=4) #next, try this line instead #traj_timed = trajectory.path_to_trajectory(traj,timing='sqrt-L2',speed='limited',vmax=2,amax=4)