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..."
