def qt_template(world):
"""Runs a custom Qt frame around a visualization window"""
if not glinit.available('PyQt5'):
print(
"PyQt5 is not available on your system, try sudo apt-get install python-qt5"
)
return
#Qt objects must be explicitly deleted for some reason in PyQt5...
g_mainwindow = None
#All Qt functions must be called in the vis thread.
#To hook into that thread, you will need to pass a window creation function into vis.customUI.
def makefunc(gl_backend):
global g_mainwindow
g_mainwindow = MyQtMainWindow(gl_backend)
return g_mainwindow
vis.customUI(makefunc)
vis.add("world", world)
vis.setWindowTitle("Klamp't Qt test")
vis.spin(float('inf'))
vis.kill()
#Safe cleanup of all Qt objects created in makefunc.
#If you don't have this, PyQt5 complains about object destructors being called from the wrong thread
del g_mainwindow
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 edit_template(world):
"""Shows how to pop up a visualization window with a world in which the robot configuration and a transform can be edited"""
#add the world to the visualizer
vis.add("world",world)
xform = se3.identity()
vis.add("transform",xform)
robotPath = ("world",world.robot(0).getName()) #compound item reference: refers to robot 0 in the world
vis.edit(robotPath)
vis.edit("transform")
#This prints how to get references to items in the visualizer
print("Visualization items:")
vis.listItems(indent=2)
vis.setWindowTitle("Visualization editing test")
if not MULTITHREADED:
vis.loop(setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#TODO: you may modify the world here.
vis.unlock()
time.sleep(0.01)
print("Resulting configuration",vis.getItemConfig(robotPath))
print("Resulting transform (config)",vis.getItemConfig("transform")) # this is a vector describing the item parameters
xform = list(xform) #convert se3 element from tuple to list
config.setConfig(xform,vis.getItemConfig("transform"))
print("Resulting transform (se3)",xform)
#quit the visualization thread nicely
vis.kill()
def plugin_template(world):
"""Demonstrates the GLPluginInterface functionality"""
#create a subclass of GLPluginInterface
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin) #put the plugin on top of the standard visualization functionality.
#vis.setPlugin(plugin) #use this to completely replace the standard visualization functionality with your own.
vis.add("world",world)
vis.setWindowTitle("GLPluginInterface template")
#run the visualizer
if not MULTITHREADED:
def callback(plugin=plugin):
if plugin.quit:
vis.show(False)
vis.loop(callback=callback,setup=vis.show)
else:
#if plugin.quit is True
vis.show()
while vis.shown() and not plugin.quit:
vis.lock()
#TODO: you may modify the world here
vis.unlock()
#changes to the visualization must be done outside the lock
time.sleep(0.01)
if plugin.quit:
#if you want to do other stuff after the window quits, the window needs to be hidden
vis.show(False)
print("Waiting for 2 s...")
time.sleep(2.0)
#quit the visualization thread nicely
vis.kill()
def edit_camera_xform(world_fn, xform=None, title=None):
"""Visual editor of the camera position
"""
world = WorldModel()
world.readFile(world_fn)
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
if xform is not None:
sensing.set_sensor_xform(sensor, xform)
vis.createWindow()
if title is not None:
vis.setWindowTitle(title)
vis.resizeWindow(1024, 768)
vis.add("world", world)
vis.add("sensor", sensor)
vis.add("sensor_xform", sensing.get_sensor_xform(sensor, robot))
vis.edit("sensor_xform")
def update_sensor_xform():
sensor_xform = vis.getItemConfig("sensor_xform")
sensor_xform = sensor_xform[:9], sensor_xform[9:]
sensing.set_sensor_xform(sensor, sensor_xform)
vis.loop(callback=update_sensor_xform)
sensor_xform = vis.getItemConfig("sensor_xform")
return sensor_xform[:9], sensor_xform[9:]
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 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 viewport_template(world):
"""Changes the parameters of the viewport and main window"""
#add the world to the visualizer
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
#this auto-sizes the camera
vis.autoFitCamera()
vis.setWindowTitle("Viewport modification test")
vis.spin(float('inf'))
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 animation_template(world):
"""Shows how to animate a robot."""
#first, build a trajectory with 10 random configurations
robot = world.robot(0)
times = range(10)
milestones = []
for t in times:
robot.randomizeConfig()
milestones.append(robot.getConfig())
traj = trajectory.RobotTrajectory(robot, times, milestones)
vis.add("world", world)
robotPath = ("world", world.robot(0).getName()
) #compound item reference: refers to robot 0 in the world
#we're also going to visualize the end effector trajectory
#eetraj = traj.getLinkTrajectory(robot.numLinks()-1,0.05)
#vis.add("end effector trajectory",eetraj)
#uncomment this to automatically visualize the end effector trajectory
vis.add("robot trajectory", traj)
vis.setAttribute("robot trajectory", "endeffectors", [13, 20])
vis.setWindowTitle("Animation test")
MANUAL_ANIMATION = False
if not MANUAL_ANIMATION:
#automatic animation, just call vis.animate
vis.animate(robotPath, traj)
if not MULTITHREADED:
#need to set up references to function-local variables manually, and the easiest way is to use a default argument
def callback(robot=robot):
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
pass
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def basic_template(world):
"""Shows how to pop up a visualization window with a world"""
#add the world to the visualizer
vis.add("world", world)
#adding a point
vis.add("point", [1, 1, 1])
vis.setColor("point", 0, 1, 0)
vis.setAttribute("point", "size", 5.0)
#adding lines is currently not super convenient because a list of lists is treated as
#a Configs object... this is a workaround to force the vis module to treat it as a polyline.
vis.add("line", trajectory.Trajectory([0, 1], [[0, 0, 0], [1, 1, 1]]))
vis.setAttribute("line", "width", 1.0)
sphere = klampt.GeometricPrimitive()
sphere.setSphere([1.5, 1, 1], 0.2)
vis.add("sphere", sphere)
vis.setColor("sphere", 0, 0, 1, 0.5)
box = klampt.GeometricPrimitive()
box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2])
g = klampt.Geometry3D(box)
vis.add("box", g)
vis.setColor("box", 0, 0, 1, 0.5)
vis.setWindowTitle("Basic visualization test")
if not MULTITHREADED:
print("Running vis loop in single-threaded mode with vis.loop()")
#single-threaded code
def callback():
#TODO: you may modify the world here.
pass
vis.loop(setup=vis.show, callback=callback)
else:
print("Running vis loop in multithreaded mode")
#multithreaded code
vis.show()
while vis.shown():
vis.lock()
#TODO: you may modify the world here. Do not modify the internal state of any
#visualization items outside of the lock
vis.unlock()
#outside of the lock you can use any vis.X functions, including vis.setItemConfig()
#to modify the state of objects
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def visualize(self):
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.unlock()
while vis.shown():
time.sleep(0.01)
vis.kill()
def debug_stable_faces(obj, faces):
from klampt import vis, Geometry3D, GeometricPrimitive
from klampt.math import se3
import random
vis.createWindow()
obj.setTransform(*se3.identity())
vis.add("object", obj)
for i, f in enumerate(faces):
gf = GeometricPrimitive()
gf.setPolygon(np.stack(f).flatten())
color = (1, 0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(),
0.5)
vis.add("face{}".format(i),
Geometry3D(gf),
color=color,
hide_label=True)
vis.setWindowTitle("Stable faces")
vis.dialog()
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def visualize_pc_in_klampt_vis(self, pcloud_fname):
title = pcloud_fname + " klampt world"
vis_window_id = vis.createWindow(title)
vis.setWindowTitle(title)
world = WorldModel()
vis.add("world", world)
pcd = o3d.io.read_point_cloud(pcloud_fname)
print(pcd)
pc_xyzs = np.asarray(pcd.points)
pc_xyzs_as_list = pc_xyzs.flatten().astype("float")
# pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0])
pcloud = PointCloud()
pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list)
print(pcloud.numPoints())
vis.add("pcloud", pcloud)
# vis.setColor("pcloud", 0, 0, 1, a=1)
# vis.setAttribute("p1", "size", 5.0)
box = klampt.GeometricPrimitive()
box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2])
g = klampt.Geometry3D(box)
vis.add("box", g)
vis.setColor("box", 0, 0, 1, 0.5)
coordinates.setWorldModel(world)
vis.add("coordinates", coordinates.manager())
vis.show()
while vis.shown():
vis.lock()
vis.unlock()
time.sleep(0.01)
vis.kill()
#vis.autoFitCamera()
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
collisionFlag = False
collisionChecker = collide.WorldCollider(world)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
print("Starting.....")
source = [0, 0, 0, 0, 0, 0]
goal = [3, -3.5, 0, 0, 0, 0]
sourceCollisionFlag = False
goalCollisionFlag = False
successFlag = False
vis.lock()
robot.setConfig(source)
vis.unlock()
if checkCollision():
sourceCollisionFlag = True
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.setWindowTitle("Klamp't Resource Browser")
vis.run()
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
traj.times.append(len(traj.times)*0.5)
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))
def mousefunc(self, button, state, x, y):
print("Mouse button", button, "state", state, "at point", x, y)
def motionfunc(self, x, y, dx, dy):
if 'shift' in self.modifiers():
self.q[2] = float(y) / 400
self.q[3] = float(x) / 400
self.world.robot(0).setConfig(self.q)
return True
return False
if __name__ == "__main__":
print("""================================================================
mouse_capture.py: A simple program where the mouse motion, when
shift-clicking, gets translated into joint values for an animated robot.
========================================================================
""")
world = klampt.WorldModel()
res = world.readFile("../../data/tx90blocks.xml")
if not res:
raise RuntimeError("Unable to load world")
plugin = MouseCapture(world)
vis.add("world", world)
vis.pushPlugin(plugin)
vis.setWindowTitle("mouse_capture.py")
vis.spin(float('inf')) #shows the window
vis.kill()
vis.setColor(("world",world.robot(0).getName()),1,1,0,1)
coordinates.setRobotModel(robot)
eenames = [robot.link(e).getName() for e in endeffectors]
eeobjectives = []
for e in endeffectors:
f = coordinates.frame(robot.link(e).getName())
fw = coordinates.addFrame(robot.link(e).getName()+"_tgt",robot.link(e).getTransform())
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)
glVertex3f(V[i][0], V[i][1], V[i][2])
glVertex3f(V[j][0], V[j][1], V[j][2])
glEnd()
glDisable(GL_BLEND)
glEnable(GL_LIGHTING)
return True
if __name__ == '__main__':
if problem == "3":
from klampt import WorldModel
from klampt import vis
world = WorldModel()
world.readFile("../../data/tx90cuptable.xml")
plugin = RigidObjectCSpacePlugin(world, world.rigidObject(0))
vis.setWindowTitle("Rigid object planning")
vis.run(plugin)
exit()
space = None
start = None
goal = None
if problem == "1":
space = CircleObstacleCSpace()
space.addObstacle(Circle(0.5, 0.5, 0.36))
start = (0.06, 0.5)
goal = (0.94, 0.5)
elif problem == "2":
space = RigidBarCSpace()
space.addObstacle(Circle(0.5, 0.5, 0.4))
start = (0.1, 0.1, 0.0)
goal = (0.9, 0.9, 6.20)
#vis.autoFitCamera()
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
collisionFlag = False
collisionChecker = collide.WorldCollider(world)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle(
"Visualization for kinematic simulation of Unicycle Robot")
simTime = 3000
startTime = time.time()
oldTime = startTime
start = [-1, -1, math.pi / 4]
goal = [2.5, 2, math.pi / 4]
goalstartflag = False
colli_flag, obsName = collisionchecking(goal, obs)
if colli_flag:
strng = "Goal collides with " + obsName
print(strng)
goalstartflag = True
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
colli_flag, obsName = collisionchecking(goal, obs)
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 vis_world(self):
world = WorldModel()
self.rposer = None
res = world.readFile(self.world_file)
if not res:
raise RuntimeError("Unable to load terrain model")
res = world.readFile(self.robot_file)
if not res:
raise RuntimeError("Unable to load robot model")
vis.createWindow(self.worldname)
vis.setWindowTitle(self.worldname + " visualization")
vis.add("world", world)
vp = vis.getViewport()
# vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.autoFitCamera()
vis.show()
q = [
0,
0.0,
0.5, # torso x y z
0,
0,
0, # torso roll pitch yaw
0,
0,
0,
-0.785398,
-1.5707,
-1.5707,
0.785398,
0, # fr
0,
0,
0,
0.785398,
1.5707,
1.5707,
-0.785398,
0, # br
0,
0,
0,
-0.785398,
-1.5707,
-1.5707,
0.785398,
0, # bl
0,
0,
0,
0.785398,
1.5707,
1.5707,
-0.785398,
0, # fl
]
world.robot(0).setConfig(q)
#Put your control handler here
#right now, just sets g to an oscillation between 0 and 199
#TODO: build a BaseController that outputs qcmd to the emulator
g = int(self.sim.getTime()*50.0)
maxval = 120
if int(g/maxval)%2 == 1:
g = maxval-1 - g%maxval
else:
g = g % maxval
#print g
g = [g,g,g]
self.robotiqEmulator.send_command(g,scissor=30)
if __name__ == "__main__":
print("""robotiqtest.py: A program to test the behavior of the RobotiQ
emulator. Right now it just opens and closes the gripper repeatedly.
Press s to toggle simulation.""")
world = klampt.WorldModel()
if not world.readFile('robotiq.xml'):
print("robotiq.xml couldn't be read, exiting")
exit(1)
viewer = MyGLViewer(world)
vis.setWindowTitle("Robotiq gripper test")
vis.run(viewer)
#add the world elements individually to the visualization
vis.add("robot", robot)
for i in range(1, world.numRobots()):
vis.add("robot" + str(i), world.robot(i))
for i in range(world.numRigidObjects()):
vis.add("rigidObject" + str(i), world.rigidObject(i))
for i in range(world.numTerrains()):
vis.add("terrain" + str(i), world.terrain(i))
#if you want to just see the robot in a pop up window...
if DO_SIMPLIFY and DEBUG_SIMPLIFY:
print("#########################################")
print("Showing the simplified robot")
print("#########################################")
vis.setWindowTitle("Simplified robot")
vis.dialog()
#Automatic construction of space
if not CLOSED_LOOP_TEST:
if not MANUAL_SPACE_CREATION:
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-3,
movingSubset='all')
else:
#Manual construction of space
collider = WorldCollider(world)
space = robotcspace.RobotCSpace(robot, collider)
space.eps = 1e-3
space.setup()
robotiq_140.maximum_span = 0.140 - 0.01
robotiq_140.minimum_span = 0
robotiq_140.open_config = [0]*6
robotiq_140.closed_config = [0.7,0.7,0.7,0.7,0.7,0.7]
robotiq_85_kinova_gen3 = GripperInfo.mounted(robotiq_85,os.path.join(data_dir,"robots/kinova_with_robotiq_85.urdf"),"gripper:Link_0","robotiq_85-kinova_gen3")
robotiq_140_trina_left = GripperInfo.mounted(robotiq_140,os.path.join(data_dir,"robots/TRINA.urdf"),"left_gripper:base_link","robotiq_140-trina-left")
robotiq_140_trina_right = GripperInfo.mounted(robotiq_140,os.path.join(data_dir,"robots/TRINA.urdf"),"right_gripper:base_link","robotiq_140-trina-right")
if __name__ == '__main__':
from klampt import vis
import sys
if len(sys.argv) == 1:
grippers = [i for i in GripperInfo.all_grippers]
print("ALL GRIPPERS",grippers)
else:
grippers = sys.argv[1:]
for i in grippers:
g = GripperInfo.get(i)
print("SHOWING GRIPPER",i)
g.add_to_vis()
vis.setWindowTitle(i)
def setup():
vis.show()
def cleanup():
vis.show(False)
vis.clear()
vis.loop(setup=setup,cleanup=cleanup)
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
