description="Press OK to add waypoint, cancel to stop")
if save:
if False and not space.feasible(q):
print("Configuration is infeasible. Failures:")
print(" ", space.cspace.feasibilityFailures(q))
print("Please pick another")
else:
if cindex >= len(configs):
configs.append(q)
else:
configs[cindex] = q
cindex += 1
else:
break
if cindex == 0:
vis.kill()
exit(0)
configs = configs[:cindex]
resource.set("planningtest.configs", configs)
if CLOSED_LOOP_TEST:
#need to project those onto the manifold
for i, q in enumerate(configs):
configs[i] = space.solveConstraints(q)
resource.edit(
"IK solved configs",
configs,
"Configs",
description="These configurations try to solve the IK constraint",
world=world)
def main():
print(
"================================================================================"
)
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print(
"USAGE: klampt_sim [world_file] [trajectory (.traj) or controller (.py)]"
)
print(
"================================================================================"
)
if len(sys.argv) <= 1:
exit()
world = WorldModel()
#load up any world items, control modules, or paths
control_modules = []
for fn in sys.argv[1:]:
path, base = os.path.split(fn)
mod_name, file_ext = os.path.splitext(base)
if file_ext == '.py' or file_ext == '.pyc':
sys.path.append(os.path.abspath(path))
mod = importlib.import_module(mod_name, base)
control_modules.append(mod)
elif file_ext == '.path':
control_modules.append(fn)
else:
res = world.readFile(fn)
if not res:
print("Unable to load model " + fn)
print("Quitting...")
sys.exit(1)
viewer = MyGLViewer(world)
for i, c in enumerate(control_modules):
if isinstance(c, str):
sys.path.append(os.path.abspath("../control"))
import trajectory_controller
#it's a path file, try to load it
controller = trajectory_controller.make(world.robot(i), c)
else:
try:
maker = c.make
except AttributeError:
print("Module", c.__name__, "must have a make() method")
print("Quitting...")
sys.exit(1)
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i), controller)
vis.setWindowTitle("Klamp't Simulation Tester")
if SPLIT_SCREEN_TEST:
viewer2 = MyGLViewer(world)
vis.setPlugin(viewer)
vis.addPlugin(viewer2)
viewer2.window.broadcast = True
vis.show()
while vis.shown():
time.sleep(0.01)
vis.kill()
else:
vis.run(viewer)
def test_grasp(robotname, object_set, objectname, xyzrpy):
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
m_object = make_object(object_set, objectname, world)
#this sets the initial condition for the simulation
xform = xyzrpy_to_xform(xyzrpy)
set_moving_base_xform(robot, xform[0], xform[1])
#now the simulation is launched
program = GLSimulationPlugin(world)
sim = program.sim
#setup some simulation parameters
visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
#create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
#emulator takes the robot index (0), start link index (6), and start driver index (6)
hand = module.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
#the result of simple_controller.make() is now attached to control the robot
import simple_controller
sim.setController(robot, simple_controller.make(sim, hand, delta))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity())
#this code manually updates the visualization
vis.add("world", world)
vis.show()
t0 = time.time()
count = 0
initial_z = m_object.getTransform()[1][2]
while vis.shown():
vis.lock()
sim.simulate(0.01)
sim.updateWorld()
vis.unlock()
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.001))
t0 = t1
count += 1
if count == 800:
break
if m_object.getTransform()[1][2] > initial_z + 0.05:
print "grasp successful"
vis.kill()
return True
print "grasp failed"
vis.kill()
return False
def simulation_template(world):
"""Runs a custom simulation plugin"""
viewer = MyGLSimulationViewer(world)
vis.run(viewer)
vis.kill()
def main():
print("""
===============================================================================
A program to quickly browse Klamp't objects.
USAGE: %s [item1 item2 ...]
where the given items are world, robot, terrain, object, or geometry files. Run
it without arguments
%s
for an empty reference world. You may add items to the reference world using
the `Add to World` button. If you know what items to use in the reference
world, run it with
%s world.xml
or
%s item1 item2 ...
where the items are world, robot, terrain, object, or geometry files.
===============================================================================
""" % (sys.argv[0], sys.argv[0], sys.argv[0], sys.argv[0]))
#must be explicitly deleted for some reason in PyQt5...
g_browser = None
def makefunc(gl_backend):
global g_browser
browser = ResourceBrowser(gl_backend)
g_browser = browser
dw = QtWidgets.QDesktopWidget()
x = dw.width() * 0.8
y = dw.height() * 0.8
browser.setFixedSize(x, y)
for fn in sys.argv[1:]:
res = browser.world.readFile(fn)
if not res:
print("Unable to load model", fn)
print("Quitting...")
sys.exit(1)
print("Added", fn, "to world")
if len(sys.argv) > 1:
browser.emptyVisPlugin.add("world", browser.world)
return browser
vis.customUI(makefunc)
vis.show()
vis.setWindowTitle("Klamp't Resource Browser")
vis.spin(float('inf'))
vis.kill()
del g_browser
return
#this code below is incorrect...
app = QtWidgets.QApplication(sys.argv)
browser = ResourceBrowser()
for fn in sys.argv[1:]:
res = browser.world.readFile(fn)
if not res:
print("Unable to load model", fn)
print("Quitting...")
sys.exit(1)
print("Added", fn, "to world")
if len(sys.argv) > 1:
browser.emptyVisPlugin.add("world", browser.world)
dw = QtWidgets.QDesktopWidget()
x = dw.width() * 0.8
y = dw.height() * 0.8
browser.setFixedSize(x, y)
#browser.splitter.setWindowState(QtCore.Qt.WindowMaximized)
browser.setWindowTitle("Klamp't Resource Browser")
browser.show()
# Start the main loop.
res = app.exec_()
return res
class Simulation:
def __init__(self, world):
self.world = world
self.robot = None
self.collision_checker = None
self.step_size = 0.03
self.actions = np.radians(np.arange(180, -180, -45))
# self.actions = np.array([np.pi, np.pi/2, 0, -np.pi/2])
self.actions = np.array([-np.pi, 0, np.pi / 2, -np.pi / 2])
self.reward_system = {
"collision": -1,
"boundary": -100,
"free": -0.1,
"goal": 200
}
self.distance_factor = 0.05
self.start_pc = None
self.goal_pc = None
self.goal_range = 0.1 # size of the goal
self.terrain_limit = [1, 1, 0]
self.vis = None
def reset(self, spc):
# This function resets the robot to the start position again
self.robot.setConfig(spc)
def check_collision(self):
for iT in range(self.world.numTerrains()):
collRT0 = self.collision_checker.robotTerrainCollisions(
self.world.robot(0), iT)
collision_flag = False
for i, j in collRT0:
collision_flag = True
strng = "Robot collides with " + j.getName()
vis.addText("textCol", strng)
vis.setColor("textCol", 1, 0, 0)
break
for iR in range(self.world.numRigidObjects()):
collRT2 = self.collision_checker.robotObjectCollisions(
self.world.robot(0), iR)
for i, j in collRT2:
if j.getName() != "goal":
collision_flag = True
strng = self.world.robot(
0).getName() + " collides with " + j.getName()
vis.addText("textCol", strng)
vis.setColor("textCol", 1, 0, 0)
if collision_flag:
vis.addText("textCol", "Collision")
vis.setColor("textCol", 1, 0.0, 0.0)
if not collision_flag:
vis.addText("textCol", "No collision")
vis.setColor("textCol", 0.4660, 0.6740, 0.1880)
return collision_flag
def check_goal_distance(self, cpc):
"""
This method checks the distance of the gaol from the current position of thr robot
:param cpc: current position coordinate of the robot
:return:
"""
if type(cpc) is np.ndarray:
distance = np.sqrt(
np.square(cpc[0, 0] - self.goal_pc[0]) +
np.square(cpc[0, 1] - self.goal_pc[1]))
if distance < self.goal_range:
return 0
else:
# return the distance from the goal
return distance
else:
distance = np.sqrt(
np.square(cpc[0] - self.goal_pc[0]) +
np.square(cpc[1] - self.goal_pc[1]))
if distance < self.goal_range:
return 0
else:
# return the distance from the goal
return distance
def create(self, start_pc, goal_pc):
"""
This method cretes the simulation
:param start_pc: robot's initial position coordinate
:param goal_pc: goal position coordinate
:return:
"""
print "Creating the Simulator"
object_dir = "/home/jeet/PycharmProjects/DeepQMotionPlanning/"
self.start_pc = start_pc
self.goal_pc = goal_pc
coordinates.setWorldModel(self.world)
getDoubleRoomDoor(self.world, 8, 8, 1)
builder = Builder(object_dir)
# Create a goal cube
n_objects = 1
width = 0.1
depth = 0.1
height = 0.1
x = goal_pc[0]
y = goal_pc[1]
z = goal_pc[2] / 2
thickness = 0.005
color = (0.2, 0.6, 0.3, 1.0)
builder.make_objects(self.world, n_objects, "goal", width, depth,
height, thickness, self.terrain_limit, color,
self.goal_pc)
# Create a obstacle cube
n_objects = 4
width = 0.2
depth = 0.2
height = 0.2
x = 2.3
y = 0.8
z = goal_pc[2] / 2
thickness = 0.001
color = (0.8, 0.2, 0.2, 1.0)
builder.make_objects(self.world, n_objects, "rigid", width, depth,
height, thickness, self.terrain_limit, color)
self.vis = vis
vis.add("world", self.world)
# Create the view port
vp = vis.getViewport()
vp.w, vp.h = 1200, 900
vp.x, vp.y = 0, 0
vis.setViewport(vp)
# Create the robot
self.robot = sphero6DoF(self.world.robot(0), "", None)
self.robot.setConfig(start_pc)
# Create the axis representation
# vis.add("WCS", [so3.identity(), [0, 0, 0]])
# vis.setAttribute("WCS", "size", 24)
# Add text messages component for collision check and robot position
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
vis.addText("textStep", "Steps: ")
vis.setAttribute("textStep", "size", 24)
vis.addText("textGoalDistance", "Goal Distance: ")
vis.setAttribute("textGoalDistance", "size", 24)
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
self.collision_checker = collide.WorldCollider(self.world)
vis.setWindowTitle("Simulator")
vis.show()
return vis, self.robot, self.world
def get_current_state(self):
"""
This method returns teh current configuration of the robot
:return:
"""
config = self.robot.getConfig()
state = np.array([[config[0], config[1]]])
return state
def get_next_state(self, action, step_count, query=False):
"""
This method implements the take action functionality by asking the robot to take the action, and return the
next robot configuration
:param action: action to take
:param step_count: Number of steps taken by the robot so far
:param query: A boolean variable denotes weather it slearning phase or query phase, if it is a query phase
add a delay for better visualization
:return:
"""
pc = self.robot.getConfig()
old_pc = copy.deepcopy(pc)
# Compute the new positions
pc[0] = pc[0] + self.step_size * np.cos(self.actions[action])
pc[1] = pc[1] + self.step_size * np.sin(self.actions[action])
pc[2] = 0.1
pc[3] = 0
pc[4] = 0
pc[5] = 0
self.robot.setConfig(pc)
boundary_reached = False
if np.abs(pc[0]) > self.terrain_limit[0] or np.abs(
pc[1]) > self.terrain_limit[1]:
boundary_reached = True
q2f = ['{0:.2f}'.format(elem) for elem in pc]
label = "Steps: " + str(step_count)
vis.addText("textStep", label)
cpc = self.robot.getConfig()
# compute the distance
distance = self.check_goal_distance(cpc)
label_goal = "Goal Distance: " + str(distance)
vis.addText("textGoalDistance", label_goal)
label = "Robot configuration: " + str(q2f)
vis.addText("textConfig", label)
collision = self.check_collision()
goal_reached = False
# Update the robot positions
if boundary_reached:
self.robot.setConfig(old_pc)
reward = self.reward_system['boundary']
print("Hit Boundary", reward)
pc = old_pc
elif collision:
self.robot.setConfig(old_pc)
reward = self.reward_system['collision']
print("Collision", reward)
# Don't let it cross the boundary
pc = old_pc
elif distance == 0:
reward = self.reward_system['goal']
goal_reached = True
else:
reward = self.reward_system['free'] * distance
# send only the x and y position
state = np.array([[pc[0], pc[1]]])
if query:
time.sleep(0.002)
return reward, state, goal_reached
def run(self):
while vis.shown():
vis.lock()
vis.unlock()
# changes to the visualization must be done outside the lock
vis.clearText()
print "Ending klampt.vis visualization."
vis.kill()
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 load_pcloud(self, save_file):
"""
Converts a geometry to another type, if a conversion is available. The
interpretation of param depends on the type of conversion, with 0
being a reasonable default.
Available conversions are:
PointCloud -> TriangleMesh, if the point cloud is structured. param is the threshold
for splitting triangles by depth discontinuity, by default infinity.
"""
long_np_cloud = np.fromfile(save_file)
print(long_np_cloud.shape, ": shape of long numpy cloud")
num_points = long_np_cloud.shape[0] / 3
np_cloud = np.zeros(shape=(num_points, 3))
pcloud = klampt.PointCloud()
scaling_factor = 0.1
points = []
xs = []
ys = []
zs = []
for x in range(num_points):
i = x * 3
x_val = long_np_cloud[i] * scaling_factor
y_val = long_np_cloud[i + 1] * scaling_factor
z_val = long_np_cloud[i + 2] * scaling_factor
np_cloud[x][0] = x_val
np_cloud[x][1] = y_val
np_cloud[x][2] = z_val
xs.append(x_val)
ys.append(y_val)
zs.append(z_val)
points.append(np_cloud[x])
points.sort(key=lambda tup: tup[2])
x_sorted = sorted(xs) # sorted
y_sorted = sorted(ys) # sorted
z_sorted = sorted(zs) # sorted
xfit = stats.norm.pdf(x_sorted, np.mean(x_sorted), np.std(x_sorted))
yfit = stats.norm.pdf(y_sorted, np.mean(y_sorted), np.std(y_sorted))
zfit = stats.norm.pdf(z_sorted, np.mean(z_sorted), np.std(z_sorted))
# plot with various axes scales
plt.figure(1)
# linear
plt.subplot(221)
plt.plot(x_sorted, xfit)
plt.hist(x_sorted, normed=True)
plt.title("X values")
plt.grid(True)
plt.subplot(222)
plt.plot(y_sorted, yfit)
plt.hist(y_sorted, normed=True)
plt.title("Y values")
plt.grid(True)
plt.subplot(223)
plt.plot(z_sorted, zfit)
plt.hist(z_sorted, normed=True)
plt.title("Z values")
plt.grid(True)
# Format the minor tick labels of the y-axis into empty strings with
# `NullFormatter`, to avoid cumbering the axis with too many labels.
plt.gca().yaxis.set_minor_formatter(NullFormatter())
# Adjust the subplot layout, because the logit one may take more space
# than usual, due to y-tick labels like "1 - 10^{-3}"
plt.subplots_adjust(top=0.92,
bottom=0.08,
left=0.10,
right=0.95,
hspace=0.25,
wspace=0.35)
# plt.show()
median_z = np.median(zs)
threshold = 0.25 * scaling_factor
for point in points:
if np.fabs(point[2] - median_z) < threshold:
pcloud.addPoint(point)
print(pcloud.numPoints(), ": num points")
# Visualize
pcloud.setSetting("width", "3")
pcloud.setSetting("height", str(len(points) / 3))
g3d_pcloud = Geometry3D(pcloud)
mesh = g3d_pcloud.convert("TriangleMesh", 0)
world = robotsim.WorldModel()
vis.add("world", world)
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("PointCloud World")
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.autoFitCamera()
vis.show()
vis.lock()
vis.add("mesh", mesh)
vis.unlock()
while vis.shown():
time.sleep(0.01)
vis.kill()
print("done")
def endEnv(self):
vis.clearText()
print "Ending klampt.vis visualization."
vis.kill()
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()
dataset,objname = o.split('/',2)
try:
index = int(objname)
objname = objects[dataset][index]
except:
pass
shelved.append((dataset,objname))
else:
#format: dataset/object
for o in sys.argv[2:]:
dataset,objname = o.split('/',2)
try:
index = int(objname)
objname = objects[dataset][index]
except:
pass
shelved.append((dataset,objname))
launch_shelf(robot,shelved)
else:
#just plan grasping
try:
index = int(sys.argv[2])
objname = objects[dataset][index]
except IndexError:
index = random.randint(0,len(objects[dataset])-1)
objname = objects[dataset][index]
except ValueError:
objname = sys.argv[2]
launch_simple(robot,dataset,objname)
vis.kill()
def main():
print(
"================================================================================"
)
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print(
"USAGE: klampt_sim [world_file] [trajectory (.traj/.path) or controller (.py)]"
)
print()
print(
" Try: klampt_sim athlete_plane.xml motions/athlete_flex_opt.path "
)
print(" [run from Klampt-examples/data/]")
print(
"================================================================================"
)
if len(sys.argv) <= 1:
exit()
world = WorldModel()
#load up any world items, control modules, or paths
control_modules = []
for fn in sys.argv[1:]:
path, base = os.path.split(fn)
mod_name, file_ext = os.path.splitext(base)
if file_ext == '.py' or file_ext == '.pyc':
sys.path.append(os.path.abspath(path))
mod = importlib.import_module(mod_name, base)
control_modules.append(mod)
elif file_ext == '.path':
control_modules.append(fn)
else:
res = world.readFile(fn)
if not res:
print("Unable to load model " + fn)
print("Quitting...")
sys.exit(1)
viewer = MyGLViewer(world)
for i, c in enumerate(control_modules):
if isinstance(c, str):
from klampt.control.blocks import trajectory_tracking
from klampt.model.trajectory import RobotTrajectory
from klampt.io import loader
traj = loader.load('Trajectory', c)
rtraj = RobotTrajectory(world.robot(i), traj.times,
traj.milestones)
#it's a path file, try to load it
controller = trajectory_tracking.TrajectoryPositionController(
rtraj)
else:
try:
maker = c.make
except AttributeError:
print("Module", c.__name__, "must have a make() method")
print("Quitting...")
sys.exit(1)
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i), controller)
vis.setWindowTitle("Klamp't Simulation Tester")
if SPLIT_SCREEN_TEST:
viewer2 = MyGLViewer(world)
vis.setPlugin(viewer)
vis.addPlugin(viewer2)
viewer2.window.broadcast = True
vis.show()
while vis.shown():
time.sleep(0.01)
vis.kill()
else:
vis.run(viewer)
