def remove_visualization(self): if self.name: try: vis.hide(self.name) except AttributeError: msg = "Error removing SupportTriangle (" + self.name + ") from klamp't vis" Logger.log(msg, "FAIL")
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
def ContactDataUnplot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): ContactName = "Stage" + str(StepNo) + "LinkNo" + str( LimbNo) + "Point:" + str(i) vis.hide(ContactName, True)
def PIPCleaner(i, vis): # This function is used to remove the previous PIP to make sure that no residual PIPs exist. Edge_Index = str(i) vis.hide("PIPEdge:" + Edge_Index, True) vis.hide("PIPEdgeCOM:" + Edge_Index, True) vis.hide("PIPEdgex:" + Edge_Index, True) vis.hide("PIPEdgey:" + Edge_Index, True) vis.hide("PIPEdgez:" + Edge_Index, True)
def TransitionDataUnplot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): TransName = "Step" + str(StepNo) + "Limb" + str( LimbNo) + "TransPoint:" + str(i) vis.hide(TransName, 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 ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj): StartTime = ImpulseObj[0] EndTime = ImpulseObj[1] ImpulForce = ImpulseObj[2] DurationTime = EndTime - StartTime OriPoint = SimRobot.link(19).getWorldPosition([0.0, 0.0, 0.0]) ImpulseScale = 0.001 * (SimulationTime - StartTime) / DurationTime EndPoint = OriPoint[:] EndPoint[0] += ImpulForce[0] * ImpulseScale EndPoint[1] += ImpulForce[1] * ImpulseScale EndPoint[2] += ImpulForce[2] * ImpulseScale if SimulationTime < StartTime: pass elif SimulationTime > EndTime: vis.hide("Impulse", True) else: vis.add("Impulse", Trajectory([0, 1], [OriPoint, EndPoint])) vis.hideLabel("Impulse", True) vis.setColor("Impulse", 235.0 / 255.0, 52.0 / 255.0, 52.0 / 255.0, 1.0) vis.setAttribute("Impulse", 'width', 5.0)
assert f != None vis.add("ee_"+robot.link(e).getName(),fw) vis.edit("ee_"+robot.link(e).getName()) obj = coordinates.ik_fixed_objective(f) eeobjectives.append(obj) #this tests the cartesian interpolation stuff print "***** BEGINNING CARTESIAN INTERPOLATION TEST *****" vis.setWindowTitle("Klamp't Cartesian interpolation test") vis.pushPlugin(InterpKeyCapture(endeffectors,eeobjectives)) vis.show() while vis.shown(): coordinates.updateFromWorld() time.sleep(0.1) vis.popPlugin() vis.hide("ghost1") vis.hide("ghost2") vis.animate(("world",world.robot(0).getName()),None) print print #this tests the "bump" function stuff print "***** BEGINNING BUMP FUNCTION TEST *****" configs = resource.get("cartesian_test"+world.robot(0).getName()+".configs",description="Make a reference trajectory for bump test",world=world) if configs is None: print "To test the bump function, you need to create a reference trajectory" vis.kill() exit(0) print "Found trajectory with",len(configs),"configurations" traj = trajectory.RobotTrajectory(robot,range(len(configs)),configs) vis.setWindowTitle("Klamp't Trajectory bump test")
def ContactDataUnplot(vis, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): vis.hide("Point_" + str(i))
def WeightedContactDataUnPlot(vis, OptimalContact_data): for i in range(OptimalContact_data.size / 3): vis.hide("PointWeights_" + str(i))
def ExperimentVisualizer(world, ContactLinkDictionary, ExpTraj, PIPInfoList, ImpulseObj, PlanningObj, SpecificPath, Para): ExpViewer = MyGLPlugin(world) vis.pushPlugin(ExpViewer) vis.add("world", world) vis.show() SimRobot = world.robot(0) FailureStateTraj = ExpTraj[0] CtrlStateTraj = ExpTraj[1] PlanStateTraj = ExpTraj[2] EdgeAList = PIPInfoList[0] EdgeBList = PIPInfoList[1] EdgeCOMList = PIPInfoList[2] EdgexList = PIPInfoList[3] EdgeyList = PIPInfoList[4] EdgezList = PIPInfoList[5] EdgeVertexList = PIPInfoList[6] TimeStep = CtrlStateTraj.times[1] - CtrlStateTraj.times[0] PlotDuration = 10 * TimeStep StateTrajLength = len(CtrlStateTraj.times) PIPTrajLength = len( EdgeAList) # Here PIPTraj could be less than the length of state StateTraj = [] StateType = Para[0] VisMode = Para[1] if StateType == "F": print "Failure State Traj!" StateTraj = FailureStateTraj.milestones elif StateType == "C": print "Controlled State Traj!" StateTraj = CtrlStateTraj.milestones else: print "Planned State Traj!" StateTraj = PlanStateTraj.milestones if (len(PlanningObj) == 0): while vis.shown(): # This is the main plot program for i in range(0, StateTrajLength): SimulationTime = 1.0 * i * TimeStep vis.lock() Config = StateTraj[i] SimRobot.setConfig(Config) ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj) vis.unlock() time.sleep(1.0 * TimeStep) return else: pass PlanningTimeList = PlanningObj[0] PlanningResList = PlanningObj[1] SimulationTime = 0.0 while vis.shown(): # This is the main plot program for i in range(0, StateTrajLength): SimulationTime = 1.0 * i * TimeStep vis.lock() Config = StateTraj[i] SimRobot.setConfig(Config) ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj) if VisMode == "PIP": COMPos = SimRobot.getCom() if (i >= (StateTrajLength - PIPTrajLength)): EdgeIndex = i + PIPTrajLength - StateTrajLength EdgeAList_i = EdgeAList[EdgeIndex] EdgeBList_i = EdgeBList[EdgeIndex] EdgeCOMList_i = EdgeCOMList[EdgeIndex] EdgexList_i = EdgexList[EdgeIndex] EdgeyList_i = EdgeyList[EdgeIndex] EdgezList_i = EdgezList[EdgeIndex] EdgeVertexList_i = EdgeVertexList[EdgeIndex] 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] PIPVisualizer(j, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COMPos, vis) elif VisMode == "Poly": FeasiFlag = 1 # for j in range(0, len(EdgeAList_i)): if (i >= (StateTrajLength - PIPTrajLength)): EdgeIndex = i + PIPTrajLength - StateTrajLength EdgeVertexList_i = EdgeVertexList[EdgeIndex] try: h = ConvexHull(EdgeVertexList_i) except: FeasiFlag = 0 if FeasiFlag == 1: h = ConvexHull(EdgeVertexList_i) hrender = draw_hull.PrettyHullRenderer(h) vis.add("ContactPolytope", h) vis.setDrawFunc("ContactPolytope", my_draw_hull) else: print "Input Contact Polytope Infeasible!" else: EndEffectorTrajPlot(vis, SimulationTime, PlanningObj, SpecificPath, PlotDuration, TimeStep) vis.unlock() time.sleep(1.0 * TimeStep) if VisMode == "PIP" and i >= (StateTrajLength - PIPTrajLength): EdgeIndex = i + PIPTrajLength - StateTrajLength for i in range(0, len(EdgeAList[EdgeIndex])): PIPCleaner(i, vis) if VisMode == "Traj": PlanningPairList = PlanningObj[1] StepLimbPair = PlanningPairList[len(PlanningPairList) - 1] StepNo = StepLimbPair[0] LimbNo = StepLimbPair[1] for i in range(0, LimbNo + 1): CandidateContacts_data = ContactDataLoader( SpecificPath, StepNo, i, "CandidateContacts") CandidateContactWeights_data = ContactDataLoader( SpecificPath, StepNo, i, "CandidateContactWeights") CandidateContacts_data, CandidateContactWeights_data = ContactDataRefine( CandidateContacts_data, CandidateContactWeights_data) WeightedContactDataUnPlot(vis, StepNo, i, CandidateContacts_data) TransitionDataUnplot(vis, StepNo, i) if VisMode == "Poly": vis.hide("ContactPolytope", True)
def GhostPlot(vis, NumberOfPoses, StepIndex, PoseSepIndex): FrameIndex = int(math.floor(StepIndex / PoseSepIndex)) - 1 if FrameIndex == -1: return for i in range(0, FrameIndex): vis.hide("Ghost" + str(i), hidden=False)
def TransitionDataUnplot(vis, StepNo, LimbNo): TransName = "Stage" + str(StepNo) + "LinkNo" + str(LimbNo) + "Path" vis.hide(TransName, True)
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) #or this line #traj_timed = trajectory.path_to_trajectory(traj2.configTrajectory().discretize(0.1),timing='sqrt-L2',speed=0.3) vis.animate("point",traj_timed) vis.spin(float('inf'))
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"
else: vis.setColor("qsoln",1,0,0,0.5) #initialize the next step from the last solved configuration qinit = qcollfree """ #debug printing for c in constraints: c.setx(qcollfree) distances = [c.minvalue(qcollfree) for c in constraints] for c in constraints: c.clearx() print("Distances",distances) """ for i in oldcps: vis.hide(i) oldcps = [] for i in xrange(len(cps)): for j in xrange(len(cps[i])): name = "cp(%d,%d)"%(i,j) vis.add(name,cps[i][j][:3]) vis.setColor(name,0,0,0,1) vis.hideLabel(name) oldcps.append(name) #let the goal wander around #q0 = qcollfree #let the seed wander around #qinit = qcollfree vis.unlock() t1 = time.time() vis.logPlot("timing","opt",t1-t0)
def WeightedContactDataUnPlot(vis, StepNo, LimbNo, OptimalContact_data): for i in range(OptimalContact_data.size / 3): ContactName = "Stage" + str(StepNo) + "LinkNo" + str( LimbNo) + "Point:" + str(i) vis.hide(ContactName, True)
vis.autoFitCamera() print "Starting visualization window..." #run the visualizer, which runs in a separate thread vis.setWindowTitle("Basic visualization test") vis.show() iteration = 0 while vis.shown(): vis.lock() #TODO: you may modify the world here. This line tests a sin wave. pt[2] = 1 + math.sin(iteration * 0.03) vis.unlock() #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..."