q[i] = qmax[i]
traj.milestones.append(q)
save,traj.milestones = resource.edit("trajectory",traj.milestones,world=world)
vis.animate("robot",traj)
#pop up the window to show the trajectory
vis.spin(float('inf'))
if len(sys.argv)>1:
vis.animate("robot",None)
sim = Simulator(world)
sim.simulate(0)
trajectory.execute_path(traj.milestones,sim.controller(0))
vis.setWindowTitle("Simulating path using trajectory.execute_trajectory")
if vis.multithreaded():
#for some tricky Qt reason, need to sleep before showing a window again
#Perhaps the event loop must complete some extra cycles?
time.sleep(0.01)
vis.show()
t0 = time.time()
while vis.shown():
#print "Time",sim.getTime()
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print("Executing timed trajectory")
trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause')
vis.setItemConfig("robot",sim.controller(0).getCommandedConfig())
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.0))
t0 = t1
import klampt
from klampt.io import resource
from klampt import vis
from klampt.model.trajectory import Trajectory, RobotTrajectory
import sys
#set this to True to test multi-threaded visualization, False to test single-threaded
MULTITHREADED = vis.multithreaded()
def config_edit_template(world):
"""Shows how to edit Config, Configs, and Trajectory resources"""
config1 = resource.get("resourcetest1.config",
description="First config, always edited",
doedit=True,
editor='visual',
world=world)
print("Config 1:", config1)
config2 = resource.get("resourcetest1.config",
description="Trying this again...",
editor='visual',
world=world)
print("Config 2:", config2)
config3 = resource.get("resourcetest2.config",
description="Another configuration",
editor='visual',
world=world)
print("Config 3:", config3)
if config3 != None: