def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w,vp.h = 400,600
vis.setViewport(vp)
vis.dialog()
vp.w,vp.h = 640,480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def launch_xform_viewer(robotname):
"""Launches a very simple program that spawns an object from one of the
databases.
It launches a visualization of the mvbb decomposition of the object, and corresponding generated poses.
It then spawns a hand and tries all different poses to check for collision
"""
world = WorldModel()
robot = make_moving_base_robot(robotname, world)
set_moving_base_xform(robot, se3.identity()[0], se3.identity()[1])
xform = resource.get("default_initial_%s.xform" % robotname,
description="Initial hand transform",
default=se3.identity(),
world=world,
doedit=False)
print "Transform:", xform
program = XFormVisualizer(world, xform)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.01)
vis.kill()
return
def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None):
"""Launches a very simple program that spawns a box with dimensions specified in boxes_db.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform",
default=se3.identity(), world=world, doedit=False)
for box_dims, poses in box_db.db.items():
if world.numRigidObjects() > 0:
world.remove(world.rigidObject(0))
obj = make_box(world,
box_dims[0],
box_dims[1],
box_dims[2])
poses_filtered = []
R,t = obj.getTransform()
obj.setTransform(R, [0, 0, box_dims[2]/2.])
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p_T_h = np.array(se3.homogeneous(xform))
p_T_h[2][3] += 0.02
for pose in poses:
w_T_p = w_T_o.dot(pose)
w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h))
if CollisionTestPose(world, robot, obj, w_T_h_des_se3):
pose_pp = se3.from_homogeneous(pose)
t_pp = pose_pp[1]
q_pp = so3.quaternion(pose_pp[0])
q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]]
print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims
elif w_T_p[2][3] <= 0.:
print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table"
else:
poses_filtered.append(pose)
print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses"
# embed()
# 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)
program = FilteredMVBBTesterVisualizer(box_dims,
poses_filtered,
world,
p_T_h,
module,
box_db,
links_to_check)
vis.setPlugin(None)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.1)
return
def main():
print("============================================================")
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print("USAGE: simtest.py [world_file] [controller files (.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 multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def run_ex3():
world = WorldModel()
res = world.readFile("ex3_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.setWindowTitle("Pick and place test, use a/b/c/d to select target")
vis.pushPlugin(GLPickAndPlacePlugin(world))
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def run_ex2():
world = WorldModel()
res = world.readFile("ex2_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.pushPlugin(GLTransferPlanPlugin(world))
vis.setWindowTitle(
"Transfer plan test, press r/f to plan with right/forward target")
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def run_ex1():
world = WorldModel()
res = world.readFile("ex1_file.xml")
if not res: raise RuntimeError("Unable to load world file")
vis.add("world", world)
vis.setWindowTitle(
"Transit plan test, press l/r to plan with left/right arm")
vis.pushPlugin(GLTransitPlanPlugin(world))
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def run(self):
"""Standardized interface for running program"""
vp = vis.getViewport()
#Square screen
#vp.w,vp.h = 800,800
#For saving HD quality movies
vp.w, vp.h = 1024, 768
vp.clippingplanes = self.clippingplanes
vis.setViewport(vp)
#vis.run(program)
vis.setPlugin(self)
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
def run(self):
"""Standardized interface for running program"""
if KLAMPT_VERSION >= 0.7:
vp = vis.getViewport()
#Square screen
#vp.w,vp.h = 800,800
#For saving HD quality movies
vp.w, vp.h = 1024, 768
vp.clippingplanes = self.clippingplanes
vis.setViewport(vp)
#vis.run(program)
vis.setPlugin(self)
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
else:
#Square screen
#self.width,self.height = 800,800
#For saving HD quality movies
self.width, self.height = 1024, 768
GLBaseClass.run(self)
def launch_shelf(robotname, objects):
"""Launches the task 2 program that asks the robot to retrieve some set of objects
packed within a shelf.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
box = make_box(world, *box_dims)
shelf = make_shelf(world, *shelf_dims)
shelf.geometry().translate((0, shelf_offset, shelf_height))
rigid_objects = []
for objectset, objectname in objects:
object = make_object(objectset, objectname, world)
#TODO: pack in the shelf using x-y translations and z rotations
object.setTransform(
*se3.mul((so3.identity(), [0, shelf_offset, shelf_height +
0.01]), object.getTransform()))
rigid_objects.append(object)
xy_jiggle(world, rigid_objects, [shelf],
[-0.5 * shelf_dims[0], -0.5 * shelf_dims[1] + shelf_offset],
[0.5 * shelf_dims[0], 0.5 * shelf_dims[1] + shelf_offset], 100)
doedit = True
xform = resource.get("shelf/default_initial_%s.xform" % (robotname, ),
description="Initial hand transform",
default=robot.link(5).getTransform(),
world=world)
if not xform:
print "User quit the program"
return
set_moving_base_xform(robot, xform[0], xform[1])
#now the simulation is launched
program = GLSimulationProgram(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)
#controlfunc is now attached to control the robot
import shelf_controller
sim.setController(robot, shelf_controller.make(sim, hand, program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
while vis.shown():
time.sleep(0.1)
return
#this code manually updates the vis
vis.add("world", world)
vis.show()
t0 = time.time()
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
return
if not res:
raise RuntimeError("Unable to load model "+fn)
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"
raise
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i),controller)
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 launch_shelf(robotname,objects):
"""Launches the task 2 program that asks the robot to retrieve some set of objects
packed within a shelf.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname,world)
box = make_box(world,*box_dims)
shelf = make_shelf(world,*shelf_dims)
shelf.geometry().translate((0,shelf_offset,shelf_height))
rigid_objects = []
for objectset,objectname in objects:
object = make_object(objectset,objectname,world)
#TODO: pack in the shelf using x-y translations and z rotations
object.setTransform(*se3.mul((so3.identity(),[0,shelf_offset,shelf_height + 0.01]),object.getTransform()))
rigid_objects.append(object)
xy_jiggle(world,rigid_objects,[shelf],[-0.5*shelf_dims[0],-0.5*shelf_dims[1]+shelf_offset],[0.5*shelf_dims[0],0.5*shelf_dims[1]+shelf_offset],100)
doedit = True
xform = resource.get("shelf/default_initial_%s.xform"%(robotname,),description="Initial hand transform",default=robot.link(5).getTransform(),world=world)
if not xform:
print "User quit the program"
return
set_moving_base_xform(robot,xform[0],xform[1])
#now the simulation is launched
program = GLSimulationProgram(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)
#controlfunc is now attached to control the robot
import shelf_controller
sim.setController(robot,shelf_controller.make(sim,hand,program.dt))
#the next line latches the current configuration in the PID controller...
sim.controller(0).setPIDCommand(robot.getConfig(),robot.getVelocity())
#this code uses the GLSimulationProgram structure, which gives a little more control over the visualization
vis.setPlugin(program)
program.reshape(800,600)
vis.show()
while vis.shown():
time.sleep(0.1)
return
#this code manually updates the vis
vis.add("world",world)
vis.show()
t0 = time.time()
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
return
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)
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
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.show()
while vis.shown():
time.sleep(0.1)
print "Showing a dialog, back to normal..."
vis.setPlugin(None)
vis.dialog()
print "Showing again, back to normal..."
vis.setWindowTitle("Basic visualization test")
vis.show()
while vis.shown():
time.sleep(0.01)
print "Ending klampt.vis visualization."
vis.kill()
