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)
