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 callback(robot_controller=robot_controller,drawing_controller=drawing_controller,trace=trace, storage=[sim_world,planning_world,sim,controller_vis]): start_clock = time.time() dt = 1.0/robot_controller.controlRate() #advance the controller robot_controller.startStep() drawing_controller.advance(dt) robot_controller.endStep() #update the visualization sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition())) Tee=sim_robot.link(ee_link).getTransform() wp = se3.apply(Tee,ee_local_pos) if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01: trace.milestones.append(wp) trace.times.append(0) if len(trace.milestones)==2: vis.add("trace",trace,color=(0,1,1,1),pointSize=0) if len(trace.milestones) > 200: trace.milestones = trace.milestones[-100:] trace.times = trace.times[-100:] if len(trace.milestones)>2: if _backend=='IPython': vis.remove("trace") vis.add("trace",trace,color=(0,1,1,1),pointSize=0) else: vis.dirty("trace") controller_vis.update() cur_clock = time.time() duration = cur_clock - start_clock time.sleep(max(0,dt-duration))
def PIP_Remove(i, vis): # This function is used to remove the previous PIP to make sure that no residual PIPs exist. Edge_Index = str(i) vis.remove("PIPEdge:" + Edge_Index) vis.remove("PIPEdgeCOM:" + Edge_Index) vis.remove("PIPEdgex:" + Edge_Index) vis.remove("PIPEdgey:" + Edge_Index) vis.remove("PIPEdgez:" + Edge_Index)
def setMode(m): global mode, drawExtra mode = m vis.lock() for s in drawExtra: vis.remove(s) drawExtra = set() vis.unlock()
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 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 remove_from_vis(self): """Removes a previously-added gripper from the klampt.vis scene.""" prefix = "gripper_"+self.name from klampt import vis try: vis.remove(prefix+"_world") except Exception: pass if self.center is not None: vis.remove(prefix+"_center") if self.primary_axis is not None: vis.remove(prefix+"_primary") if self.secondary_axis is not None: vis.remove(prefix+"_secondary") elif self.maximum_span is not None: vis.remove(prefix+"_opening")
vis.add("cpbgrad", vectorops.madd(res.cp2, res.grad2, l)) vis.setColor('cpagrad', 1, 1, 0) vis.setColor('cpbgrad', 1, 1, 0) drawExtra.add("cpagrad") drawExtra.add("cpbgrad") vis.unlock() elif mode == 'contacts': try: res = a.contacts(b, 0.05, 0.01) except Exception as e: print("Exception encountered:", e) continue tq1 = time.time() vis.lock() for s in drawExtra: vis.remove(s) drawExtra = set() for i, (p1, p2) in enumerate(zip(res.points1, res.points2)): vis.add("cpa" + str(i), [v for v in p1], hide_label=True, color=[1, 1, 0, 1], size=5) vis.add("cpb" + str(i), [v for v in p2], hide_label=True, color=[1, 0.5, 0, 1], size=5) l = 0.1 vis.add("n" + str(i), Trajectory([0, 1], [[v for v in p1], vectorops.madd(p1, res.normals[i], l)]),
def Traj_Vis(world, DOF, robot_traj, PIP_traj, CPFlag, 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() # ipdb.set_trace() state_traj = robot_traj[0] sim_robot = world.robot(0) EdgeAList = PIP_traj[0] EdgeBList = PIP_traj[1] EdgeCOMList = PIP_traj[2] EdgexList = PIP_traj[3] EdgeyList = PIP_traj[4] EdgezList = PIP_traj[5] TotalTrajLength = len(state_traj) EffectiveTrajLength = len(EdgeAList) RedundantTrajLength = TotalTrajLength - EffectiveTrajLength NumberOfConfigs = len(state_traj) TransScale = 0.35 PVKRBcolor = [0, 0.4470, 0.7410] PVKCPcolor = [0.6588, 0.6588, 0.1961] PVKHJBcolor = [0.4940, 0.1840, 0.5560] ZSCcolor = [0.3010, 0.7450, 0.9330] OEcolor = [0.6350, 0.0780, 0.1840] CPcolor = [0.25, 0.25, 0.25] ZMPcolor = [0.75, 0, 0.75] if CPFlag is 2: state_traj = robot_traj[0] PVKRB_traj = robot_traj[1] PVKCP_traj = robot_traj[2] PVKHJB_traj = robot_traj[3] ZSC_traj = robot_traj[4] OE_traj = robot_traj[5] CP_traj = robot_traj[6] ZMP_traj = robot_traj[7] PVKRBFlag, PVKRBIndex = FailureIndicator(PVKRB_traj) PVKCPFlag, PVKCPIndex = FailureIndicator(PVKCP_traj) PVKHJBFlag, PVKHJBIndex = FailureIndicator(PVKHJB_traj) ZSCFlag, ZSCIndex = FailureIndicator(ZSC_traj) OEFlag, OEIndex = FailureIndicator(OE_traj) CPFlag, CPIndex = FailureIndicator(CP_traj) ZMPFlag, ZMPIndex = FailureIndicator(ZMP_traj) Flags = [ PVKRBFlag, PVKCPFlag, PVKHJBFlag, ZSCFlag, OEFlag, CPFlag, ZMPFlag ] Indices = [ PVKRBIndex, PVKCPIndex, PVKHJBIndex, ZSCIndex, OEIndex, CPIndex, ZMPIndex ] if PVKRBFlag is True: GhostPose = state_traj[PVKRBIndex + RedundantTrajLength] vis.add("PVKRBGhost", GhostPose) vis.hide("PVKRBGhost") vis.setColor("PVKRBGhost", PVKRBcolor[0], PVKRBcolor[1], PVKRBcolor[2], TransScale) if PVKCPFlag is True: GhostPose = state_traj[PVKCPIndex + RedundantTrajLength] vis.add("PVKCPGhost", GhostPose) vis.hide("PVKCPGhost") vis.setColor("PVKCPGhost", PVKCPcolor[0], PVKCPcolor[1], PVKCPcolor[2], TransScale) if PVKHJBFlag is True: GhostPose = state_traj[PVKHJBIndex + RedundantTrajLength] vis.add("PVKHJBGhost", GhostPose) vis.hide("PVKHJBGhost") vis.setColor("PVKHJBGhost", PVKHJBcolor[0], PVKHJBcolor[1], PVKHJBcolor[2], TransScale) if ZSCFlag is True: GhostPose = state_traj[ZSCIndex + RedundantTrajLength] vis.add("ZSCGhost", GhostPose) vis.hide("ZSCGhost") vis.setColor("ZSCGhost", ZSCcolor[0], ZSCcolor[1], ZSCcolor[2], TransScale) if OEFlag is True: GhostPose = state_traj[OEIndex + RedundantTrajLength] vis.add("OEGhost", GhostPose) vis.hide("OEGhost") vis.setColor("OEGhost", OEcolor[0], OEcolor[1], OEcolor[2], TransScale) if CPFlag is True: GhostPose = state_traj[CPIndex + RedundantTrajLength] vis.add("CPGhost", GhostPose) vis.hide("CPGhost") vis.setColor("CPGhost", CPcolor[0], CPcolor[1], CPcolor[2], TransScale) if ZMPFlag is True: GhostPose = state_traj[ZMPIndex + RedundantTrajLength] vis.add("ZMPGhost", GhostPose) vis.hide("ZMPGhost") vis.setColor("ZMPGhost", ZMPcolor[0], ZMPcolor[1], ZMPcolor[2], TransScale) while vis.shown(): # This is the main plot program InfeasiFlag = 0 for i in range(0, EffectiveTrajLength): vis.lock() config_i = state_traj[i + RedundantTrajLength] sim_robot.setConfig(config_i[0:DOF]) COM_Pos = sim_robot.getCom() Robot_COM_Plot(sim_robot, vis) EdgeAList_i = EdgeAList[i] EdgeBList_i = EdgeBList[i] EdgeCOMList_i = EdgeCOMList[i] EdgexList_i = EdgexList[i] EdgeyList_i = EdgeyList[i] EdgezList_i = EdgezList[i] for j in range(0, len(EdgeAList_i)): EdgeA = EdgeAList_i[j] EdgeB = EdgeBList_i[j] EdgeCOM = EdgeCOMList_i[j] Edgex = EdgexList_i[j] Edgey = EdgeyList_i[j] Edgez = EdgezList_i[j] PIP_Subplot(j, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos, vis) # # For the 7 fall indicators # if (PVKRBFlag is True) and (i>=PVKRBIndex): # vis.hide("PVKRBGhost", False) # if (PVKCPFlag is True) and (i>=PVKCPIndex): # vis.hide("PVKCPGhost", False) # if (PVKHJBFlag is True) and (i>=PVKHJBIndex): # vis.hide("PVKHJBGhost", False) # if (ZSCFlag is True) and (i>=ZSCIndex): # vis.hide("ZSCGhost", False) # if (OEFlag is True) and (i>OEIndex): # vis.hide("OEGhost", False) # if (CPFlag is True) and (i>CPIndex): # vis.hide("CPGhost", False) # if (ZMPFlag is True) and (i>ZMPIndex): # vis.hide("ZMPGhost", False) # print Flags # print Indices if CPFlag is 1 or 2: try: h = ConvexHull(EdgeAList_i) except: InfeasiFlag = 1 if InfeasiFlag is 0: h = ConvexHull(EdgeAList_i) hrender = draw_hull.PrettyHullRenderer(h) vis.add("blah", h) vis.setDrawFunc("blah", my_draw_hull) else: print "Input Contact Polytope Infeasible!" vis.unlock() time.sleep(delta_t) for j in range(0, len(EdgeAList_i)): PIP_Remove(j, vis) if ((CPFlag is 1 or 2) and (InfeasiFlag is 0)): vis.remove("blah") # if (PVKRBFlag is True): # vis.hide("PVKRBGhost") # if (PVKCPFlag is True): # vis.hide("PVKCPGhost") # if (PVKHJBFlag is True): # vis.hide("PVKHJBGhost") # if (ZSCFlag is True): # vis.hide("ZSCGhost") # if (OEFlag is True): # vis.hide("OEGhost") # if (CPFlag is True): # vis.hide("CPGhost") # if (ZMPFlag is True): # vis.hide("ZMPGhost") print "End"
def clear_visitems(names): for name in list(set(names)): vis.remove(name)
def main(*arg): Exp_Name = arg[0] VisFlag = arg[1] # The default robot to be loaded is the HRP2 robot. Robot_Option = "../user/hrp2/" world = WorldModel() # WorldModel is a pre-defined class ipdb.set_trace() if ".path" in Exp_Name: # Then this means that we are given the robot trajectories for experimentations. Real_Exp_Name = copy.deepcopy(Exp_Name) Realer_Exp_Name = "" for str_i in Real_Exp_Name: if str_i == ".": break Realer_Exp_Name = Realer_Exp_Name + str_i delta_t, DOF, robot_traj, PVKRB_traj, PVKCP_traj, PVKHJB_traj, ZSC_traj, OE_traj, CP_traj, ZMP_traj = Traj_Loader_fn( Realer_Exp_Name) # The next step is to load in robot's XML file XML_path = ExpName + "Envi/Envi" + Realer_Exp_Name + ".xml" result = world.readFile( XML_path ) # Here result is a boolean variable indicating the result of this loading operation if not result: raise RuntimeError("Unable to load model " + fn) Contact_Link_Dictionary = Contact_Link_Reader("ContactLink.txt", ExpName + "User/") Contact_Status_Dictionary = Contact_Status_Reader( "InitContact.txt", ExpName + "User/") # There are five Vis flag options: """ * VisFlag = 0 : Pure Animation * VisFlag = 1 : Pure Animation with ePIPs and edges * VisFlag = 2 : Pure Animation with eCH and ePIPs * VisFlag = 3 : Pure Animation With eCH, ePIPs, and Ghosts """ if (VisFlag == 0): Robot_Traj_Plot(world, DOF, robot_traj, Contact_Link_Dictionary, delta_t) else: if (VisFlag == 1): PIPList = PIP_Traj_Reader(Realer_Exp_Name) CPFlag = 0 Traj_Vis(world, DOF, robot_traj, PIPList, CPFlag, delta_t) else: if VisFlag is 2: PIPList = PIP_Traj_Reader(Realer_Exp_Name) CPFlag = 1 Traj_Vis(world, DOF, robot_traj, PIPList, CPFlag, delta_t) else: if VisFlag is 3: PIPList = PIP_Traj_Reader(Realer_Exp_Name) CPFlag = 2 Traj_Vis(world, DOF, [ robot_traj, PVKRB_traj, PVKCP_traj, PVKHJB_traj, ZSC_traj, OE_traj, CP_traj, ZMP_traj ], PIPList, CPFlag, delta_t) else: raise RuntimeError( "Flag value not feasible for trajectory visualization!" ) else: # In this case, what we have is a config # The following function can be used in two ways: the first way is to load the Config_Init.config file while the second way is to load two DOF, Config_Init, Velocity_Init = State_Loader_fn( Exp_Name + ".config", Robot_Option) # DOF, Config_Init, Velocity_Init = State_Loader_fn("Init_Config.txt", "Init_Velocity.txt", Robot_Option) # DOF, Config_Init, Velocity_Init = State_Loader_fn("Opt_Init_Config.txt", "Opt_Init_Velocity.txt", Robot_Option) # State_Writer_fn(Config_Init, "Init_Config_from_txt.config", Robot_Option) # State_Writer_fn(Config_Init, Velocity_Init, "Inn_Config.txt", "Inn_Velo.txt", Robot_Option) # According to the initial condition of the robot contact status, a basic optimization may have to be contacted to enforce the initial constraints. # Now it is the validation of the feasibility of the given initial condition State_Init = Config_Init + Velocity_Init Convex_Edges_List = Convex_Edge_Reader(Exp_Name + "CHEdges.txt", Robot_Option) delta_t = 0.5 if (PIP_Flag == 0): Robot_Config_Plot(world, DOF, State_Init, Contact_Link_Dictionary, Convex_Edges_List) else: if PIP_Flag is 1: # In this case, we have to load in another sets of information for visualization PIPList = PIP_Info_Reader(Exp_Name + "PIPs.txt", Robot_Option) PIP_Config_Plot(world, DOF, State_Init, PIPList, 0) else: if PIP_Flag is 2: PIPList = PIP_Info_Reader(Exp_Name + "PIPs.txt", Robot_Option) PIP_Config_Plot(world, DOF, State_Init, PIPList, 1) else: if PIP_Flag is 3: # This function is specific for the four visualization of robot contact polytope # Four plots should be shown alternatively # 1. Contact Polytope with Edges # 2. Contact Polytope with PIPsExpName # 3. Contact Polytope with EPIPs # 4. E Contact Polytope with EPIPs Convex_Edges_List = Convex_Edge_Reader( Exp_Name + "CHEdges.txt", Robot_Option) ContactVerticies = ContactVerticesGenerator( Convex_Edges_List) Intersection_List = Intersection_Reader( Exp_Name + "Intersections.txt", Robot_Option) PIPList = PIP_Info_Reader(Exp_Name + "PIPs.txt", Robot_Option) # 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_Init[0:DOF]) COM_Pos = sim_robot.getCom() # t_orbital = ... # q_orbital = ... # vis.add("q_orbital",q_orbital) # vis.hide("q_orbital") # vis.setColor("q_orbital",1,0,0,0.5) while vis.shown(): # This is the main plot program vis.lock() # sim_robot.setConfig(Config_Init[0:DOF]) # if vis.animationTime() < t_orbital: # vis.hide("q_orbital") # else: # vis.hide("q_orbital",False) if robot_viewer.mode_no == 1: # Here we plot the full contact polytope h = ConvexHull(ContactVerticies) hrender = draw_hull.PrettyHullRenderer(h) vis.add("blah", h) vis.setDrawFunc("blah", my_draw_hull) if robot_viewer.mode_no == 2: h = ConvexHull(ContactVerticies) hrender = draw_hull.PrettyHullRenderer(h) vis.add("blah", h) vis.setDrawFunc("blah", my_draw_hull) # Also edges Convex_Edges_Plot(sim_robot, Convex_Edges_List, vis) # Then intersection from COM for i in range(0, len(Intersection_List)): COM2IntersectionPlot( i, vis, COM_Pos, Intersection_List[i]) if robot_viewer.mode_no == 3: h = ConvexHull(ContactVerticies) hrender = draw_hull.PrettyHullRenderer(h) vis.add("blah", h) vis.setDrawFunc("blah", my_draw_hull) # Also edges Convex_Edges_Plot(sim_robot, Convex_Edges_List, vis) PIPs_Number = len(PIPList[0]) EdgeAList = PIPList[0] EdgeBList = PIPList[1] EdgeCOMList = PIPList[2] EdgexList = PIPList[3] EdgeyList = PIPList[4] EdgezList = PIPList[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) if robot_viewer.mode_no == 4: # Effective Polytope with Effective PIPs PIPs_Number = len(PIPList[0]) EdgeAList = PIPList[0] # EdgeBList = PIPList[1] # for i in EdgeBList: # EdgeAList.append(i); h = ConvexHull(EdgeAList) hrender = draw_hull.PrettyHullRenderer(h) vis.add("blah", h) vis.setDrawFunc("blah", my_draw_hull) EdgeAList = PIPList[0] EdgeBList = PIPList[1] EdgeCOMList = PIPList[2] EdgexList = PIPList[3] EdgeyList = PIPList[4] EdgezList = PIPList[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) vis.unlock() print "mode number: " + str(robot_viewer.mode_no) time.sleep(delta_t) # Robot_COM_Plot(sim_robot, vis) # Now we should delete the imposed object if robot_viewer.mode_no == 1: vis.remove("blah") if robot_viewer.mode_no == 2: vis.remove("blah") EdgeNo = len(Convex_Edges_List) / 2 for i in range(0, EdgeNo): Edge_Index = str(i) vis.remove("Edge:" + Edge_Index) for i in range(0, len(Intersection_List)): Edge_Index = str(i) vis.remove("PIPEdgefromCOM:" + Edge_Index) if robot_viewer.mode_no == 3: vis.remove("blah") EdgeNo = len(Convex_Edges_List) / 2 for i in range(0, EdgeNo): Edge_Index = str(i) vis.remove("Edge:" + Edge_Index) EdgeAList = PIPList[0] for i in range(0, len(EdgeAList)): Edge_Index = str(i) vis.remove("PIPEExpNamedge:" + Edge_Index) vis.remove("PIPEdgeCOM:" + Edge_Index) vis.remove("PIPEdgex:" + Edge_Index) vis.remove("PIPEdgey:" + Edge_Index) vis.remove("PIPEdgez:" + Edge_Index) if robot_viewer.mode_no == 4: vis.remove("blah") EdgeAList = PIPList[0] for i in range(0, len(EdgeAList)): Edge_Index = str(i) vis.remove("PIPEdge:" + Edge_Index) vis.remove("PIPEdgeCOM:" + Edge_Index) vis.remove("PIPEdgex:" + Edge_Index) vis.remove("PIPEdgey:" + Edge_Index) vis.remove("PIPEdgez:" + Edge_Index) robot_viewer.mode_no = robot_viewer.mode_no + 1 if robot_viewer.mode_no == 5: robot_viewer.mode_no = 1 robot_viewer.mode_no = 3
def modification_template(world): """Tests a variety of miscellaneous vis functions""" vis.add("world",world) robot = world.robot(0) vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque import random for i in range(10): #set some links to random colors randlink = random.randint(0,robot.numLinks()-1) color = (random.random(),random.random(),random.random()) vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color) #test the on-screen text display vis.addText("text2","Here's some red text") vis.setColor("text2",1,0,0) vis.addText("text3","Here's bigger text") vis.setAttribute("text3","size",24) vis.addText("text4","Transform status") vis.addText("textbottom","Text anchored to bottom of screen",(20,-30)) #test a point pt = [2,5,1] vis.add("some point",pt) #test a rigid transform vis.add("some blinking transform",[so3.identity(),[1,3,0.5]]) vis.edit("some point") #vis.edit("some blinking transform") #vis.edit("coordinates:ATHLETE:ankle roll 3") #test an IKObjective link = world.robot(0).link(world.robot(0).numLinks()-1) #point constraint obj = ik.objective(link,local=[[0,0,0]],world=[pt]) #hinge constraint #obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]]) #transform constraint #obj = ik.objective(link,R=link.getTransform()[0],t=pt) vis.add("ik objective",obj) #enable plotting vis.addPlot('plot') vis.addPlotItem('plot','some point') vis.setPlotDuration('plot',10.0) #run the visualizer, which runs in a separate thread vis.setWindowTitle("Manual animation visualization test") class MyCallback: def __init__(self): self.iteration = 0 def __call__(self): vis.lock() #TODO: you may modify the world here. This line tests a sin wave. pt[2] = 1 + math.sin(self.iteration*0.03) vis.unlock() #changes to the visualization with vis.X functions can done outside the lock if (self.iteration % 100) == 0: if (self.iteration / 100)%2 == 0: vis.hide("some blinking transform") vis.addText("text4","The transform was hidden") vis.logPlotEvent('plot','hide') else: vis.hide("some blinking transform",False) vis.addText("text4","The transform was shown") vis.logPlotEvent('plot','show') #this is another way of changing the point's data without needing a lock/unlock #vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True) #or #vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)]) if self.iteration == 200: vis.addText("text2","Going to hide the text for a second...") if self.iteration == 400: #use this to remove text vis.clearText() if self.iteration == 500: vis.addText("text2","Text added back again") vis.setColor("text2",1,0,0) self.iteration += 1 callback = MyCallback() if not MULTITHREADED: vis.loop(callback=callback,setup=vis.show) else: vis.show() while vis.shown(): callback() time.sleep(0.01) #use this to remove a plot vis.remove("plot") vis.kill()
def remove_from_vis(self, prefix): from klampt import vis vis.remove(prefix + "_ik") for i, c in enumerate(self.contacts): vis.remove(prefix + "_c" + str(i))
def remove_from_vis(self, prefix): from klampt import vis Grasp.remove_from_vis(self, prefix) vis.remove(prefix + "_config")
def update(self): from klampt import vis t = 0 try: t = self.interface.clock() vis.addText(self.tag + "clock", '%.3f' % (t, ), (self.text_x, self.text_y)) except NotImplementedError: vis.addText(self.tag + "clock", str(self.interface) + " provides no clock", (self.text_x, self.text_y)) try: stat = self.interface.status() if stat != 'ok': vis.addText(self.tag + "status", 'Status: ' + stat, (self.text_x, self.text_y + 15), color=(1, 0, 0)) else: try: vis.remove(self.tag + "status") except Exception: pass except NotImplementedError: vis.addText(self.tag + "status", str(self.interface) + " provides no status", (self.text_x, self.text_y + 15)) moving = False endTime = 0 try: moving = self.interface.isMoving() if moving: endTime = self.interface.destinationTime() if moving: if endTime > t: vis.addText(self.tag + "moving", "Moving, %.3fs left" % (endTime - t, ), (self.text_x, self.text_y + 30)) else: vis.addText(self.tag + "moving", "Moving", (self.text_x, self.text_y + 30)) else: try: vis.remove(self.tag + "moving") except Exception: pass except NotImplementedError: pass try: qsns = self.interface.configToKlampt( self.interface.sensedPosition()) vis.add(self.tag + "q_sns", qsns, robot=self.visRobotIndex, color=(0, 1, 0, 0.5)) except NotImplementedError: qsns = None try: qcmd = self.interface.configToKlampt( self.interface.commandedPosition()) if qcmd != qsns: vis.add(self.tag + "q_cmd", qcmd, robot=self.visRobotIndex, color=(1, 1, 0, 0.5)) except NotImplementedError: pass try: if moving and endTime > t: qdes = self.interface.configToKlampt( self.interface.destinationPosition()) vis.add(self.tag + "q_dest", qdes, robot=self.visRobotIndex, color=(1, 0, 0, 0.5)) else: try: vis.remove(self.tag + "q_dest") except Exception: pass except NotImplementedError: pass
def showTwoStep(plugin, world, robot, ground1, ground2, ground3, ground4, link_foot, link_foot_other): """Read the data file and select trajectories to show, they are composed of two steps""" data = np.load('data/twoStepBunch.npz')['output'] ndata = len(data) N = 20 force_len = 0.5 vis.show() for j in range(ndata): i = j print('Entering traj %d' % i) l0, l1, h0, h1, vel, phase0, phase1 = data[i] # set those grounds ground1.setConfig([0, 0, 0, 0, 0, 0]) ground2.setConfig([l0, 0, h0, 0, 0, 0]) ground3.setConfig([l0 + l1, 0, h0 + h1, 0, 0, 0]) ground4.setConfig([2 * l0 + l1, 0, 2 * h0 + h1, 0, 0, 0]) while vis.shown() and not plugin.quit: if plugin.nextone: # check if we want next one plugin.nextone = False break # show phase0 t, q, force = getTraj(phase0) h_ = t[1] - t[0] if h_ < 0: break nPoint = len(t) for k in range(nPoint): vis.lock() useq = copy_copy(q[k], False) robot.setConfig(useq) footpos = link_foot.getWorldPosition([0, 0, 0.5]) support = np.array([force[k, 0], 0, force[k, 1]]) use_support = support / np.linalg.norm(support) * force_len force_end = vectorops.add(footpos, use_support.tolist()) vis.add("force", Trajectory([0, 1], [footpos, force_end])) vis.unlock() time.sleep(h_ * 5.0) vis.remove('force') # phase 1 t, q, force = getTraj(phase1) h_ = t[1] - t[0] if h_ < 0: break print('h_ = %f' % h_) for k in range(nPoint): vis.lock() useq = copy_copy(q[k], True) robot.setConfig(useq) footpos = link_foot_other.getWorldPosition([0, 0, 0.5]) support = np.array([force[k, 0], 0, force[k, 1]]) use_support = support / np.linalg.norm(support) * force_len force_end = vectorops.add(footpos, use_support.tolist()) vis.add("force", Trajectory([0, 1], [footpos, force_end])) vis.unlock() time.sleep(h_ * 5.0) vis.remove('force') while vis.shown() and not plugin.quit: vis.lock() vis.unlock() time.sleep(0.05) print("Ending visualization.") vis.kill()
#changes to the visualization must be done outside the lock if (iteration % 100) == 0: if (iteration / 100) % 2 == 0: vis.hide("some blinking transform") vis.addText("text4", "The transform was hidden") vis.logPlotEvent('plot', 'hide') else: vis.hide("some blinking transform", False) vis.addText("text4", "The transform was shown") vis.logPlotEvent('plot', 'show') #this is another way of changing the point's data #vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True) time.sleep(0.01) iteration += 1 vis.clearText() vis.remove("plot") """ #Now testing ability to re-launch windows print "Showing again..." vis.show() while vis.shown(): time.sleep(0.01) """ print "Doing a dialog..." vis.setWindowTitle("Dialog test") print "calling dialog()" vis.dialog() print "Doing a split screen program..." vp.w, vp.h = 640, 480
def deleteRM(self): for i in range(self.rmCnt): fName = "a" + str(i) vis.remove(fName)
def deletePath(self): for i in range(self.pathCnt): fName = "p" + str(i) vis.remove(fName)