"""Klamp't visualization routines. See
`vistemplate.py in Klampt-examples <https://github.com/krishauser/Klampt-examples/blob/master/Python3/demos/vis_template.py>`_
for an example of how to run this module.
OVERVIEW
--------
The visualization module lets you draw most Klamp't objects in a 3D world
using a simple interface. It also lets you customize the GUI using Qt
widgets, OpenGL drawing, and keyboard/mouse intercept routines.
Main features include:
- Simple interface to modify the visualization
- Simple interface to animate and render trajectories
- Simple interface to edit certain Klamp't objects (configurations, points,
transforms)
- Simple interface to drawing text and text labels, and drawing plots
- Multi-window, multi-viewport support
- Automatic camera setup
- Unified interface to PyQt, GLUT, IPython, and HTML backends.
- PyQT is the backend with the fullest amount of features.
- GLUT loses resource editing and advanced windowing functionality.
- IPython loses plugins, resource editing, custom drawing, and advanced
windowing functionality.
- HTML loses plugins, resource editing, custom drawing, and advanced
windowing functionality.
The resource editing functionality in the :mod:`klampt.io.resource` module
(which uses the widgets in :mod:`klampt.vis.editors`) use this module as well.
INSTRUCTIONS
-------------------
Basic use of the vis module is fairly straightforward:
0. (optional) Configure the rendering backend.
1. Add things to the visualization scene with ``vis.add(name,thing)``. Worlds,
geometries, points, transforms, trajectories, contact points, and more can
be added in this manner.
2. Modify the appearance of things using modifier calls like
``vis.setColor(name,r,g,b,a)``.
3. Launch windows and/or visualization thread (OpenGL or IPython modes)
4. Continue adding, modifying, and removing things as you desire.
More advanced functions allow you to dynamically launch multiple windows,
capture user input, and embed the visualization into Qt windows.
The default scene manager lets you set up and modify the visualization scene
(Steps 1 and 2). These just mirror the methods in
:class:`VisualizationScene`, which is how the default scene manager is
implemented. See Klampt-examples/Python/demos/vistemplate.py for more
examples.
The vis module provides some basic methods for capturing user interaction.
To add other user interaction functionality, you may create a
:class:`~klampt.vis.glinterface.GLPluginInterface` subclass to add
functionality on top of the default visualization world. To do so, call
``vis.pushPlugin(plugin)``. Note that custom rendering (the ``display()``
and ``display_screen()`` methods) are only available with the OpenGL
rendering backend.
Only one rendering backend can be chosen during the lifetime of your process,
and each backend has its own quirks with regards to window launching and
configuration. We'll describe the different options below.
OpenGL (PyQt, GLUT)
~~~~~~~~~~~~~~~~~~~~
OpenGL-based visualizations use either PyQt or GLUT to handle windowing and are
used by default. They run in the current process, have the best performance,
and offer the richest set of features.
Quick start
^^^^^^^^^^^^^^^
- To show the visualization and quit when the user closes the window::
vis.run()
- To show the visualization and return when the user closes the window::
vis.dialog()
... do stuff afterwards ...
vis.kill() #cleanup, not strictly needed but good practice
- To show the visualization and run a script alongside it until the user
closes the window (multithreaded mode)::
vis.show()
while vis.shown():
vis.lock()
... do stuff ...
[to exit the loop call vis.show(False)]
vis.unlock()
time.sleep(dt)
... do stuff afterwards ...
vis.kill() #cleanup, not strictly needed but good practice
- To show the visualization and run python commands until the user closes
the window (single-threaded mode)::
def callback():
... do stuff ...
[to exit the loop manually call vis.show(False)]
vis.loop(setup=vis.show,callback=callback)
vis.kill() #cleanup, not strictly needed but good practice
- To run a window with a custom plugin (GLPluginInterface) and terminate on
closure::
vis.run(plugin)
- To show a dialog or parallel window::
vis.setPlugin(plugin)
... then call
vis.dialog()
... or
vis.show()
... do stuff afterwards ...
vis.kill()
- To add a GLPluginInterface that just customizes a few things on top of
the default visualization::
vis.pushPlugin(plugin)
vis.dialog()
vis.popPlugin()
- To run plugins side-by-side in the same window::
vis.setPlugin(plugin1)
vis.addPlugin(plugin2) #this creates a new split-screen
vis.dialog()
... or
vis.show()
... do stuff afterwards ...
vis.kill()
- To run a custom Qt window or dialog containing a visualization window::
vis.setPlugin([desired plugin or None for visualization])
def makeMyUI(qtglwidget):
return MyQtMainWindow(qtglwidget)
vis.customUI(makeMyUI)
vis.dialog() #if you return a QDialog
... or
vis.show() #if you return a QWidget or QMainWindow
... do stuff afterwards ...
vis.kill()
- To launch a second window after the first is closed: just call whatever you
want again. Note: if show was previously called with a plugin and you wish to
revert to the default visualization, you should call setPlugin(None) first to
restore the default.
- To create a separate window with a given plugin::
w1 = vis.createWindow("Window 1") #w1=0
show()
w2 = vis.createWindow("Window 2") #w2=1
vis.setPlugin(plugin)
vis.dialog()
#to restore commands to the original window
vis.setWindow(w1)
while vis.shown():
...
vis.kill()
Implementation Details
^^^^^^^^^^^^^^^^^^^^^^
There are two primary modes of running OpenGL visualizations: multi-threaded
and single-threaded.
- Multi-threaded mode pops up a window using :func:`show`, and the caller can
then continue to interact with the vis module.
IMPORTANT: multi-threaded mode is only supported on some systems (Linux,
Windows using Qt). Due to weird OpenGL and Qt behavior in multi-threaded
programs, if you are using multithreaded mode, you should only interact
with OpenGL and the visualization using the methods in this module.
Custom OpenGL calls can be implemented inside GLPluginInterface plugins
and customDrawFunc.
- Single-threaded mode blocks the calling thread using :func:`loop`. To
interact with the scene, the caller will provide callbacks that can modify
the visualization world, pop up windows etc.
Single-threaded mode is the most compatible, and is the only mode that works
with GLUT and Mac OS.
There are also some convenience functions that will work in both modes, such
as :func:`run`, :func:`spin`, and func:`dialog`.
.. note::
In multithreaded mode, when changing the data shown by the window (e.g.,
modifying the configurations of robots in a WorldModel) you must call
``vis.lock()`` before accessing the data and then call ``vis.unlock()``
afterwards.
The biggest drawback of single-threaded operation is that you can only start
blocking dialogs at the outer-most level, not inside loop(). So if you have
a GUI that's running in real-time, in a multi-threaded visualization your
code can pop up a dialog (like an editor) and the continue running with the
returned value. There are some workarounds in single-thread mode (providing
a callback to the dialog function) but these are not nearly as convenient.
It is possible to start in single-threaded mode and convert to multi-threaded,
but the converse is not possible.
In OpenGL mode, you can also completely override the scene manager and run your
own OpenGL calls using a subclass of
:class:`~klampt.vis.glinterface.GLPluginInterface`. Here, you will need to
perform all the necessary OpenGL drawing / interaction inside its hooks.
To use this, you should call ``vis.setPlugin(plugin)`` to override the default
visualization behavior before creating your window. See
Klampt-examples/Python/demos/visplugin.py for an example of this in use.
IPython (Jupyter notebook)
~~~~~~~~~~~~~~~~~~~~~~~~~~
IPython visualizations run in a Jupyter notebook in a web browser, using a
WebGL widget to render the content. The Python code communicates with the
browser upon certain calls to update the visualization.
This mode will be enabled by default inside a Jupyter notebook, or if you first
call ``vis.init('IPython')``. In the cell in which you want to show the WebGL
widget, call ``vis.show()``. To show multiple widgets you can create new vis
windows, and use ``setWindow`` to switch between which widget you'd like
subsequent calls to modify.
The WebGL widget is updated automatically upon ``addX``, ``setColor``,
``clear``, and ``hide`` calls, but it can't tell when something changes in the
world, like a robot configuration or object transform. When the state of
something in the world changes, you must manually make a call to
``vis.update()``.
.. note::
For optimal performance when calling a lot of scene modifiers, you should
use ``vis.lock() / unlock()`` calls to block off the start and end of
scene modification. Doing so means that the WebGL widget
is only re-rendered at the very end of your calls.
Note the semantics here are slightly different from the normal sense of
locking / unlocking, which is to prevent thread clashes amongst changes to
the underlying object data.
This mode does NOT support plugins, dialogs, or custom draw functions. Also,
certain types of geometries like VolumeGrids are not supported.
Animations are supported, but you will manually have to advance the animations
and call ``vis.update()`` or ``vis.scene().update()`` for each frame.
See :class:`~klampt.vis.ipython.Playback` for a convenient widget
that handles this somewhat automatically.
HTML
~~~~~~~~~~~~~~~~~~~~
This output mode outputs an HTML + Javascript page that uses WebGL to display
the scene. Unlike other methods, this is not a live visualization that
interacts with your Python script, but rather you create a scene / animation
and then retrieve the HTML afterwards. This mode is appropriate for Google
Colab, for example.
To show the HTML page in an IPython notebook (e.g., Jupyter or Colab),
call ``vis.show()`` after the animation is done. This will show the visualizer
in an IFrame, whose size is the current viewport size.
To use it with a specified time step, use the following::
vis.add("world",world)
dt = 1.0/30.0 #30fps
while not done:
... change the world, add / remove things, etc ...
vis.stepAnimation(dt)
vis.show()
To have it capture a real-time process, use the ``vis.update()`` function
instead, as follows::
vis.add("world",world)
while not done:
... change the world, add / remove things, etc ...
vis.update()
time.sleep(1.0/3.0)
vis.show()
(Don't mix animations and updates, or else the resulting animation will be
strange.)
You may also retrieve raw HTML, either by casting ``vis.nativeWindow()`` to
a str, calling ``vis.nativeWindow().iframe(width,height)``, or
``vis.nativeWindow().page()``. In the first case, the HTML does not contain
the <html> or <body> tags, and takes up the whole screen. The .iframe()
option places the code into an IFrame of a given size, and the page() option
wraps the code into a full HTML page.
For example, to turn a Trajectory ``traj`` into a WebGL animation, use this
code::
vis.add("world",world)
vis.animate(("world",world.robot(0).getName()),traj)
t = 0
while t < traj.endTime():
vis.stepAnimation(dt)
t += dt
vis.show()
To turn a Simulator into a WebGL animation, use this code::
sim = Simulator(world)
vis.add("world",world)
while not done:
... add forces, send commands to the robot, etc...
sim.simulate(dt)
vis.stepAnimation(dt)
vis.show()
WINDOWING API
--------------
- :func:`debug`: a super easy way to visualize Klamp't items.
- :func:`init`: initializes the visualization. Can configure here
what backend(s) to use.
- :func:`createWindow`: creates a new visualization window and returns an
integer identifier.
- :func:`setWindow`: sets the active window for all subsequent calls. ID 0 is
the default visualization window.
- :func:`getWindow`: gets the active window ID.
- :func:`nativeWindow`: returns the current window object used by the backend.
- :func:`setWindowTitle`: sets the title of the visualization window.
- :func:`getWindowTitle`: returns the title of the visualization window.
- :func:`resizeWindow`: resizes the window. For OpenGL, this can be done
after the window is shown. Otherwise, it must take place before showing.
- :func:`scene`: returns the current :class:`VisualizationScene`
- :func:`setPlugin`: sets the current plugin (a
:class:`~klampt.vis.glinterface.GLPluginInterface` instance). This plugin
will now capture input from the visualization and can override any of the
default behavior of the visualizer. Set plugin=None if you want to return to
the default visualization.
- :func:`pushPlugin`: adds a new plugin (e.g., to capture input) on top of
the old one.
- :func:`splitView`: adds a second scene / viewport to the current
window. If a plugin is provided (a :class:`GLPluginInterface` instance) then
the new view is set to use this plugin.
- :func:`run`: pops up a dialog and then kills the program afterwards.
- :func:`kill`: kills all previously launched visualizations and terminates the
visualization thread. Afterwards, you may not be able to start new windows.
Call this to cleanly quit.
- :func:`multithreaded`: returns true if multithreading is available.
- :func:`loop`: Runs the visualization
thread inline with the main thread. The setup() function is called at the
start, the callback() function is run every time the event thread is idle,
and the cleanup() function is called on termination.
NOTE FOR MAC USERS: having the GUI in a separate thread is not supported on
Mac, so the ``loop`` function must be used rather than ``show``/``spin``.
NOTE FOR GLUT USERS: this may only be run once.
- :func:`dialog`: pops up a dialog box (does not return to calling thread until
closed).
- :func:`show`: shows/hides a visualization window. If not called
from the visualization loop, a new visualization thread is run in parallel
with the calling script.
- :func:`spin`: shows the visualization window for the desired amount
of time before returning, or until the user closes the window.
- :func:`shown`: returns true if the window is shown.
- :func:`lock`: locks the visualization scene for editing. The visualization
will be paused until unlock() is called.
- :func:`unlock` unlocks the visualization world. Must only be called once
after every lock().
- :func:`update`: manually triggers a redraw of the current scene.
- :func:`threadCall`: Call a user-defined function inside the visualization
thread. Advanced users may wish to inject calls that are incompatible with
being run outside the Qt or OpenGL thread.
- :func:`customUI`: launches a user-defined UI window with the OpenGL window
embedded into it (Qt only).
SCENE MODIFICATION API
------------------------
The following methods operate on the current scene (as returned from
:func:`scene`).
Objects accepted by :func:`add` include:
- text (str)
- Vector3 (3-list)
- Matrix3 (:mod:`klampt.math.so3` item)
- RigidTransform (:mod:`klampt.math.se3` item)
- Config (n-list): n must match the number of links of a RobotModel in "world"
- Configs (list of n-lists)
- :class:`Trajectory` and its subclasses
- polylines (use :class:`Trajectory` type)
- :class:`WorldModel`
- :class:`RobotModel`
- :class:`RobotModelLink`
- :class:`RigidObjectModel`
- :class:`TerrainModel`
- :class:`Geometry3D`
- :class:`GeometricPrimitive`
- :class:`PointCloud`
- :class:`TriangleMesh`
- :class:`Simulator`
- :class:`ContactPoint`
- objects from the :mod:`~klampt.model.coordinates` module.
See func:`setAttribute` for a list of attributes that can be used to customize
an object's appearance. Common attributes include ``color``, ``hide_label``,
``type``, ``position`` (for text) and ``size`` (for points).
In OpenGL modes and IPython mode, many objects can be edited using the
:func:`edit` function. In OpenGL, this will provide a visual editing widget,
while in IPython this will pop up IPython widgets.
If you are modifying the internal data of an object in an external loop
(as opposed to inside a plugin) be sure to call :func:`lock`/:func:`unlock`
before/after doing so to prevent the visualization from accessing the
object's data .
Scene management functions
~~~~~~~~~~~~~~~~~~~~~~~~~~
- :func:`add`: adds an item to the
visualization. name is a unique identifier. If an item with the same name
already exists, it will no longer be shown. Keyword attributes can be
given to customize the appearance of the object (see :func:`setAttribute`.)
- :func:`clear`: clears the visualization world.
- :func:`listItems`: prints out all names of visualization objects in the scene
or under a given object
- :func:`getItemName`: retrieves the name / path of a given object in the
scene, or returns None if the object doesnt exist.
- :func:`dirty`: marks the given item as dirty and recreates the OpenGL display
lists. You may need to call this if you modify an item's geometry, for
example.
- :func:`remove`: removes an item from the visualization.
- :func:`setItemConfig`: sets the configuration of a named item.
- :func:`getItemConfig`: returns the configuration of a named item.
- :func:`hide`: hides/unhides an item. The item is not removed, it just
becomes invisible.
- :func:`hideLabel`: hides/unhides an item's text label.
- :func:`setLabel`: changes an item's text label from its name to a custom
string.
- :func:`setAppearance`: changes the Appearance of an item.
- :func:`revertAppearance`: restores the Appearance of an item
- :func:`setAttribute`: sets an attribute to change an item's appearance.
- :func:`getAttribute`: gets an attribute of an item's appearance.
- :func:`getAttributes`: gets all relevant attributes of an item's appearance.
- :func:`setColor`: changes the color of an item.
- :func:`setDrawFunc`: sets a custom OpenGL drawing function for an item.
Animation functions
~~~~~~~~~~~~~~~~~~~~~~~~~~
- :func:`animate` Starts an animation on an item. The animation be a
:class:`Trajectory` or a list of configurations. Works with points, so3
elements, se3 elements, rigid objects, or robots.
- :func:`pauseAnimation`: Turns animation on/off.
- :func:`stepAnimation`: Moves forward the animation time by the given
amount, in seconds.
- :func:`animationTime`: Gets/sets the current animation time
- :func:`setTimeCallback`: sets a function that will return the global time
used by the animation timing, visualization plots, and movie-saving
functions. This must be monotonically non-decreasing.
Text and plots
~~~~~~~~~~~~~~~~
Like other items in the visualization scene, text and plots are referred to by
string identifiers.
Text is usually attached to 2D pixel coordinates, but in OpenGL mode can also
be attached to 3D points. Use the ``position`` attribute to control where the
text is located and the ``size`` attribute to control its size.
Plots are added to the scene and then items are added to the plot. The
configuration of a visualization item is then shown as a live display (OpenGL).
You may also log custom numeric data with :func:`logPlot` and event data using
:func:`logPlotEvent`.
- :func:`addText`: adds text to the visualizer.
- :func:`clearText`: clears all previously added text.
- :func:`addPlot`: creates a new empty plot.
- :func:`addPlotItem`: adds a visualization item to a plot.
- :func:`logPlot`: logs a custom visualization item to a plot
- :func:`logPlotEvent`: logs an event on the plot.
- :func:`hidePlotItem`: hides an item in the plot.
- :func:`setPlotDuration`: sets the plot duration.
- :func:`setPlotRange`: sets the y range of a plot.
- :func:`setPlotPosition`: sets the upper left position of the plot on the screen.
- :func:`setPlotSize`: sets the width and height of the plot.
- :func:`savePlot`: saves a plot to a CSV (extension .csv) or Trajectory
(extension .traj) file.
User Interaction
~~~~~~~~~~~~~~~~
- :func:`edit`: turns on/off visual editing of some item. Points, transforms,
``coordinates.Point``, ``coordinates.Transform``, ``coordinates.Frame``,
:class:`RobotModel`, and :class:`RigidObjectModel` are currently accepted.
- :func:`pick`: asks the user to click on an item in the scene.
- :func:`addAction`: adds a keyboard shortcut (GLUT) / menu items (Qt) /
buttons (IPython).
- :func:`addButton`: adds a button to the GUI.
- :func:`addCheckbox`: adds a checkbox to the GUI.
- :func:`addSelect`: adds a selection dropdown or listbox to the GUI.
- :func:`addInput`: adds a string, integer, or float input box to the GUI.
- :func:`addSlider`: adds a slider to the GUI.
- :func:`addWidget`: adds a widget(s) to the GUI from a JSON description.
- :func:`setGUI`: Sets the GUI from a JSON description.
- :func:`bindWidget`: Binds GUI feedback to a callback function.
- :func:`removeWidget`: Removes a widget from the GUI.
- :func:`getWidgetState`: Gets the state of a widget from the GUI.
Global appearance / camera control functions
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- :func:`getViewport`: Returns the :class:`GLViewport` for the currently active
view.
- :func:`setViewport`: Sets the :class:`GLViewport` for the currently active
scene. (This may also be used to resize windows.)
- :func:`setBackgroundColor`: Sets the background color for the active
view.
- :func:`autoFitCamera`: Automatically fits the camera to all objects in the
visualization. A scale > 1 magnifies the zoom.
- :func:`followCamera`: Sets the camera to follow a target.
- :func:`saveJsonConfig`: Saves the configuration to a JSON object or JSON
file.
- :func:`loadJsonConfig`: Loads the configuration from a JSON object or JSON
file.
- :func:`screenshot`: returns a screenshot of the scene. Can retrieve depth,
as well.
- :func:`screenshotCallback`: sets a callback that will receive a screenshot
of the scene after rendering is done.
Utility functions
~~~~~~~~~~~~~~~~~
- :func:`objectToVisType` Auto-determines a type of object compatible with the
visualizer.
- :func:`autoFitViewport`: Automatically fits a :class:`GLViewport` to see all
the given objects.
NAMING CONVENTION
-----------------
The world, if one exists, should be given the name 'world'. Configurations and
paths are drawn with reference to the first robot in the world.
All items that refer to a name (except add) can either be given a top level
item name (a string) or a sub-item (a sequence of strings, given a path from
the root to the leaf). For example, if you've added a RobotWorld under the
name 'world' containing a robot called 'myRobot', then::
vis.setColor(('world','myRobot'),0,1,0)
will turn the robot green. If 'link5' is the robot's 5th link, then::
vis.setColor(('world','myRobot','link5'),0,0,1)
will turn the 5th link blue.
A shortcut is to use the :func:`getItemName` function on the given robot,
e.g.::
robot = world.robot(0)
vis.setColor(vis.getItemName(robot),0,0,1)
If there's a Simulator instance added to the scene under the name 'sim' or
'simulator', the animation timing for Qt movie saving and HTML animations
will follow the simulation time rather than wall clock time.
"""
import threading
from numpy import ptp
from ..robotsim import *
from ..math import vectorops,so3,se3
from . import gldraw
from . import glinit
from .glinterface import GLPluginInterface
from .glprogram import GLPluginProgram
from .glviewport import GLViewport
from . import glcommon
import time
import signal
import weakref
import sys
from ..model import types
from ..model import config
from ..model import coordinates
from ..model.subrobot import SubRobotModel
from ..model.trajectory import *
from ..model.multipath import MultiPath
from ..model.contact import ContactPoint,Hold
from ..model.collide import bb_empty,bb_create,bb_union
from typing import Callable,Union,Sequence
import warnings
#Type for references to items in a visualization world
ItemPath = Union[str,Sequence[str]]
#the global lock for all visualization calls
_globalLock = threading.RLock()
#the chosen backend
_backend = None
#the _WindowManager instance
_window_manager = None
#the OpenGL module (will be taken from glinit.init())
GL = None
def _isnotebook():
try:
shell = get_ipython().__class__.__name__
if shell == 'ZMQInteractiveShell':
return True # Jupyter notebook or qtconsole
elif shell == 'TerminalInteractiveShell':
return False # Terminal running IPython
elif shell == 'Shell':
if 'google.colab' in sys.modules:
return True
return False # Other type (?)
else:
return False # Other type (?)
except NameError:
return False # Probably standard Python interpreter
[docs]def init(backends=None):
"""Initializes the vis module using some visualization backend. `backends`
can be None, in which case it tries using PyQt, then GLUT, then IPython in
that order. It can also be a string or list of strings from the following
set:
- 'PyQt': uses PyQT + OpenGL
- 'PyQt4' / 'PyQt5': uses a specific version of PyQT
- 'GLUT': uses GLUT + OpenGL
- 'IPython': uses an IPython widget
- 'HTML': outputs an HTML / Javascript widget
"""
global _backend,_window_manager,GL
if backends is None:
if _isnotebook():
if 'google.colab' in sys.modules:
backends = ['HTML']
else:
backends = ['IPython']
else:
backends = ['PyQt','GLUT','HTML']
if isinstance(backends,str):
backends = [backends]
if _backend is not None:
#already initialized
if _backend in backends or _backend[:4] in backends:
if _backend in ['IPython','HTML']:
#force a reset for IPython / HTML
_window_manager.reset()
return _backend
print("vis.init(): Trying to reset from backend",_backend,"to one of",backends)
_window_manager.reset()
_window_manager = None
_backend = None
OpenGLBackends = ['PyQt','PyQt4','PyQt5','GLUT']
order = [[]]
for backend in backends:
if backend in ['IPython','HTML']:
order.append(backend)
order.append([])
else:
order[-1].append(backend)
for trials in order:
if trials == 'IPython':
_backend = 'IPython'
from .backends import vis_ipython
_window_manager = vis_ipython.IPythonWindowManager()
return _backend
elif trials == 'HTML':
_backend = 'HTML'
from .backends import vis_html
_window_manager = vis_html.HTMLWindowManager()
return _backend
elif len(trials)>0:
res = glinit.init(trials)
GL = glinit.GL()
if res is not None:
_backend = glinit.active()
if glinit.active() == 'GLUT':
from .backends import vis_glut
_window_manager = vis_glut.GLUTWindowManager()
warnings.warn("klampt.visualization: QT is not available, falling back to poorer\nGLUT interface. Returning to another GLUT thread will not work\nproperly.")
else:
from .backends import vis_qt
_window_manager = vis_qt.QtWindowManager()
import atexit
atexit.register(kill)
return res
return None
def _init():
global _backend
if _backend is not None:
return
if init() is None:
raise RuntimeError("Unable to initialize visualization")
[docs]def debug(*args,**kwargs):
"""A super easy way to visualize Klamp't items.
The argument list can be a list of Klamp't items, and can also include
strings or dicts. If a string precedes an item, then it will be labeled
by the string. If a dict follows an item, the dict will specify
attributes for the item. It can also contain the 'animation' key, in
which case it should contain a Trajectory animating the item.
Keyword arguments may include:
- title: the window title
- animation: if only one item is given, sets a looping animation
- centerCamera: the name of the item that the camera should look at, or
True to center on the whole scene.
- followCamera: the name of the item that the camera will follow if
animated, or None (default).
- dialog: True if a dialog should be shown (default), False if a standard
show() should be used.
- anything else: Treated as named klamp't item.
"""
global _backend
_init()
oldWindow = getWindow()
if oldWindow is None:
oldWindow = 0
myWindow = createWindow()
nextName = None
lastName = None
itemcount = 0
if 'world' in kwargs:
add('world',kwargs['world'])
del kwargs['world']
animationDuration = 0
for i,arg in enumerate(args):
if isinstance(arg,str):
nextName = arg
elif isinstance(arg,dict):
if lastName is None:
warnings.warn("vis.debug(): dict of attributes must follow an object")
continue
for (k,v) in arg.items():
if k == 'animation':
animate(lastName,v)
animationDuration = max(animationDuration,v.endTime())
else:
try:
setAttribute(lastName,k,v)
except Exception:
warnings.warn("vis.debug(): Couldn't set attribute {} of item {}".format(k,lastName))
else:
label = None
if nextName is None:
name = None
if hasattr(arg,'getName'):
try:
name = arg.getName()
except Exception:
pass
if hasattr(arg,'name') and isinstance(arg.name,str):
name = arg.name
if name is None:
try:
type = types.object_to_type(arg)
name = type + '['+str(itemcount)+']'
except ValueError:
name = 'item['+str(itemcount)+']'
else:
name = nextName
label = name
add(name,arg)
itemcount += 1
lastName = name
nextName = None
title = None
doDialog = True
centerCamera = None
followCameraItem = None
animation = None
for k,v in kwargs.items():
if k=='title':
title = v
elif k=='world':
pass
elif k=='dialog':
doDialog = v
elif k=='animation':
animation = v
elif k=='followCamera':
followCameraItem = v
elif k=='centerCamera':
centerCamera = v
else:
add(k,v)
lastName = k
itemcount += 1
if title is not None:
setWindowTitle(title)
else:
setWindowTitle("Klampt debugging: "+','.join(scene().items.keys()))
if animation is not None:
animate(lastName,animation)
if centerCamera is True:
autoFitCamera(rotate=False)
elif centerCamera:
if isinstance(centerCamera,int):
centerCamera = 'item['+str(centerCamera)+']'
elif not isinstance(centerCamera,(str,tuple)):
centerCamera = getItemName(centerCamera)
if centerCamera is None:
warnings.warn("vis.debug(): could not center camera, invalid object name")
else:
vp = getViewport()
try:
autoFitViewport(vp,[scene().getItem(centerCamera)],rotate=False)
setViewport(vp)
except Exception:
warnings.warn("vis.debug(): Centering camera failed")
import traceback
traceback.print_exc()
if followCameraItem is not None:
if followCameraItem is True:
followCamera(lastName,center=True)
else:
if not isinstance(followCameraItem,(str,tuple)):
followCameraItem = getItemName(followCameraItem)
if followCameraItem is None:
warnings.warn("vis.debug(): could not follow camera, invalid object name")
followCamera(followCameraItem,center=True)
if _backend == 'HTML':
#dump out the animation
if animation is not None:
animationDuration = max(animationDuration,animation.endTime())
if animationDuration > 0:
dt = 1.0/30.0
t = 0
while t < animationDuration:
stepAnimation(dt)
t += dt
show()
setWindow(oldWindow)
elif _backend == 'IPython':
#setup a Playback widget from the animation
if animation is not None:
animationDuration = max(animationDuration,animation.endTime())
if animationDuration > 0:
framerate = 30
my_scene = scene()
def advance():
my_scene.stepAnimation(1.0/framerate)
my_scene.update()
def reset():
my_scene.animationTime(0)
from .ipython import Playback
playback = Playback(nativeWindow(),advance=advance,reset=reset,maxframes=int(animationDuration*framerate),framerate=framerate)
from IPython.display import display
display(playback)
show()
else:
if doDialog:
dialog()
setWindow(oldWindow)
else:
show()
#open ended...
[docs]def nativeWindow():
"""Returns the active window data used by the backend. The result will be
a subclass of :class:`~klampt.vis.glprogram.GLPluginProgram` if OpenGL is
used (PyQt or GLUT) or a :class:`~klampt.vis.ipython.KlamptWidget`
"""
global _window_manager
if _window_manager is None:
return None
return _window_manager.frontend()
[docs]def scene() -> "VisualizationScene":
"""Returns the active scene used by the backend. The result will be
a subclass of :class:`VisualizationScene`. In some backends, there
will be exactly one window per scene, but in others (split screen)
there may be multiple windows per scene.
"""
global _window_manager
if _window_manager is None:
return None
return _window_manager.scene()
[docs]def createWindow(title=None) -> int:
"""Creates a new window (and sets it active).
Returns:
An identifier of the window (for use with :func:`setWindow`).
"""
global _globalLock,_window_manager
_init()
with _globalLock:
id = _window_manager.createWindow(title)
return id
[docs]def setWindow(id : int) -> None:
"""Sets the currently active window.
Note:
ID 0 is the default visualization window.
"""
global _globalLock,_window_manager
_init()
with _globalLock:
_window_manager.setWindow(id)
[docs]def getWindow() -> int:
"""Retrieves ID of currently active window or -1 if no window is active"""
global _window_manager
_init()
return _window_manager.getWindow()
[docs]def setPlugin(plugin: GLPluginInterface) -> None:
"""Lets the user capture input via a glinterface.GLPluginInterface class.
Set plugin to None to disable plugins and return to the standard
visualization.
Args:
plugin (GLPluginInterface): a plugin that will hereafter capture input
from the visualization and can override any of the default behavior
of the visualizer. Can be set to None if you want to return to the
default visualization.
"""
global _globalLock,_window_manager
_init()
with _globalLock:
_window_manager.setPlugin(plugin)
[docs]def pushPlugin(plugin: GLPluginInterface) -> None:
"""Adds a new plugin on top of the old one.
Args:
plugin (GLPluginInterface): a plugin that will optionally intercept GUI
callbacks. Unhandled callbacks will be forwarded to the next plugin
on the stack.
"""
global _globalLock,_window_manager
_init()
with _globalLock:
_window_manager.pushPlugin(plugin)
[docs]def popPlugin() -> None:
"""Reverses a prior pushPlugin() call"""
global _window_manager
_init()
with _globalLock:
_window_manager.popPlugin()
[docs]def splitView(plugin : GLPluginInterface=None) -> None:
"""Adds a second OpenGL viewport in the same window, governed by the given
plugin.
Args:
plugin (GLPluginInterface): the plugin used for the second viewport.
If None, the new viewport will have the default visualization
plugin.
"""
global _window_manager
_init()
with _globalLock:
_window_manager.splitView(plugin)
[docs]def addPlugin(plugin : GLPluginInterface=None) -> None:
"""Adds a second OpenGL viewport in the same window, governed by the given
plugin. DEPRECATED: use :func:`splitView` instead.
Args:
plugin (GLPluginInterface): the plugin used for the second viewport.
If None, the new viewport will have the default visualization
plugin.
"""
warnings.warn("{} will be deprecated in favor of {} in a future version of Klampt".format('addPlugin','splitView'),DeprecationWarning)
splitView(plugin)
[docs]def run(plugin : GLPluginInterface=None) -> None:
"""A blocking call to start a single window and then kill the visualization
once the user closes the window.
Args:
plugin (GLPluginInterface, optional): If given, the plugin used to handle all
rendering and user input. If plugin is None, the default visualization is
used.
Note:
Works in both multi-threaded and single-threaded mode.
"""
global _window_manager
_init()
if plugin is not None:
setPlugin(plugin)
_window_manager.run()
kill()
[docs]def multithreaded() -> bool:
"""Returns true if the current GUI system allows multithreading. Useful for apps
that will work cross-platform with Macs and systems with only GLUT.
"""
global _window_manager
_init()
return _window_manager.multithreaded()
[docs]def dialog():
"""A blocking call to start a single dialog window with the current plugin. It is
closed by pressing OK or closing the window."""
global _window_manager
_init()
return _window_manager.dialog()
[docs]def setWindowTitle(title : str) -> None:
global _window_manager
_init()
_window_manager.setWindowName(title)
[docs]def getWindowTitle() -> str:
global _window_manager
return _window_manager.getWindowName()
[docs]def resizeWindow(w : int, h : int):
"""Resizes the current window. For OpenGL, this can be done after the
window is shown. Otherwise, it must take place before showing."""
global _window_manager
return _window_manager.resizeWindow(w,h)
[docs]def kill() -> None:
"""This should be called at the end of the calling program to cleanly terminate the
visualization thread"""
global _backend,_window_manager
if _backend is None:
return
_window_manager.kill()
_window_manager = None
_backend = None
[docs]def loop(setup : Callable=None, callback : Callable=None, cleanup : Callable=None) -> None:
"""Runs the visualization thread inline with the main thread.
The setup() function is called at the start, the callback() function is run
every time the event thread is idle, and the cleanup() function is called
on termination.
NOTE FOR MAC USERS: a multithreaded GUI is not supported on Mac, so the loop()
function must be used rather than "show and wait".
NOTE FOR GLUT USERS: this may only be run once.
"""
global _window_manager
_init()
_window_manager.loop(setup,callback,cleanup)
[docs]def show(display : bool=True) -> None:
"""Shows or hides the current window.
NOTE FOR MAC USERS: due to a lack of support of multithreading on Mac, this
will not work outside of the setup / callback / cleanup functions given in a
call to loop().
"""
global _window_manager
_init()
with _globalLock:
if display:
_window_manager.show()
else:
_window_manager.hide()
[docs]def spin(duration : float) -> None:
"""Spin-shows a window for a certain duration or until the window is closed."""
global _window_manager
_init()
_window_manager.spin(duration)
[docs]def lock() -> None:
"""Begins a locked section. Needs to be called any time you modify a
visualization item outside of the visualization thread. unlock() must be
called to let the visualization thread proceed.
"""
global _window_manager
_window_manager.lock()
[docs]def unlock() -> None:
"""Ends a locked section acquired by lock()."""
global _window_manager
_window_manager.unlock()
[docs]def update() -> None:
"""Manually triggers a redraw of the current window."""
global _window_manager
_window_manager.update()
[docs]def shown() -> bool:
"""Returns true if a visualization window is currently shown."""
global _globalLock,_window_manager
_init()
with _globalLock:
res = _window_manager.shown()
return res
[docs]def customUI(func : Callable) -> None:
"""Tells the next created window/dialog to use a custom UI function. Only
available in PyQT mode.
This is used to build custom editors and windows that are compatible with
other UI functionality.
Args:
func (function): a 1-argument function that takes a configured Klamp't
QtWindow as its argument and returns a QDialog, QMainWindow, or
QWidget.
(Could also be used with GLUT, but what would you do with a GLUTWindow?)
"""
global _globalLock,_window_manager
_init()
with _globalLock:
_window_manager.set_custom_ui(func)
[docs]def threadCall(func : Callable) -> None:
"""Call `func` inside the visualization thread. This is useful for some
odd calls that are incompatible with being run outside the Qt or OpenGL
thread.
Possible use cases include performing extra OpenGL draws for camera
simulation.
"""
global _globalLock,_window_manager
with _globalLock:
_window_manager.threadCall(func)
######### CONVENIENCE ALIASES FOR VisualizationScene methods ###########
[docs]def addAction(hook : Callable, short_text : str, key : str=None, description : str=None) -> None:
"""Adds a callback to the window that can be triggered by menu choice or
keyboard. Alias for nativeWindow().addAction().
Args:
hook (function): a python callback function, taking no arguments, called
when the action is triggered.
short_text (str): the text shown in the menu bar.
key (str, optional): a shortcut keyboard command (e.g., can be 'k' or 'Ctrl+k').
description (str, optional): if provided, this is a tooltip that shows up
when the user hovers their mouse over the menu item.
"""
_init()
nativeWindow().addAction(hook,short_text,key,description)
[docs]def clear() -> None:
"""Clears the visualization world."""
if _backend is None:
return
scene().clear()
[docs]def add(name : str, item,keepAppearance=False,**kwargs) -> None:
"""Adds an item to the visualization.
Args:
name (str): a unique identifier. If an item with the same name already
exists, it will no longer be shown.
keepAppearance (bool, optional): if True, then if there was an item that
had the same name, the prior item's appearance will be kept.
kwargs: key-value pairs to be added into the attributes dictionary. e.g.
vis.add("geom",geometry,color=[1,0,0,1]) adds a geometry while setting
its color to red.
"""
_init()
scene().add(name,item,keepAppearance,**kwargs)
[docs]def listItems(name=None,indent=0) -> None:
_init()
scene().listItems(name,indent)
[docs]def getItemName(object) -> ItemPath:
"""Retrieves the name / path of a given object in the scene, or returns
None if the object doesnt exist.
"""
if _backend is None:
return None
return scene().getItemName(object)
[docs]def dirty(item_name : str='all') -> None:
"""Marks the given item as dirty and recreates the OpenGL display lists.
You may need to call this if you modify an item's geometry, for example.
If things start disappearing from your world when you create a new window,
you may need to call this too.
"""
scene().dirty(item_name)
[docs]def animate(name : ItemPath, animation, speed : float=1.0, endBehavior : str='loop') -> None:
"""Sends an animation to the named object.
Works with points, so3 elements, se3 elements, rigid objects, or robots, and may work
with other objects as well.
Args:
name (str): the named object
animation: may be a Trajectory or a list of configurations.
speed (float, optional): a modulator on the animation speed. If the animation
is a list of milestones, it is by default run at 1 milestone per second.
endBehavior (str, optional): either 'loop' (animation repeats forever) or 'halt'
(plays once).
"""
scene().animate(name,animation,speed,endBehavior)
[docs]def pauseAnimation(paused=True) -> None:
"""Pauses or unpauses the animation."""
scene().pauseAnimation(paused)
[docs]def stepAnimation(amount : float) -> None:
"""Moves forward the animation time by ``amount``, given in seconds"""
scene().stepAnimation(amount)
[docs]def animationTime(newtime : float=None) -> None:
"""Gets/sets the current animation time
If newtime is None (default), this gets the animation time.
If newtime is not None, this sets a new animation time.
"""
return scene().animationTime(newtime)
[docs]def setTimeCallback(timefunc : Callable=None) -> None:
"""Sets a function that will return the window's global time. This
will be used by the animation timing, visualization plots, and movie-saving
functions.
Args:
timefunc (callable): returns a monotonically non-decreasing float.
If None, reverts back to using time.time().
"""
_init()
scene().setTimeCallback(timefunc)
[docs]def remove(name : ItemPath) -> None:
"""Removes an item from the visualization"""
return scene().remove(name)
[docs]def getItemConfig(name : ItemPath) -> Sequence[float]:
"""Returns a configuration of an item from the visualization. Useful for
interacting with edited objects.
Returns:
A list of floats describing the item's current configuration. Returns
None if ``name`` doesnt refer to an object.
"""
return scene().getItemConfig(name)
[docs]def setItemConfig(name : ItemPath, value : Sequence[float]) -> None:
"""Sets a configuration of an item from the visualization.
Args:
name (str): the item to set the configuration.
value (list of floats): the item's configuration. The number of items
depends on the object's type. See the :mod:`klampt.model.config`
module for more information.
"""
return scene().setItemConfig(name,value)
[docs]def setLabel(name : ItemPath, text : str) -> None:
"""Changes the label of an item in the visualization"""
setAttribute(name,"label",text)
[docs]def hideLabel(name : ItemPath, hidden=True) -> None:
"""Hides or shows the label of an item in the visualization"""
return scene().hideLabel(name,hidden)
[docs]def hide(name : ItemPath, hidden=True) -> None:
"""Hides an item in the visualization.
Note: the opposite of hide() is not show(), it's hide(False).
"""
scene().hide(name,hidden)
[docs]def edit(name : ItemPath, doedit=True) -> None:
"""Turns on/off visual editing of some item.
In OpenGL mode, currently accepts items of type:
- Vector3 (3-list)
- Matrix3 (9-list)
- Config (n-list, where n is the # of robot links)
- RigidTransform (se3 object)
- :class:`~klampt.robotsim.RobotModel`
- :class:`~klampt.robotsim.RigidObjectModel`
- :class:`~klampt.model.coordinates.Point`
- :class:`~klampt.model.coordinates.Transform`
- :class:`~klampt.model.coordinates.Frame`
In IPython mode, currently accepts items of type:
- Vector3 (3-lists)
- Config (n-list, where n is the # of robot links)
- RigidTransform (se3 objects)
- :class:`~klampt.robotsim.RobotModel`
- :class:`~klampt.robotsim.RigidObjectModel`
- :class:`~klampt.model.coordinates.Point`
- :class:`~klampt.model.coordinates.Transform`
- :class:`~klampt.model.coordinates.Frame`
Returns:
None if doedit=False, otherwise, an editor object which
depends on the backend. In OpenGL mode, returns one of the
:class:`~klampt.Widget` subclasses. In Jupyter, returns a ``VBox``
containing controls.
"""
return scene().edit(name,doedit)
[docs]def pick(click_callback : Callable, hover_callback : Callable=None,
highlight_color=(1,1,0,0.3), filter : Callable=None, tolerance=0.01) -> None:
"""Picks an item from the scene. ``click_callback`` is called once the
user left-clicks on an item. ``hover_callback`` is called when a user
hovers over an item.
Once the user clicks, callbacks are disabled. To re-enable callbacks, call
pick() again.
Args:
click_callback (callable): a function f(name,item,point) that receives
the name of the item, the item itself, and the point clicked on,
in world coordinates. Set this to None to disable picking.
hover_callback (callable): a function f(name,item,point) that receives
the name of the item, the item itself, and the point hovered over,
in world coordinates. Set this to None to disable picking.
highlight_color (tuple, optional): if given, hovered items will be
colored with the given color.
filter (callable, optional): a function f(name,item) that returns True
if the item should be included in the selection.
tolerance (float, optional): a collision tolerance for non-volumetric
objects.
"""
scene().pick(click_callback,hover_callback,highlight_color,filter,tolerance)
[docs]def addCheckbox(text : str, callback : Callable=None, checked=False, id=None, icon=None, iconformat=None, style=None) -> str:
"""Adds a checkbox to the GUI.
Arguments:
text (str): the label
callback (callable): a callback that is invoked when the button is
clicked.
checked (bool): whether the checkbox should be checked.
id (str, optional): if given, the id used for removing the widget and
retrieving state
icon (str): Base64 encoded icon
iconformat (str): MIME type of encoded icon
style (dict): the HTML style of the button
"""
widget = {'text':text,'type':'checkbox'}
if id is not None:
widget['id'] = id
else:
widget['id'] = text
if checked:
widget['state'] = 'checked'
else:
widget['state'] = ''
if icon is not None:
widget['icon'] = icon #TODO: encode
if iconformat is not None:
widget['iconformat'] = iconformat #TODO: encode
if style is not None:
widget['style'] = style
nativeWindow().addWidget(widget)
if callback is not None:
scene().bindWidget(widget['id'],callback)
return widget['id']
[docs]def addSelect(text : str, options : List[str], callback : Callable=None, selection=None, id=None, multiple=False, style=None) -> str:
"""Adds a selection widget to the GUI.
Arguments:
text (str): the label displayed next to the selection
options (list of str): the list of options in the box.
callback (callable): a callback that is invoked when the selection is changed.
selection (str or list of str): the selection
id (str, optional): if given, the id used for removing the widget and
retrieving state
multiple (bool): whether multiple items can be selected.
style (dict): the HTML style of the button
"""
widget = {'text':text,'type':'select','options':options}
if id is not None:
widget['id'] = id
else:
widget['id'] = text
if state is not None:
if isinstance(state,str):
state=[state]
widget['state'] = state
else:
widget['state'] = []
if style is not None:
widget['style'] = style
nativeWindow().addWidget(widget)
if callback is not None:
scene().bindWidget(widget['id'],callback)
return widget['id']
[docs]def setGUI(menu : dict) -> None:
"""Replaces a whole GUI with the JSONGUI object."""
nativeWindow().setGUI(menu)
[docs]def setAppearance(name : ItemPath, appearance : Appearance) -> None:
"""Changes the Appearance of an item, for an item that uses the Appearance
item to draw (config, geometry, robots, rigid bodies).
"""
scene().setAppearance(name,appearance)
[docs]def setAttribute(name : ItemPath, attr : str, value) -> None:
"""Sets an attribute of an item's appearance.
Args:
name (str): the name of the item
attr (str): the name of the attribute (see below)
value: the value (see below)
Accepted attributes are:
- 'robot': the index of the robot associated with this (default 0)
- 'color': the item's color (r,g,b) or (r,g,b,a)
- 'size': the size of the plot, text, point, ContactPoint, or IKObjective
- 'length': the length of axes in RigidTransform, or normal in ContactPoint
- 'width': the width of axes and trajectory curves
- 'duration': the duration of a plot
- 'pointSize': for a trajectory, the size of points (default None, set to 0
to disable drawing points)
- 'pointColor': for a trajectory, the size of points (default None)
- 'endeffectors': for a RobotTrajectory, the list of end effectors to plot
(default the last link).
- 'maxConfigs': for a Configs resource, the maximum number of drawn
configurations (default 10)
- 'fancy': for RigidTransform objects, whether the axes are drawn with
boxes or lines (default False)
- 'type': for ambiguous items, like a 3-item list when the robot has 3
links, specifies the type to be used. For example, 'Config' draws the
item as a robot configuration, while 'Vector3' or 'Point' draws it as a
point.
- 'label': a replacement label (str)
- 'hide_label': if True, the label will be hidden
"""
scene().setAttribute(name,attr,value)
[docs]def getAttribute(name : ItemPath, attr : str):
"""Gets an attribute of an item's appearance. If not previously set by the
user, the default value will be returned.
Args:
name (str): the name of the item
attr (str): the name of the attribute (see :func:`setAttribute`)
"""
return scene().getAttribute(name,attr)
[docs]def getAttributes(name : ItemPath) -> dict:
"""Gets a dictionary of all relevant attributes of an item's appearance.
If not previously set by the user, default values will be returned.
Args:
name (str): the name of the item
"""
return scene().getAttributes(name)
[docs]def revertAppearance(name : ItemPath) -> None:
scene().revertAppearance(name)
[docs]def setColor(name : ItemPath, r : float, g : float, b : float, a : float=1.0) -> None:
scene().setColor(name,r,g,b,a)
[docs]def setDrawFunc(name : ItemPath, func : Callable) -> None:
"""Sets a custom OpenGL drawing function for an item.
Args:
name (str): the name of the item
func (function or None): a one-argument function draw(data) that takes the item data
as input. Set func to None to revert to default drawing.
"""
scene().setDrawFunc(name,func)
def _getOffsets(object):
if isinstance(object,WorldModel):
res = []
for i in range(object.numRobots()):
res += _getOffsets(object.robot(i))
for i in range(object.numRigidObjects()):
res += _getOffsets(object.rigidObject(i))
return res
elif isinstance(object,RobotModel):
q = object.getConfig()
object.setConfig([0.0]*len(q))
worig = [object.link(i).getTransform()[1] for i in range(object.numLinks())]
object.setConfig(q)
wnew = [object.link(i).getTransform()[1] for i in range(object.numLinks())]
return [vectorops.sub(b,a) for a,b in zip(worig,wnew)]
elif isinstance(object,RobotModelLink):
return [object.getTransform()[1]]
elif isinstance(object,RigidObjectModel):
return [object.getTransform()[1]]
elif isinstance(object,Geometry3D):
return [object.getCurrentTransform()[1]]
elif isinstance(object,VisAppearance):
res = _getOffsets(object.item)
if len(res) != 0: return res
if len(object.subAppearances) == 0:
bb = object.getBounds()
if bb is not None and not bb_empty(bb):
return [vectorops.mul(vectorops.add(bb[0],bb[1]),0.5)]
else:
res = []
for a in object.subAppearances.values():
res += _getOffsets(a)
return res
return []
def _getBounds(object):
if isinstance(object,WorldModel):
res = []
for i in range(object.numRobots()):
res += _getBounds(object.robot(i))
for i in range(object.numRigidObjects()):
res += _getBounds(object.rigidObject(i))
return res
elif isinstance(object,RobotModel):
res = []
for i in range(object.numLinks()):
bb = object.link(i).geometry().getBB()
if bb is not None and not bb_empty(bb):
res += list(bb)
return res
elif isinstance(object,RobotModelLink):
return list(object.geometry().getBB())
elif isinstance(object,RigidObjectModel):
return list(object.geometry().getBB())
elif isinstance(object,Geometry3D):
return list(object.getBB())
elif isinstance(object,VisAppearance):
if len(object.subAppearances) == 0:
if isinstance(object.item,TerrainModel):
return []
bb = object.getBounds()
if bb is not None and not bb_empty(bb):
return list(bb)
else:
res = []
for a in object.subAppearances.values():
res += _getBounds(a)
return res
return []
[docs]def autoFitViewport(viewport : Viewport, objects : Sequence, zoom=True,rotate=True) -> None:
from ..model.geometry import fit_plane_centroid
ofs = sum([_getOffsets(o) for o in objects],[])
pts = sum([_getBounds(o) for o in objects],[])
#print("Bounding box",bb,"center",center)
#raw_input()
#reset
viewport.camera.rot = [0.,0.,0.]
viewport.camera.tgt = [0.,0.,0.]
viewport.camera.dist = 6.0
viewport.clippingplanes = (0.2,20)
if len(ofs) == 0:
return
#print(pts)
#print(ofs)
pts = pts + ofs # just in case
bb = bb_create(*pts)
center = vectorops.mul(vectorops.add(bb[0],bb[1]),0.5)
viewport.camera.tgt = center
if zoom:
radius = max(vectorops.distance(bb[0],center),0.25)
viewport.camera.dist = 1.2*radius / math.tan(math.radians(viewport.fov*0.5))
#default: oblique view
if rotate:
viewport.camera.rot = [0,math.radians(30),math.radians(45)]
#fit a plane to these points
try:
centroid,normal = fit_plane_centroid(ofs)
except Exception as e:
try:
centroid,normal = fit_plane_centroid(pts)
except Exception as e:
warnings.warn("Exception occurred during fitting to points")
import traceback
traceback.print_exc()
raise
return
if normal[2] > 0:
normal = vectorops.mul(normal,-1)
z,x,y = so3.matrix(so3.inv(so3.canonical(normal)))
roll = 0
yaw = math.atan2(normal[0],normal[1])
pitch = math.atan2(-normal[2],vectorops.norm(normal[0:2]))
viewport.camera.rot = [roll,pitch,yaw]
else:
x = [1,0,0]
y = [0,0,1]
z = [0,1,0]
if zoom:
radius = max([abs(vectorops.dot(x,vectorops.sub(center,pt))) for pt in pts] + [abs(vectorops.dot(y,vectorops.sub(center,pt)))*viewport.w/viewport.h for pt in pts])
radius = max(radius,0.25)
zmin = min([vectorops.dot(z,vectorops.sub(center,pt)) for pt in pts])
zmax = max([vectorops.dot(z,vectorops.sub(center,pt)) for pt in pts])
#orient camera to point along normal direction
viewport.camera.tgt = center
viewport.camera.dist = 1.2*radius / math.tan(math.radians(viewport.fov*0.5))
near,far = viewport.clippingplanes
if viewport.camera.dist + zmin < near:
near = max((viewport.camera.dist + zmin)*0.5, radius*0.1)
if viewport.camera.dist + zmax > far:
far = max((viewport.camera.dist + zmax)*1.5, radius*3)
viewport.clippingplanes = (near,far)
[docs]def addText(name : str, text : str, position=None, **kwargs) -> None:
"""Adds text to the visualizer. You must give an identifier to all pieces
of text, which will be used to access the text as any other vis object.
Args:
name (str): the text's unique identifier.
text (str): the string to be drawn
pos (list, optional): the position of the string. If pos=None, this is
added to the on-screen "console" display. If pos has length 2, it
is the (x,y) position of the upper left corner of the text on the
screen. Negative units anchor the text to the right or bottom of
the window. If pos has length 3, the text is drawn in the world
coordinates.
kwargs (optional): optional keywords to give to setAppearance.
To customize the text appearance, you can set the 'color', 'size', and
'position' attributes, either through the keyword arguments, or using
setAttribute(). To refer to this item, use the identifier given in
``name``.
"""
_init()
if position is None:
scene().addText(name,text,**kwargs)
else:
scene().addText(name,text,position=position,**kwargs)
[docs]def clearText() -> None:
"""Clears all text in the visualization."""
scene().clearText()
[docs]def addPlot(name : str) -> None:
"""Creates a new empty plot with the identifier ``name``."""
add(name,VisPlot())
[docs]def addPlotItem(name : str, itemname : str) -> None:
"""Adds a scene item named ``itemname`` to the plot. All of the item's
configuration variables will be plotted by default, see
:func:`hidePlotItem` to turn off drawing some of these channels.
"""
scene().addPlotItem(name,itemname)
[docs]def logPlot(name : str, itemname : str,value) -> None:
"""Logs a custom visualization item to a plot. ``itemname`` can be an
arbitrary identifier; future logPlot calls with this itemname will add
values to the plotted curve.
"""
scene().logPlot(name,itemname,value)
[docs]def logPlotEvent(name : str,eventname : str,color=None):
"""Logs an event on the plot."""
scene().logPlotEvent(name,eventname,color)
[docs]def hidePlotItem(name : str, itemname : str,hidden=True):
"""Hides an item in the plot. To hide a particular channel of a given item
pass a pair ``(itemname,channelindex)``.
For example, to hide configurations 0-5 of 'robot', call::
hidePlotItem('plot',('robot',0))
...
hidePlotItem('plot',('robot',5))
"""
scene().hidePlotItem(name,itemname,hidden)
[docs]def setPlotDuration(name : str, time : float) -> None:
"""Sets the plot duration."""
setAttribute(name,'duration',time)
[docs]def setPlotRange(name : str, vmin : float, vmax : float) -> None:
"""Sets the y range of a plot to [vmin,vmax]."""
setAttribute(name,'range',(vmin,vmax))
[docs]def setPlotPosition(name : str, x : float, y : float) -> None:
"""Sets the upper left position of the plot on the screen."""
setAttribute(name,'position',(x,y))
[docs]def setPlotSize(name : str, w : int, h : int) -> None:
"""sets the width and height of the plot, in pixels."""
setAttribute(name,'size',(w,h))
[docs]def savePlot(name : str, fn : str) -> None:
"""Saves a plot to a CSV (extension .csv) or Trajectory (extension .traj) file."""
scene().savePlot(name,fn)
[docs]def autoFitCamera(zoom=True,rotate=True,scale=1) -> None:
"""Automatically fits the camera to all items in the visualization.
Args:
zoom (bool, optional): zooms the scene to the objects
rotate (bool, optional): rotates the scene to face the objects
scale (float, optional): a scale > 1 magnifies the camera zoom.
"""
print("klampt.vis: auto-fitting camera to scene.")
scene().autoFitCamera(zoom,rotate,scale)
[docs]def followCamera(target,translate=True,rotate=False,center=False) -> None:
"""Sets the camera to follow a target. The camera starts from its current
location and keeps the target in the same position on screen.
It can operate in the following modes:
- translation (``translate=True, rotate=False``): the camera moves with the
object. This is default.
- look-at (``translate=False, rotate=True``): the camera stays in the
current location but rotates to aim toward the object.
- follow (``translate=True, rotate=True``): the camera moves as though it
were fixed to the object.
Args:
target (str, SimRobotSensor, Trajectory, or None): the target that is
to be followed. If None, the camera stops following anything.
translate (bool, optional): whether the camera should follow using
translation.
rotate (bool, optional): whether the camera should follow using
rotation.
center (bool, optional): whether the camera should first aim toward the
object before following. Default is False.
"""
scene().followCamera(target,translate,rotate,center)
[docs]def getViewport() -> GLViewport:
"""Returns the :class:`GLViewport` of the current scene"""
return scene().getViewport()
[docs]def setViewport(viewport : GLViewport) -> None:
"""Sets the current scene to use a given :class:`GLViewport`"""
scene().setViewport(viewport)
[docs]def setBackgroundColor(r : float, g : float, b : float, a : float=1) -> None:
"""Sets the background color of the current scene."""
scene().setBackgroundColor(r,g,b,a)
[docs]def saveJsonConfig(fn : str=None) -> Union[dict,None]:
"""Saves the visualization options to a JSON object or file.
If fn is provided, it's saved to a file. Otherwise, it is returned.
"""
return scene().saveJsonConfig(fn)
[docs]def loadJsonConfig(jsonobj_or_fn : Union[dict,str]):
"""Loads the visualization options from a JSON object or file.
jsonobj_or_fn can either by a dict (previously obtained by saveJsonConfig
or a str indicating a filename (previously saved using saveJsonConfig.)
"""
return scene().loadJsonConfig(jsonobj_or_fn)
[docs]def screenshot(format='auto',want_depth=False):
"""Returns a screenshot of the scene. Currently only available in OpenGL modes
(PyQt, GLUT).
``format`` describes the desired image format.
- 'auto': equivalent to 'numpy' if numpy is available, 'Image' if PIL is
available, or 'bytes' otherwise.
- 'numpy': the image will be a numpy uint8 array of shape (h,w,3).
- 'Image': the image will be a PIL Image if Python Imaging Library is
available.
- 'bytes': the image will be in the form ``(w,h,bytes)`` where ``bytes`` is a
raw bytes array of length 4*w*h. The buffer follows OpenGL convention with
the start in the lower-left corner.
Can also provide a depth image if want_depth=True. The format will a (w,h)
numpy float array, a 'F' (float) Image, or (w,h,array) an array of floats.
In multithreaded mode, this will block until rendering is
complete.
"""
global _window_manager
if _window_manager is None:
return None
return _window_manager.screenshot(format,want_depth)
[docs]def screenshotCallback(fn : Callable, format='auto', want_depth=False) -> None:
"""Sets a callback ``fn`` that will receive a screenshot of the scene when
rendering is done.
See :func:`screenshot` for a description of the ``format`` and ``want_depth``
arguments.
Currently only available in OpenGL modes (PyQt, GLUT).
To repeatedly receive screenshots, ``fn`` can call
``vis.screenshotCallback(fn)`` again.
"""
global _window_manager
if _window_manager is None:
return None
return _window_manager.screenshotCallback(fn,format,want_depth)
[docs]def objectToVisType(item, world : WorldModel) -> str:
"""Returns the default type for the given item in the current world"""
itypes = types.object_to_types(item,world)
if isinstance(itypes,(list,tuple)):
#ambiguous, still need to figure out what to draw
validtypes = []
for t in itypes:
if t=='Vector3':
validtypes.append(t)
elif t == 'Config':
if world is not None:
match = False
for i in range(world.numRobots()):
if len(item) == world.robot(i).numLinks():
validtypes.append(t)
match = True
break
if not match and len(itypes) == 1:
warnings.warn("Config-like item of length {} doesn't match any of the # links of robots in the world: {}".format(len(item),[world.robot(i).numLinks() for i in range(world.numRobots())]))
elif t=='RigidTransform':
validtypes.append(t)
elif t=='Geometry3D':
validtypes.append(t)
elif t=='Trajectory':
validtypes.append(t)
if len(validtypes) > 1:
warnings.warn("Unable to draw item of ambiguous types {}\n (Try vis.setAttribute(item,'type',desired_type_str) to disambiguate)".format(validtypes))
return
if len(validtypes) == 0:
warnings.warn("Unable to draw any of types {}".format(itypes))
return
return validtypes[0]
return itypes
#_defaultCompressThreshold = 1e-2
_defaultCompressThreshold = 1e-3
[docs]class VisPlotItem:
def __init__(self,itemname,linkitem):
self.name = itemname
self.itemnames = []
self.linkitem = linkitem
self.traces = []
self.hidden = []
self.traceRanges = []
self.luminosity = []
self.compressThreshold = _defaultCompressThreshold
if linkitem is not None:
q = config.getConfig(linkitem.item)
assert q is not None
from collections import deque
self.traces = [deque() for i in range(len(q))]
self.itemnames = config.getConfigNames(linkitem.item)
[docs] def customUpdate(self,item,t,v):
for i,itemname in enumerate(self.itemnames):
if item == itemname:
self.updateTrace(i,t,v)
self.traceRanges[i] = (min(self.traceRanges[i][0],v),max(self.traceRanges[i][1],v))
return
else:
from collections import deque
self.itemnames.append(item)
self.traces.append(deque())
i = len(self.itemnames)-1
self.updateTrace(i,t,v)
self.traceRanges[i] = (min(self.traceRanges[i][0],v),max(self.traceRanges[i][1],v))
#raise ValueError("Invalid item specified: "+str(item))
[docs] def update(self,t):
if self.linkitem is None:
return
q = config.getConfig(self.linkitem.item)
assert len(self.traces) == len(q)
for i,v in enumerate(q):
self.updateTrace(i,t,v)
self.traceRanges[i] = (min(self.traceRanges[i][0],v),max(self.traceRanges[i][1],v))
[docs] def discard(self,tstart):
for t in self.traces:
if len(t)<=1: return
while len(t) >= 2:
if t[1][0] < tstart:
t.popleft()
else:
break
[docs] def updateTrace(self,i,t,v):
import random
assert i < len(self.traces)
assert i <= len(self.hidden)
assert i <= len(self.luminosity)
while i >= len(self.hidden):
self.hidden.append(False)
while i >= len(self.traceRanges):
self.traceRanges.append((v,v))
if i >= len(self.luminosity):
initialLuminosity = [0.5,0.25,0.75,1.0]
while i >= len(self.luminosity):
if len(self.luminosity)<len(initialLuminosity):
self.luminosity.append(initialLuminosity[len(self.luminosity)])
else:
self.luminosity.append(random.uniform(0,1))
trace = self.traces[i]
#trange = self.traceRanges[i][1] - self.traceRanges[i][0]
if len(trace) > 0 and trace[-1][0] >= t:
tsafe = trace[-1][0]+1e-8
if v != trace[-1][1]:
trace.append((tsafe,v))
return
trace[-1] = (tsafe,v)
return
if self.compressThreshold is None:
trace.append((t,v))
else:
if len(trace) < 2:
trace.append((t,v))
else:
pprev = trace[-2]
prev = trace[-1]
assert prev > pprev,"Added two items with the same time?"
assert t > prev[0]
slope_old = (prev[1]-pprev[1])/(prev[0]-pprev[0])
slope_new = (v-prev[1])/(t-prev[0])
if (slope_old > 0 != slope_new > 0) or abs(slope_old-slope_new) > self.compressThreshold:
trace.append((t,v))
else:
#near-linear, just extend along straight line
trace[-1] = (t,v)
[docs]class VisPlot:
def __init__(self):
self.items = []
self.colors = []
self.events = dict()
self.eventColors = dict()
self.outfile = None
self.outformat = None
def __del__(self):
self.endSave()
[docs] def update(self,t,duration,compressThreshold):
for i in self.items:
i.compressThreshold = compressThreshold
i.update(t)
if self.outfile:
self.dumpCurrent()
self.discard(t-duration)
else:
self.discard(t-60.0)
[docs] def discard(self,tmin):
for i in self.items:
i.discard(tmin)
delevents = []
for e,times in self.events.items():
while len(times) > 0 and times[0] < tmin:
times.popleft()
if len(times)==0:
delevents.append(e)
for e in delevents:
del self.events[e]
[docs] def addEvent(self,name,t,color=None):
if name in self.events:
self.events[name].append(t)
else:
from collections import deque
self.events[name] = deque([t])
if color is None:
import random
color = (random.uniform(0.01,1),random.uniform(0.01,1),random.uniform(0.01,1))
color = vectorops.mul(color,1.0/max(color))
if color is not None:
self.eventColors[name] = color
if len(color)==3:
self.eventColors[name] += [1.0]
[docs] def autoRange(self):
vmin = float('inf')
vmax = -float('inf')
for i in self.items:
for j in range(len(i.traceRanges)):
if not i.hidden[j]:
vmin = min(vmin,i.traceRanges[j][0])
vmax = max(vmax,i.traceRanges[j][1])
if math.isinf(vmin):
return (0.,1.)
if vmax == vmin:
vmax += 1.0
return (float(vmin),float(vmax))
[docs] def renderGL(self,window,x,y,w,h,duration,vmin=None,vmax=None):
if GL is None: raise RuntimeError("OpenGL wasn't initialized yet?")
if vmin is None:
vmin,vmax = self.autoRange()
import random
while len(self.colors) < len(self.items):
c = (random.uniform(0.01,1),random.uniform(0.01,1),random.uniform(0.01,1))
c = vectorops.mul(c,1.0/max(c))
self.colors.append(c)
GL.glColor3f(0,0,0)
GL.glBegin(GL.GL_LINE_LOOP)
GL.glVertex2f(x,y)
GL.glVertex2f(x+w,y)
GL.glVertex2f(x+w,y+h)
GL.glVertex2f(x,y+h)
GL.glEnd()
window.draw_text((x-18,y+4),'%.2f'%(vmax,),9)
window.draw_text((x-18,y+h+4),'%.2f'%(vmin,),9)
tmax = 0
for i in self.items:
for trace in i.traces:
if len(trace)==0: continue
tmax = max(tmax,trace[-1][0])
for i,item in enumerate(self.items):
for j,trace in enumerate(item.traces):
if len(trace)==0: continue
labelheight = trace[-1][1]
if len(item.name)==0:
label = item.itemnames[j]
else:
label = str(item.name) + '.' + item.itemnames[j]
labelheight = (labelheight - vmin)/(vmax-vmin)
labelheight = y + h - h*labelheight
GL.glColor3fv(vectorops.mul(self.colors[i],item.luminosity[j]))
window.draw_text((x+w+3,labelheight+4),label,9)
GL.glBegin(GL.GL_LINE_STRIP)
for k in range(len(trace)-1):
if trace[k+1][0] > tmax-duration:
u,v = trace[k]
if trace[k][0] < tmax-duration:
#interpolate so x is at tmax-duration
u2,v2 = trace[k+1]
#u + s(u2-u) = tmax-duration
s = (tmax-duration-u)/(u2-u)
v = v + s*(v2-v)
u = (tmax-duration)
u = (u-(tmax-duration))/duration
v = (v-vmin)/(vmax-vmin)
GL.glVertex2f(x+w*u,y+(1-v)*h)
u,v = trace[-1]
u = (u-(tmax-duration))/duration
v = (v-vmin)/(vmax-vmin)
GL.glVertex2f(x+w*u,y+(1-v)*h)
GL.glEnd()
if len(self.events) > 0:
for e,times in self.events.items():
for t in times:
if t < tmax-duration: continue
labelx = (t - (tmax-duration))/duration
labelx = x + w*labelx
c = self.eventColors[e]
GL.glColor4f(c[0]*0.5,c[1]*0.5,c[2]*0.5,c[3])
window.draw_text((labelx,y+h+12),e,9)
GL.glEnable(GL.GL_BLEND)
GL.glBlendFunc(GL.GL_SRC_ALPHA,GL.GL_ONE_MINUS_SRC_ALPHA)
GL.glBegin(GL.GL_LINES)
for e,times in self.events.items():
for t in times:
if t < tmax-duration: continue
labelx = (t - (tmax-duration))/duration
labelx = x + w*labelx
c = self.eventColors[e]
GL.glColor4f(c[0],c[1],c[2],c[3]*0.5)
GL.glVertex2f(labelx,y)
GL.glVertex2f(labelx,y+h)
GL.glEnd()
GL.glDisable(GL.GL_BLEND)
[docs] def beginSave(self,fn):
import os
ext = os.path.splitext(fn)[1]
if ext == '.csv' or ext == '.traj':
self.outformat = ext
else:
raise ValueError("Invalid extension for visualization plot, can only accept .csv or .traj")
self.outfile = open(fn,'w')
if self.outformat == '.csv':
#output a header
self.outfile.write("time")
for i in self.items:
self.outfile.write(",")
fullitemnames = []
if len(i.name) != 0:
name = None
if isinstance(i.name,(list,tuple)):
name = '.'.join(v for v in i.name)
else:
name = i.name
fullitemnames = [name+'.'+itemname for itemname in i.itemnames]
else:
fullitemnames = i.itemnames
self.outfile.write(",".join(fullitemnames))
self.outfile.write("\n")
self.dumpAll()
[docs] def endSave(self):
if self.outfile is not None:
self.outfile.close()
[docs] def dumpAll(self):
assert self.outfile is not None
if len(self.items) == 0: return
cols = []
mindt = float('inf')
mint = float('inf')
maxt = -float('inf')
for i in self.items:
if len(i.traces) == 0:
continue
for j,trace in enumerate(i.traces):
times,vals = list(zip(*trace))
if isinstance(vals[0],(int,float)):
vals = [[v] for v in vals]
traj = Trajectory(times,vals)
cols.append(traj)
mint = min(mint,traj.times[0])
maxt = max(maxt,traj.times[-1])
for k in range(len(traj.times)-1):
mindt = min(mindt,traj.times[k+1] - traj.times[k])
assert mindt > 0, "For some reason, there is a duplicate time?"
N = int((maxt - mint)/mindt)
dt = (maxt - mint)/N
times = [mint + i*(maxt-mint)/N for i in range(N+1)]
for i in range(N+1):
vals = [col.eval(times[i]) for col in cols]
if self.outformat == '.csv':
self.outfile.write(str(times[i])+',')
self.outfile.write(','.join([str(v[0]) for v in vals]))
self.outfile.write('\n')
else:
self.outfile.write(str(times[i])+'\t')
self.outfile.write(str(len(vals))+' ')
self.outfile.write(' '.join([str(v[0]) for v in vals]))
self.outfile.write('\n')
[docs] def dumpCurrent(self):
if len(self.items) == 0: return
assert len(self.items[0].traces) > 0, "Item has no channels?"
assert len(self.items[0].traces[0]) > 0, "Item has no readings yet?"
t = self.items[0].traces[0][-1]
vals = []
for i in self.items:
if len(i.traces) == 0:
continue
for j,trace in enumerate(i.traces):
vals.append(trace[-1][1])
if self.outformat == '.csv':
self.outfile.write(str(t)+',')
self.outfile.write(','.join([str(v) for v in vals]))
self.outfile.write('\n')
else:
self.outfile.write(str(t)+'\t')
self.outfile.write(str(len(vals))+' ')
self.outfile.write(' '.join([str(v) for v in vals]))
self.outfile.write('\n')
[docs]def drawTrajectory(traj,width,color,pointSize=None,pointColor=None):
"""Draws a trajectory of points or transforms.
By default draws points along the trajectory. To turn this off, set
pointSize = 0.
"""
if GL is None: raise RuntimeError("OpenGL not initialized?")
if isinstance(traj,list):
if len(traj)==0:
return
if pointSize is None:
pointSize = width+2
if pointColor is None:
pointColor = (color[0]*0.75,color[1]*0.75,color[2]*0.75,color[3])
#R3 trajectory
GL.glDisable(GL.GL_LIGHTING)
GL.glColor4f(*color)
if len(traj) == 1:
GL.glPointSize(max(width,pointSize))
GL.glBegin(GL.GL_POINTS)
GL.glVertex3fv(traj[0])
GL.glEnd()
if len(traj) >= 2 and width > 0:
GL.glLineWidth(width)
GL.glBegin(GL.GL_LINE_STRIP)
for p in traj:
GL.glVertex3fv(p)
GL.glEnd()
GL.glLineWidth(1.0)
if len(traj) >= 2 and pointSize > 0:
GL.glColor4f(*pointColor)
GL.glPointSize(pointSize)
GL.glBegin(GL.GL_POINTS)
for p in traj:
GL.glVertex3fv(p)
GL.glEnd()
elif isinstance(traj,SE3Trajectory):
pointTraj = []
for m in traj.milestones:
pointTraj.append(m[9:12])
drawTrajectory(pointTraj,width,color,pointSize,pointColor)
elif isinstance(traj,SE3HermiteTrajectory):
pointTraj = []
velTraj = []
for m in traj.milestones:
pointTraj.append(m[9:12])
velTraj.append(m[21:24])
drawTrajectory(HermiteTrajectory(traj.times,pointTraj,velTraj),width,color,pointSize,pointColor)
else:
if len(traj.milestones)==0:
return
wp = traj.waypoint(traj.milestones[0])
if len(wp) == 3:
if len(wp) == len(traj.milestones[0]):
drawTrajectory(traj.milestones,width,color,pointSize,pointColor)
else: #discrepancy, must be hermite
if width > 0:
discretized = traj.discretize(traj.duration()/len(traj.milestones)*0.1)
drawTrajectory(discretized.milestones,width,color,0,None)
if pointSize is None or pointSize > 0:
drawTrajectory([traj.waypoint(m) for m in traj.milestones],0,color,pointSize,pointColor)
elif len(wp) == 2:
#R2 trajectory
if len(wp) == len(traj.milestones[0]):
drawTrajectory([v + [0.0] for v in traj.milestones],width,color,pointSize,pointColor)
else: #discrepancy, must be hermite
if width > 0:
discretized = traj.discretize(traj.duration()/len(traj.milestones)*0.1)
drawTrajectory([m + [0.0] for m in discretized.milestones],width,color,0,None)
if pointSize is None or pointSize > 0:
drawTrajectory([traj.waypoint(m) + [0.0] for m in traj.milestones],0,color,pointSize,pointColor)
[docs]def drawRobotTrajectory(traj,robot,ees,width=2,color=(1,0.5,0,1),pointSize=None,pointColor=None):
"""Draws trajectories for the robot's end effectors. Note: no additional discretization is performed,
only the end effector points at the trajectory's milestones are shown. If you want more accurate trajectories,
first call traj.discretize(eps)."""
for i,ee in enumerate(ees):
if ee < 0: ees[i] = robot.numLinks()-1
pointTrajectories = []
for ee in ees:
pointTrajectories.append([])
if isinstance(traj,Trajectory):
traj = traj.milestones
for m in traj:
robot.setConfig(m)
for ee,eetraj in zip(ees,pointTrajectories):
eetraj.append(robot.link(ee).getTransform()[1])
for ptraj in pointTrajectories:
drawTrajectory(ptraj,width,color,pointSize,pointColor)
class _CascadingDict:
"""A hierarchical dictionary structure that can be defined with respect
to a parent dict or _CascadingDict. Items in this dict override the
items in the parent. Deleting an item from this dict does not delete
from the parent.
Be careful when modifying sub-items of top-level keys. You may not know
whether the key accesses the parent or this object::
parent = {'foo':[1,2,3]}
obj = _CascadingDict(parent=parent)
obj['foo'][0] = 'hello' #this actually changes the value of parent['foo']
obj2 = _CascadingDict(parent=parent)
print(obj2['foo']) #prints ['hello',2,3]
obj['foo'] = 4 #modifies the object's key 'foo'
print(obj2['foo']) #still prints ['hello',2,3], since the non-overridden key
#is still pointing to parent
"""
def __init__(self,rhs=None,parent=None):
if rhs is not None:
if isinstance(rhs,_CascadingDict):
self.overrides = rhs
else:
self.overrides = dict(rhs)
else:
self.overrides = dict()
self.parent = None
def setParent(self,parent):
self.parent = parent
def __getitem__(self,item):
try:
return self.overrides[item]
except KeyError:
if self.parent is None:
raise
else:
return self.parent[item]
def __setitem__(self,key,value):
self.overrides[key] = value
def __delitem__(self,key):
del self.overrides[key]
def get(self,item,default=None):
try:
return self.__getitem__(item)
except KeyError:
return default
def flatten(self):
"""Returns a normal dict containing all items in this or its parents"""
pdict = {}
if isinstance(self.parent,_CascadingDict):
pdict = self.parent.flatten()
elif isinstance(self.parent,dict):
pdict = self.parent.copy()
for k,v in self.overrides.items():
pdict[k] = v
return pdict
def __str__(self):
return str(self.flatten())
def __repr__(self):
return repr(self.flatten())
def __contains__(self,item):
if item in self.overrides:
return True
if self.parent is None:
return False
return item in self.parent
_default_str_attributes = {'color':[0,0,0,1], 'position':None, 'size':12 }
_default_Trajectory_attributes = { 'robot':0, "width":3, "color":(1,0.5,0,1), "pointSize":None, "pointColor":None }
_default_RobotTrajectory_attributes = { 'robot':0, "width":3, "color":(1,0.5,0,1), "pointSize":None, "pointColor":None , "endeffectors":[-1]}
_default_VisPlot_attributes = {'compress':_defaultCompressThreshold, 'duration':5., 'position':None, 'range':(None,None), 'size':(200,150), 'hide_label':True}
_default_Point_attributes = { "size":5.0, "color":(0,0,0,1) }
_default_Direction_attributes = { "length":0.15, "color":[0,1,1,1] }
_default_Frame_attributes = { "length":0.1, "width":0.01 }
_default_ContactPoint_attributes = { "size":5.0, "length":0.05, "color":[1,0.5,0,1] }
_default_IKObjective_attributes = { "size":5.0, "color":(0,0,0,1), "length":0.1, "width": 0.01, "axis_color":[0.5,0,0.5,1], "axis_width":3.0, "axis_length":0.1 }
_default_Geometry_attributes = { "size":None, "color":None }
_default_RigidTransform_attributes = { "fancy":False, "length":0.1, "width":0.01 }
def _default_attributes(item,type=None):
"""Returns an attribute dictionary for the defaults of a given item.
If the item is ambiguous, you can provide the type argument.
"""
res = {}
if isinstance(item,str):
return _default_str_attributes
elif isinstance(item,(WorldModel,RobotModel)):
pass
elif hasattr(item,'appearance'):
res['color'] = item.appearance().getColor()
pass
elif isinstance(item,(Trajectory,MultiPath)):
if isinstance(item,RobotTrajectory):
return _default_RobotTrajectory_attributes
else:
return _default_Trajectory_attributes
elif isinstance(item,VisPlot):
return _default_VisPlot_attributes
elif isinstance(item,coordinates.Point):
return _default_Point_attributes
elif isinstance(item,coordinates.Direction):
return _default_Direction_attributes
elif isinstance(item,coordinates.Frame):
return _default_Frame_attributes
elif isinstance(item,coordinates.Transform):
pass
elif isinstance(item,coordinates.Group):
res['hide_label'] = True
elif isinstance(item,ContactPoint):
return _default_ContactPoint_attributes
elif isinstance(item,Hold):
pass
elif isinstance(item,IKObjective):
return _default_IKObjective_attributes
elif isinstance(item,(GeometricPrimitive,TriangleMesh,PointCloud,Geometry3D)):
return _default_Geometry_attributes
else:
if type is not None:
itypes = type
else:
try:
itypes = objectToVisType(item,None)
res["type"]=itypes
except Exception as e:
if hasattr(item,'drawGL'):
#assume it's a SimRobotSensor, Appearance, or SubRobotModel
return
warnings.warn(str(e))
warnings.warn("Unsupported object type {} of type {}".format(item,item.__class__.__name__))
return
if itypes is None:
warnings.warn("Unable to convert item {} to drawable".format(str(item)))
return
elif itypes == 'Config':
pass
elif itypes == 'Configs':
res["maxConfigs"] = min(10,len(item))
elif itypes == 'Vector3':
return _default_Point_attributes
elif itypes == 'RigidTransform':
return _default_RigidTransform_attributes
else:
warnings.warn("klampt.vis: Unable to draw item of type \"%s\""%(str(itypes),))
res['hidden'] = True
res['hide_label'] = True
return res
[docs]class VisAppearance:
"""The core class that governs all of the drawing of an object
in the visualization. Can accommodate any drawable Klampt type.
"""
def __init__(self,item,name = None, type=None):
self.name = name
self.useDefaultAppearance = True
self.customAppearance = None
self.customDrawFunc = None
#For group items, this allows you to customize appearance of sub-items
self.subAppearances = {}
self.animation = None
self.animationStartTime = 0
self.animationSpeed = 1.0
self.attributes = _default_attributes(item,type)
if not isinstance(self.attributes,_CascadingDict):
self.attributes = _CascadingDict(self.attributes)
if 'hide_label' not in self.attributes:
self.attributes['hide_label'] = False
if 'hidden' not in self.attributes:
self.attributes['hidden'] = False
self.attributes['label'] = name
#used for Qt text rendering
self.widget = None
#used for visual editing of certain items
self.editor = None
#cached drawing
self.displayCache = [glcommon.CachedGLObject()]
self.displayCache[0].name = name
#temporary configuration of the item
self.drawConfig = None
self.transformChanged = False
self.setItem(item)
[docs] def setItem(self,item):
self.item = item
self.subAppearances = {}
#Parse out sub-items which can have their own appearance changed
if isinstance(item,WorldModel):
for i in range(item.numRobots()):
self.subAppearances[("Robot",i)] = VisAppearance(item.robot(i),item.robot(i).getName())
for i in range(item.numRigidObjects()):
self.subAppearances[("RigidObject",i)] = VisAppearance(item.rigidObject(i),item.rigidObject(i).getName())
for i in range(item.numTerrains()):
self.subAppearances[("Terrain",i)] = VisAppearance(item.terrain(i),item.terrain(i).getName())
elif isinstance(item,RobotModel):
for i in range(item.numLinks()):
self.subAppearances[("Link",i)] = VisAppearance(item.link(i),item.link(i).getName())
elif isinstance(item,coordinates.Group):
for n,f in item.frames.items():
self.subAppearances[("Frame",n)] = VisAppearance(f,n)
for n,p in item.points.items():
self.subAppearances[("Point",n)] = VisAppearance(p,n)
for n,d in item.directions.items():
self.subAppearances[("Direction",n)] = VisAppearance(d,n)
for n,g in item.subgroups.items():
self.subAppearances[("Subgroup",n)] = VisAppearance(g,n)
elif isinstance(item,Hold):
if item.ikConstraint is not None:
self.subAppearances["ikConstraint"] = VisAppearance(item.ikConstraint,"ik")
for n,c in enumerate(item.contacts):
self.subAppearances[("contact",n)] = VisAppearance(c,n)
[docs] def markChanged(self,config=True,appearance=True):
if appearance:
for c in self.displayCache:
c.mark_changed()
for (k,a) in self.subAppearances.items():
a.markChanged(config,appearance)
if config:
self.update_editor(True)
self.transformChanged = True
[docs] def destroy(self):
for c in self.displayCache:
c.destroy()
for (k,a) in self.subAppearances.items():
a.destroy()
self.subAppearances = {}
[docs] def drawText(self,text,point):
"""Draws the given text at the given point"""
if len(point) != 3:
warnings.warn("drawText INCORRECT POINT SIZE {} {}".format(point,text))
return
if not all(math.isfinite(v) for v in point):
warnings.warn("drawText INVALID POINT {} {}".format(point,text))
return
self.widget.addLabel(text,point[:],[0,0,0])
[docs] def updateAnimation(self,t):
"""Updates the configuration, if it's being animated"""
if not self.animation:
if self.drawConfig is not None:
self.markChanged(config=True,appearance=False)
self.drawConfig = None
else:
u = self.animationSpeed*(t-self.animationStartTime)
q = self.animation.eval(u,self.animationEndBehavior)
self.drawConfig = config.getConfig(q)
self.markChanged(config=True,appearance=False)
for n,app in self.subAppearances.items():
app.updateAnimation(t)
[docs] def updateTime(self,t):
"""Updates in real time"""
if isinstance(self.item,VisPlot):
compressThreshold = self.attributes['compress']
duration = self.attributes['duration']
for items in self.item.items:
if items.linkitem:
items.linkitem.swapDrawConfig()
self.item.update(t,duration,compressThreshold)
for items in self.item.items:
if items.linkitem:
items.linkitem.swapDrawConfig()
[docs] def swapDrawConfig(self):
"""Given self.drawConfig!=None, swaps out the item's curren
configuration with self.drawConfig. Used for animations"""
if self.drawConfig:
try:
newDrawConfig = config.getConfig(self.item)
if len(newDrawConfig) != len(self.drawConfig):
warnings.warn("Incorrect length of draw configuration? {} vs {}".format(len(self.drawConfig),len(newDrawConfig)))
config.setConfig(self.item,self.drawConfig)
self.drawConfig = newDrawConfig
except Exception as e:
warnings.warn("Exception thrown during animation update. Probably have incorrect length of configuration")
import traceback
traceback.print_exc()
self.animation = None
pass
for n,app in self.subAppearances.items():
app.swapDrawConfig()
[docs] def clearDisplayLists(self):
if isinstance(self.item,WorldModel):
for r in range(self.item.numRobots()):
for link in range(self.item.robot(r).numLinks()):
self.item.robot(r).link(link).appearance().refresh()
for i in range(self.item.numRigidObjects()):
self.item.rigidObject(i).appearance().refresh()
for i in range(self.item.numTerrains()):
self.item.terrain(i).appearance().refresh()
elif hasattr(self.item,'appearance'):
self.item.appearance().refresh()
elif isinstance(self.item,RobotModel):
for link in range(self.item.numLinks()):
self.item.link(link).appearance().refresh()
for n,o in self.subAppearances.items():
o.clearDisplayLists()
self.markChanged(config=False,appearance=True)
[docs] def transparent(self):
"""Returns true if the item is entirely transparent, None if mixed transparency, and False otherwise"""
if len(self.subAppearances)!=0:
anyTransp = False
anyOpaque = False
for n,app in self.subAppearances.items():
if app.transparent():
anyTransp = True
else:
anyOpaque = True
if anyTransp and anyOpaque:
return None
else:
return anyTransp
if hasattr(self.item,'appearance'):
if self.useDefaultAppearance or 'color' not in self.attributes:
if isinstance(self.item,WorldModel):
#corner case: empty world
return False
else:
return self.item.appearance().getColor()[3] < 1.0
try:
return (self.attributes['color'][3] < 1.0)
except:
return False
[docs] def getAttributes(self):
if len(self.subAppearances) > 0:
return {}
return self.attributes.flatten()
[docs] def drawGL(self,world=None,viewport=None,draw_transparent=None):
"""Draws the specified item in the specified world, with all the
current modifications in attributes.
If a name or label are given, and
``self.attributes['hide_label'] != False``, then the label is shown.
The drawing passes are controlled by ``draw_transparent`` -- opaque
items should be rasterized before transparent ones.
Args:
world (WorldModel): the world model
viewport (Viewport): the C++ viewport of the current view, which is
compatible with the Klampt C++ Widget class.
draw_transparent (bool or None): If None, then everything is drawn.
If True, then only transparent items are drawn. If False, then
only opaque items are drawn. (This only affects WorldModels)
"""
if self.attributes["hidden"]:
return
if self.customDrawFunc is not None:
self.customDrawFunc(self.item)
return
item = self.item
name = None
if not self.attributes["hide_label"]:
name = self.attributes['label']
#set appearance
if not self.useDefaultAppearance and hasattr(item,'appearance'):
if not hasattr(self,'oldAppearance'):
self.oldAppearance = item.appearance().clone()
if self.customAppearance is not None:
item.appearance().set(self.customAppearance)
elif "color" in self.attributes:
item.appearance().setColor(*self.attributes["color"])
if self.editor is not None:
assert viewport is not None
#some weird logic to determine whether to draw now...
if draw_transparent is None or draw_transparent == self.transparent() or (self.transparent() is None and draw_transparent==False):
#KLUDGE:
#might be a robot, make sure the appearances are all up to date before drawing the editor
for n,app in self.subAppearances.items():
if app.useDefaultAppearance or not hasattr(app.item,'appearance'):
continue
if not hasattr(app,'oldAppearance'):
app.oldAppearance = app.item.appearance().clone()
if app.customAppearance is not None:
app.item.appearance().set(app.customAppearance)
elif "color" in app.attributes:
app.item.appearance().setColor(*app.attributes["color"])
if isinstance(self.editor,RobotPoser) and isinstance(self.item,(list,tuple)) and world is not None:
#KLUDGE: config editor; make sure that the robot in the world model is restored
#use the
robot = world.robot(0)
oldconfig = robot.getConfig()
oldAppearances = []
if "color" in self.attributes:
for i in range(robot.numLinks()):
oldAppearances.append(robot.link(i).appearance().clone())
robot.link(i).appearance().setColor(*self.attributes['color'])
else:
oldconfig = None
self.editor.drawGL(viewport)
if oldconfig is not None:
world.robot(0).setConfig(oldconfig)
if len(oldAppearances) > 0:
for i in range(robot.numLinks()):
robot.link(i).appearance().set(oldAppearances[i])
#Restore sub-appearances
for n,app in self.subAppearances.items():
if app.useDefaultAppearance or not hasattr(app.item,'appearance'):
continue
app.item.appearance().set(app.oldAppearance)
if isinstance(self.editor,RobotPoser):
#the widget took care of everything, dont continue drawing the item
#revert the robot's actual pose
#revert appearance if necessary
if not self.useDefaultAppearance and hasattr(item,'appearance'):
item.appearance().set(self.oldAppearance)
return
if len(self.subAppearances)!=0:
for n,app in self.subAppearances.items():
if draw_transparent is True:
if not app.transparent():
continue
elif draw_transparent is False:
if app.transparent():
continue
app.widget = self.widget
app.drawGL(world,viewport,draw_transparent)
elif hasattr(item,'drawGL'):
item.drawGL()
elif hasattr(item,'drawWorldGL'):
item.drawWorldGL()
elif isinstance(item,str):
pos = self.attributes["position"]
if pos is not None and len(pos)==3:
col = self.attributes["color"]
self.widget.addLabel(self.item,pos,col)
elif isinstance(item,VisPlot):
pass
elif isinstance(item,Trajectory):
doDraw = False
centroid = None
if len(item.milestones) == 0:
return
robot = (world.robot(self.attributes["robot"]) if world is not None and world.numRobots() > 0 else None)
if robot is not None:
robotConfig = robot.getConfig()
treatAsRobotTrajectory = (item.__class__ == Trajectory and len(item.milestones) > 0 and robot and len(item.milestones[0]) == robot.numLinks())
if isinstance(item,RobotTrajectory) or treatAsRobotTrajectory:
ees = self.attributes.get("endeffectors",[-1])
if world:
doDraw = (len(ees) > 0)
for i,ee in enumerate(ees):
if ee < 0: ees[i] = robot.numLinks()-1
if doDraw:
robot.setConfig(item.milestones[0])
centroid = vectorops.div(vectorops.add(*[robot.link(ee).getTransform()[1] for ee in ees]),len(ees))
elif isinstance(item,(SE3Trajectory,SE3HermiteTrajectory)):
doDraw = True
centroid = item.waypoint(item.milestones[0])[1]
else:
wp = item.waypoint(item.milestones[0])
if len(wp) == 3:
#R3 trajectory
doDraw = True
centroid = wp
elif len(item.waypoint(item.milestones[0])) == 2:
#R2 trajectory
doDraw = True
centroid = wp+[0.0]
else:
#don't know how to interpret this trajectory
pass
if doDraw:
assert len(centroid)==3
def drawRaw():
width = self.attributes["width"]
color = self.attributes["color"]
pointSize = self.attributes["pointSize"]
pointColor = self.attributes["pointColor"]
if isinstance(item,RobotTrajectory) or treatAsRobotTrajectory:
ees = self.attributes.get("endeffectors",[-1])
drawRobotTrajectory(item,robot,ees,width,color,pointSize,pointColor)
else:
drawTrajectory(item,width,color,pointSize,pointColor)
self.displayCache[0].draw(drawRaw)
if name is not None:
self.drawText(name,centroid)
if robot is not None:
robot.setConfig(robotConfig)
elif isinstance(item,MultiPath):
robot = (world.robot(self.attributes["robot"]) if world is not None and world.numRobots() > 0 else None)
if robot is not None and item.numSections() > 0:
if len(item.sections[0].configs[0]) == robot.numLinks():
ees = self.attributes.get("endeffectors",[-1])
centroid = None
if len(ees) > 0:
for i,ee in enumerate(ees):
if ee < 0: ees[i] = robot.numLinks()-1
robot.setConfig(item.sections[0].configs[0])
centroid = vectorops.div(vectorops.add(*[robot.link(ee).getTransform()[1] for ee in ees]),len(ees))
width = self.attributes["width"]
color = self.attributes["color"]
pointSize = self.attributes["pointSize"]
pointColor = self.attributes["pointColor"]
color2 = [1-c for c in color]
color2[3] = color[3]
def drawRaw():
for i,s in enumerate(item.sections):
drawRobotTrajectory(s.configs,robot,ees,width,(color if i%2 == 0 else color2),pointSize,pointColor)
#draw it!
self.displayCache[0].draw(drawRaw)
if name is not None and centroid is not None:
self.drawText(name,centroid)
elif isinstance(item,coordinates.Point):
def drawRaw():
GL.glDisable(GL.GL_LIGHTING)
GL.glEnable(GL.GL_POINT_SMOOTH)
GL.glPointSize(self.attributes["size"])
GL.glColor4f(*self.attributes["color"])
GL.glBegin(GL.GL_POINTS)
GL.glVertex3f(0,0,0)
GL.glEnd()
#write name
GL.glDisable(GL.GL_DEPTH_TEST)
self.displayCache[0].draw(drawRaw,[so3.identity(),item.worldCoordinates()])
GL.glEnable(GL.GL_DEPTH_TEST)
if name is not None:
self.drawText(name,item.worldCoordinates())
elif isinstance(item,coordinates.Direction):
def drawRaw():
GL.glDisable(GL.GL_LIGHTING)
GL.glDisable(GL.GL_DEPTH_TEST)
L = self.attributes["length"]
source = [0,0,0]
GL.glColor4f(*self.attributes["color"])
GL.glBegin(GL.GL_LINES)
GL.glVertex3f(*source)
GL.glVertex3f(*vectorops.mul(item.localCoordinates(),L))
GL.glEnd()
GL.glEnable(GL.GL_DEPTH_TEST)
#write name
self.displayCache[0].draw(drawRaw,item.frame().worldCoordinates(),parameters = item.localCoordinates())
if name is not None:
self.drawText(name,vectorops.add(item.frame().worldCoordinates()[1],item.worldCoordinates()))
elif isinstance(item,coordinates.Frame):
t = item.worldCoordinates()
if item.parent() is not None:
tp = item.parent().worldCoordinates()
else:
tp = se3.identity()
tlocal = item.relativeCoordinates()
def drawRaw():
GL.glDisable(GL.GL_DEPTH_TEST)
GL.glDisable(GL.GL_LIGHTING)
GL.glLineWidth(2.0)
gldraw.xform_widget(tlocal,self.attributes["length"],self.attributes["width"])
GL.glLineWidth(1.0)
#draw curve between frame and parent
if item.parent() is not None:
d = vectorops.norm(tlocal[1])
vlen = d*0.5
v1 = so3.apply(tlocal[0],[-vlen]*3)
v2 = [vlen]*3
#GL.glEnable(GL.GL_BLEND)
#GL.glBlendFunc(GL.GL_SRC_ALPHA,GL.GL_ONE_MINUS_SRC_ALPHA)
#GL.glColor4f(1,1,0,0.5)
GL.glColor3f(1,1,0)
gldraw.hermite_curve(tlocal[1],v1,[0,0,0],v2,0.03*max(0.1,vectorops.norm(tlocal[1])))
#GL.glDisable(GL.GL_BLEND)
GL.glEnable(GL.GL_DEPTH_TEST)
#For some reason, cached drawing is causing OpenGL problems
#when the frame is rapidly changing
self.displayCache[0].draw(drawRaw,transform=tp, parameters = tlocal)
#GL.glPushMatrix()
#GL.glMultMatrixf(sum(zip(*se3.homogeneous(tp)),()))
#drawRaw()
#GL.glPopMatrix()
#write name
if name is not None:
self.drawText(name,t[1])
elif isinstance(item,coordinates.Transform):
#draw curve between frames
t1 = item.source().worldCoordinates()
if item.destination() is not None:
t2 = item.destination().worldCoordinates()
else:
t2 = se3.identity()
d = vectorops.distance(t1[1],t2[1])
vlen = d*0.5
v1 = so3.apply(t1[0],[-vlen]*3)
v2 = so3.apply(t2[0],[vlen]*3)
def drawRaw():
GL.glDisable(GL.GL_DEPTH_TEST)
GL.glDisable(GL.GL_LIGHTING)
GL.glColor3f(1,1,1)
gldraw.hermite_curve(t1[1],v1,t2[1],v2,0.03)
GL.glEnable(GL.GL_DEPTH_TEST)
#write name at curve
self.displayCache[0].draw(drawRaw,transform=None,parameters = (t1,t2))
if name is not None:
self.drawText(name,spline.hermite_eval(t1[1],v1,t2[1],v2,0.5))
elif isinstance(item,coordinates.Group):
pass
elif isinstance(item,ContactPoint):
def drawRaw():
GL.glDisable(GL.GL_LIGHTING)
GL.glEnable(GL.GL_POINT_SMOOTH)
GL.glPointSize(self.attributes["size"])
l = self.attributes["length"]
GL.glColor4f(*self.attributes["color"])
GL.glBegin(GL.GL_POINTS)
GL.glVertex3f(0,0,0)
GL.glEnd()
GL.glBegin(GL.GL_LINES)
GL.glVertex3f(0,0,0)
GL.glVertex3f(l,0,0)
GL.glEnd()
self.displayCache[0].draw(drawRaw,[so3.canonical(item.n),item.x])
elif isinstance(item,Hold):
pass
elif isinstance(item,IKObjective):
if hasattr(item,'robot'):
#need this to be built with a robot element.
#Otherwise, can't determine the correct transforms
robot = item.robot
elif world:
if world is not None and world.numRobots() > 0:
robot = world.robot(self.attributes.get("robot",0))
else:
robot = None
else:
robot = None
if robot is not None:
link = robot.link(item.link())
dest = robot.link(item.destLink()) if item.destLink()>=0 else None
while len(self.displayCache) < 3:
self.displayCache.append(glcommon.CachedGLObject())
self.displayCache[1].name = self.name+" target position"
self.displayCache[2].name = self.name+" curve"
if item.numPosDims() != 0:
lp,wp = item.getPosition()
#set up parameters of connector
p1 = se3.apply(link.getTransform(),lp)
if dest is not None:
p2 = se3.apply(dest.getTransform(),wp)
else:
p2 = wp
d = vectorops.distance(p1,p2)
v1 = [0.0]*3
v2 = [0.0]*3
if item.numRotDims()==3: #full constraint
R = item.getRotation()
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes["length"],self.attributes["width"])
t1 = se3.mul(link.getTransform(),(so3.identity(),lp))
t2 = (R,wp) if dest==None else se3.mul(dest.getTransform(),(R,wp))
self.displayCache[0].draw(drawRaw,transform=t1)
self.displayCache[1].draw(drawRaw,transform=t2)
vlen = d*0.1
v1 = so3.apply(t1[0],[-vlen]*3)
v2 = so3.apply(t2[0],[vlen]*3)
elif item.numRotDims()==0: #point constraint
def drawRaw():
GL.glDisable(GL.GL_LIGHTING)
GL.glEnable(GL.GL_POINT_SMOOTH)
GL.glPointSize(self.attributes["size"])
GL.glColor4f(*self.attributes["color"])
GL.glBegin(GL.GL_POINTS)
GL.glVertex3f(0,0,0)
GL.glEnd()
self.displayCache[0].draw(drawRaw,transform=(so3.identity(),p1))
self.displayCache[1].draw(drawRaw,transform=(so3.identity(),p2))
#set up the connecting curve
vlen = d*0.5
d = vectorops.sub(p2,p1)
v1 = vectorops.mul(d,0.5)
#curve in the destination
v2 = vectorops.cross((0,0,0.5),d)
else: #hinge constraint
p = [0,0,0]
d = [0,0,0]
def drawRawLine():
GL.glDisable(GL.GL_LIGHTING)
GL.glEnable(GL.GL_POINT_SMOOTH)
GL.glPointSize(self.attributes["size"])
GL.glColor4f(*self.attributes["color"])
GL.glBegin(GL.GL_POINTS)
GL.glVertex3f(*p)
GL.glEnd()
GL.glColor4f(*self.attributes["color"])
GL.glLineWidth(self.attributes["width"])
GL.glBegin(GL.GL_LINES)
GL.glVertex3f(*p)
GL.glVertex3f(*vectorops.madd(p,d,self.attributes["length"]))
GL.glEnd()
GL.glLineWidth(1.0)
ld,wd = item.getRotationAxis()
p = lp
d = ld
self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=(p,d))
p = wp
d = wd
self.displayCache[1].draw(drawRawLine,transform=dest.getTransform() if dest else se3.identity(),parameters=(p,d))
#set up the connecting curve
d = vectorops.sub(p2,p1)
v1 = vectorops.mul(d,0.5)
#curve in the destination
v2 = vectorops.cross((0,0,0.5),d)
def drawConnection():
GL.glDisable(GL.GL_LIGHTING)
GL.glDisable(GL.GL_DEPTH_TEST)
GL.glColor3f(1,0.5,0)
gldraw.hermite_curve(p1,v1,p2,v2,0.03*max(0.1,vectorops.distance(p1,p2)))
#GL.glBegin(GL.GL_LINES)
#GL.glVertex3f(*p1)
#GL.glVertex3f(*p2)
#GL.glEnd()
GL.glEnable(GL.GL_DEPTH_TEST)
#TEMP for some reason the cached version sometimes gives a GL error
self.displayCache[2].draw(drawConnection,transform=None,parameters = (p1,v1,p2,v2))
#drawConnection()
if name is not None:
self.drawText(name,wp)
else:
wp = link.getTransform()[1]
if item.numRotDims()==3: #full constraint
R = item.getRotation()
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes["length"],self.attributes["width"])
self.displayCache[0].draw(drawRaw,transform=link.getTransform())
self.displayCache[1].draw(drawRaw,transform=se3.mul(link.getTransform(),(R,[0,0,0])))
elif item.numRotDims() > 0:
#axis constraint
d = [0,0,0]
def drawRawLine():
GL.glDisable(GL.GL_LIGHTING)
GL.glColor4f(*self.attributes["axis_color"])
GL.glLineWidth(self.attributes["axis_width"])
GL.glBegin(GL.GL_LINES)
GL.glVertex3f(0,0,0)
GL.glVertex3f(*vectorops.mul(d,self.attributes["axis_length"]))
GL.glEnd()
GL.glLineWidth(1.0)
ld,wd = item.getRotationAxis()
d = ld
self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=d)
d = wd
self.displayCache[1].draw(drawRawLine,transform=(dest.getTransform()[0] if dest else so3.identity(),wp),parameters=d)
else:
#no drawing
pass
if name is not None:
self.drawText(name,wp)
elif isinstance(item,(GeometricPrimitive,TriangleMesh,PointCloud,Geometry3D)):
#this can be tricky if the mesh or point cloud has colors
if not hasattr(self,'appearance'):
self.appearance = Appearance()
self.appearance.setColor(0.5,0.5,0.5,1)
c = self.attributes["color"]
if c is not None:
self.appearance.setColor(*c)
s = self.attributes["size"]
if s:
self.appearance.setPointSize(s)
self.appearance.setSilhouette(0)
self.appearance.setCreaseAngle(0)
wp = None
geometry = None
lighting = True
restoreLineWidth = False
if isinstance(self.item,GeometricPrimitive):
if not hasattr(self,'geometry'):
self.geometry = Geometry3D(self.item)
geometry = self.geometry
if self.item.type in ['Point','Segment']:
lighting = False
if self.item.type == 'Segment':
GL.glLineWidth(self.attributes.get('width',3.0))
restoreLineWidth = True
elif isinstance(self.item,PointCloud):
if not hasattr(self,'geometry'):
self.geometry = Geometry3D(self.item)
lighting = False
geometry = self.geometry
elif isinstance(self.item,TriangleMesh):
if not hasattr(self,'geometry'):
self.geometry = Geometry3D(self.item)
geometry = self.geometry
else:
assert isinstance(self.item,Geometry3D)
if self.item.type() == 'GeometricPrimitive':
prim = self.item.getGeometricPrimitive()
if prim.type in ['Point','Segment']:
lighting = False
if prim.type == 'Segment':
GL.glLineWidth(self.attributes.get('width',3.0))
restoreLineWidth = True
elif self.item.type() == 'PointCloud':
lighting = False
geometry = self.item
if lighting:
GL.glEnable(GL.GL_LIGHTING)
else:
GL.glDisable(GL.GL_LIGHTING)
self.appearance.drawWorldGL(geometry)
if restoreLineWidth:
GL.glLineWidth(1.0)
if name is not None:
bmin,bmax = geometry.getBB()
wp = vectorops.mul(vectorops.add(bmin,bmax),0.5)
self.drawText(name,wp)
else:
try:
itypes = self.attributes['type']
except KeyError:
try:
itypes = objectToVisType(item,world)
except Exception as e:
import traceback
traceback.print_exc()
warnings.warn("Unsupported object type {} of type {}".format(item,item.__class__.__name__))
return
if itypes is None:
warnings.warn("Unable to convert item {} to drawable".format(str(item)))
return
elif itypes == 'Config':
rindex = self.attributes.get("robot",0)
if world and rindex < world.numRobots():
robot = world.robot(rindex)
if robot.numLinks() != len(item):
warnings.warn("Unable to draw Config %s, does not have the same # of DOFs as the robot: %d != %d"%(self.name,robot.numLinks(),len(item)))
else:
if not self.useDefaultAppearance:
oldAppearance = [robot.link(i).appearance().clone() for i in range(robot.numLinks())]
for i in range(robot.numLinks()):
if self.customAppearance is not None:
robot.link(i).appearance().set(self.customAppearance)
elif "color" in self.attributes:
robot.link(i).appearance().setColor(*self.attributes["color"])
oldconfig = robot.getConfig()
robot.setConfig(item)
robot.drawGL()
robot.setConfig(oldconfig)
if not self.useDefaultAppearance:
for (i,app) in enumerate(oldAppearance):
robot.link(i).appearance().set(app)
else:
warnings.warn("Unable to draw Config items without a world or robot")
elif itypes == 'Configs':
def drawAsTrajectory():
width = self.attributes.get("width",3.0)
color = self.attributes["color"]
pointSize = self.attributes.get("pointSize",None)
pointColor = self.attributes.get("pointColor",None)
drawTrajectory(Trajectory(list(range(len(item))),item),width,color,pointSize,pointColor)
if len(item) == 0:
pass
elif world and world.numRobots() >= 1:
maxConfigs = self.attributes.get("maxConfigs",min(10,len(item)))
robot = world.robot(self.attributes.get("robot",0))
if robot.numLinks() != len(item[0]):
if len(item[0]) in [2,3]: #interpret as a trajectory
self.displayCache[0].draw(drawAsTrajectory)
centroid = item[0] + [0]*(3-len(item[0]))
if name is not None:
self.drawText(name,centroid)
else:
warnings.warn("Configs items aren't the right size for the robot")
if not self.useDefaultAppearance:
oldAppearance = [robot.link(i).appearance().clone() for i in range(robot.numLinks())]
for i in range(robot.numLinks()):
if self.customAppearance is not None:
robot.link(i).appearance().set(self.customAppearance)
elif "color" in self.attributes:
robot.link(i).appearance().setColor(*self.attributes["color"])
oldconfig = robot.getConfig()
for i in range(maxConfigs):
idx = int(i*len(item))//maxConfigs
robot.setConfig(item[idx])
robot.drawGL()
robot.setConfig(oldconfig)
if not self.useDefaultAppearance:
for (i,app) in enumerate(oldAppearance):
robot.link(i).appearance().set(app)
elif len(item[0]) in [2,3]: #interpret as a trajectory
self.displayCache[0].draw(drawAsTrajectory)
centroid = item[0] + [0]*(3-len(item[0]))
if name is not None:
self.drawText(name,centroid)
else:
warnings.warn("Unable to draw Configs items without a world or robot")
elif itypes == 'Vector3':
def drawRaw():
GL.glDisable(GL.GL_LIGHTING)
GL.glEnable(GL.GL_POINT_SMOOTH)
GL.glPointSize(self.attributes.get("size",5.0))
GL.glColor4f(*self.attributes.get("color",[0,0,0,1]))
GL.glBegin(GL.GL_POINTS)
GL.glVertex3f(0,0,0)
GL.glEnd()
self.displayCache[0].draw(drawRaw,[so3.identity(),item])
if name is not None:
self.drawText(name,item)
elif itypes == 'RigidTransform':
def drawRaw():
fancy = self.attributes.get("fancy",False)
if fancy: GL.glEnable(GL.GL_LIGHTING)
else: GL.glDisable(GL.GL_LIGHTING)
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01),fancy=fancy)
self.displayCache[0].draw(drawRaw,transform=item)
if name is not None:
self.drawText(name,item[1])
else:
warnings.warn("Unable to draw item of type \"%s\""%(str(itypes),))
#revert appearance
if not self.useDefaultAppearance and hasattr(item,'appearance'):
item.appearance().set(self.oldAppearance)
[docs] def getBounds(self):
"""Returns a bounding box (bmin,bmax) or None if it can't be found"""
if len(self.subAppearances)!=0:
bb = bb_create()
for n,app in self.subAppearances.items():
bb = bb_union(bb,app.getBounds())
return bb
item = self.item
if isinstance(item,coordinates.Point):
return [item.worldCoordinates(),item.worldCoordinates()]
elif isinstance(item,coordinates.Direction):
T = item.frame().worldCoordinates()
d = item.localCoordinates()
L = self.attributes.get("length",0.1)
return bb_create(T[1],se3.apply(T,vectorops.mul(d,L)))
elif isinstance(item,coordinates.Frame):
T = item.worldCoordinates()
L = self.attributes.get("length",0.1)
return bb_create(T[1],se3.apply(T,(L,0,0)),se3.apply(T,(0,L,0)),se3.apply(T,(0,0,L)))
elif isinstance(item,ContactPoint):
L = self.attributes.get("length",0.05)
return bb_create(item.x,vectorops.madd(item.x,item.n,L))
elif isinstance(item,WorldModel):
pass
elif hasattr(item,'geometry'):
return item.geometry().getBB()
elif hasattr(item,'getBB'):
return item.getBB()
elif isinstance(item,(str,VisPlot)):
pass
else:
try:
vtype = objectToVisType(item,None)
if 'Vector3' == vtype:
#assumed to be a point
return (item,item)
elif 'RigidTransform' == vtype:
#assumed to be a rigid transform
T = item
L = self.attributes.get("length",0.1)
return bb_create(T[1],se3.apply(T,(L,0,0)),se3.apply(T,(0,L,0)),se3.apply(T,(0,0,L)))
except Exception:
raise
pass
warnings.warn("Empty bound for object {} type {}".format(self.name,self.item.__class__.__name__))
return bb_create()
[docs] def getCenter(self):
bb = self.getBounds()
if bb_empty(bb):
item = self.item
if hasattr(item,'getCurrentTransform'):
return item.getCurrentTransform()[1]
elif hasattr(item,'getTransform'):
return item.getTransform()[1]
return [0,0,0]
return vectorops.interpolate(bb[0],bb[1],0.5)
[docs] def rayCast(self,source,direction,tolerance=0.01):
"""Returns a point on the item intersected by the ray
``source + t*direction``, or None if it does not intersect.
For non-volumetric items, the item is treated as though it
were expanded by ``tolerance`` units.
"""
geom = None
if isinstance(self.item,Geometry3D):
geom = self.item
elif hasattr(self.item,'geometry') and callable(self.item.geometry):
geom = self.item.geometry()
#TODO: ray cast other items like points, transforms, trajectories
if geom is None:
return None
margin = geom.getCollisionMargin()
if geom.type() == 'PointCloud':
geom.setCollisionMargin(tolerance)
elif geom.type() == 'GeometricPrimitive' and geom.getGeometricPrimitive().type in ['Point','Segment','Line']:
geom.setCollisionMargin(tolerance)
(hit,pt) = geom.rayCast(source,direction)
geom.setCollisionMargin(margin) #restore margin
return pt if hit else None
[docs] def getSubItem(self,path):
if len(path) == 0: return self
for k,v in self.subAppearances.items():
if v.name == path[0]:
try:
return v.getSubItem(path[1:])
except ValueError as e:
raise ValueError("Invalid sub-path specified "+str(path)+" at "+str(e))
raise ValueError("Invalid sub-item specified "+str(path[0]))
[docs] def highlight(self,color):
"""Turn on/off a temporary highlight. color can be a (r,g,b,strength)
tuple or None.
"""
item = self.item
if hasattr(item,'appearance'):
self.customAppearance = item.appearance().clone()
if color is None:
self.customAppearance.setTintColor((1,1,1,1),0)
#self.useDefaultAppearance = True
#item.appearance().set(self.customAppearance)
else:
r,g,b,a = color
self.customAppearance.setTintColor((r,g,b,1),a)
self.useDefaultAppearance = False
self.markChanged(False,True)
elif 'color' in self.attributes:
if hasattr(self,'oldAppearance'):
basecolor = self.oldAppearance
else:
basecolor = self.attributes['color']
if basecolor is None:
basecolor = [0.5,0.5,0.5,1]
self.oldAppearance = basecolor
if color is None:
self.attributes['color'] = basecolor
else:
r,g,b,a = color
newcolor = vectorops.interpolate(basecolor,[r,g,b,1],a)
self.attributes['color'] = newcolor
else:
#TODO: tint other objects that don't have appearance or color?
pass
for (k,app) in self.subAppearances.items():
app.highlight(color)
[docs] def make_editor(self,world=None):
if self.editor is not None:
return
item = self.item
if isinstance(item,coordinates.Point):
res = PointPoser()
res.set(item.worldCoordinates())
res.setAxes(item.frame().worldCoordinates()[0])
elif isinstance(item,coordinates.Direction):
res = PointPoser()
res.set(item.worldCoordinates())
res.setAxes(item.frame().worldCoordinates()[0])
elif isinstance(item,coordinates.Frame):
res = TransformPoser()
res.set(*item.worldCoordinates())
elif isinstance(item,RobotModel):
res = RobotPoser(item)
elif isinstance(item,SubRobotModel):
res = RobotPoser(item._robot)
res.setActiveDofs(item._links);
elif isinstance(item,RigidObjectModel):
res = ObjectPoser(item)
elif isinstance(item,(list,tuple)):
#determine if it's a rotation, transform, or point
itype = objectToVisType(item,None)
if itype == 'Vector3':
res = PointPoser()
res.set(item)
elif itype == 'Matrix3':
res = TransformPoser()
res.enableRotation(True)
res.enableTranslation(False)
res.set(item)
elif itype == 'RigidTransform':
res = TransformPoser()
res.enableRotation(True)
res.enableTranslation(True)
res.set(*item)
elif itype == 'Config':
if world is not None and world.numRobots() > 0 and world.robot(0).numLinks() == len(item):
#it's a valid configuration
oldconfig = world.robot(0).getConfig()
world.robot(0).setConfig(item)
res = RobotPoser(world.robot(0))
world.robot(0).setConfig(oldconfig)
else:
warnings.warn("VisAppearance.make_editor(): Editor for object of type {} cannot be associated with a robot".format(itype))
return
else:
warnings.warn("VisAppearance.make_editor(): Editor for object of type {} not defined".format(itype))
return
else:
warnings.warn("VisAppearance.make_editor(): Editor for object of type {} not defined".format(item.__class__.__name__))
return
self.editor = res
[docs] def update_editor(self,item_to_editor=False):
for (name,item) in self.subAppearances.items():
item.update_editor(item_to_editor)
if self.editor is None:
return
item = self.item
if item_to_editor:
if isinstance(item,coordinates.Point):
self.editor.set(self.item.worldCoordinates())
elif isinstance(item,coordinates.Direction):
self.editor.set(self.item.worldCoordinates())
elif isinstance(item,coordinates.Frame):
self.editor.set(*self.item.worldCoordinates())
elif isinstance(self.item,RobotModel):
self.editor.set(self.item.getConfig())
elif isinstance(self.item,SubRobotModel):
self.editor.set(self.item.tofull(self.item.getConfig()))
elif isinstance(self.item,RigidObjectModel):
self.editor.set(*self.item.getTransform())
elif isinstance(self.item,(list,tuple)):
itype = objectToVisType(self.item,None)
if itype in ('Vector3','Matrix3'):
self.editor.set(self.item)
elif itype == 'RigidTransform':
self.editor.set(*self.item)
elif itype == 'Config':
self.editor.set(self.item)
else:
raise RuntimeError("Uh... unsupported type with an editor?")
else:
if not self.editor.hasFocus():
return
if isinstance(item,coordinates.Point):
self.item._localCoordinates = se3.apply(se3.inv(self.item._frame.worldCoordinates()),self.editor.get())
elif isinstance(item,coordinates.Direction):
self.item._localCoordinates = se3.apply(se3.inv(self.item._frame.worldCoordinates()),self.editor.get())
elif isinstance(item,coordinates.Frame):
self.item._worldCoordinates = self.editor.get()
self.item._relativeCoordinates = se3.mul(se3.inv(self.item.parent().worldCoordinates()),self.editor.get())
#TODO: updating downstream frames?
elif isinstance(self.item,RobotModel):
self.item.setConfig(self.editor.getConditioned(self.item.getConfig()))
elif isinstance(self.item,SubRobotModel):
self.item.setConfig(self.item.fromfull(self.editor.get()))
elif isinstance(self.item,RigidObjectModel):
self.item.setTransform(*self.editor.get())
elif isinstance(self.item,(tuple,list)):
def setList(a,b):
if isinstance(a,(list,tuple)) and isinstance(b,(list,tuple)):
if len(a) == len(b):
for i in range(len(a)):
if not setList(a[i],b[i]):
if isinstance(a,list):
a[i] = b[i]
else:
return False
return True
return False
v = self.editor.get()
if not setList(self.item,v):
self.item = v
elif isinstance(self.item,tuple):
warnings.warn("Edited a tuple... maybe a point or an xform? can't actually edit")
self.item = self.editor.get()
else:
raise RuntimeError("Uh... unsupported type with an editor?")
[docs] def remove_editor(self):
self.editor = None
[docs]class VisualizationScene:
"""Holds all of the visualization information for a scene, including
labels, edit status, and animations.
Attributes:
items (dict of str->VisAppearance): the top level items in the scene.
labels (list): internally used
t (float): current scene time, internally used
timeCallback (callable): a function time()->float that is used to get
the time update. If None, ``time.time()`` is used.
startTime (float): first time that updateTime() was called, internally
used.
doRefresh (bool): whether any item wants to refresh the visualization.
cameraController: if given, a class to update the camera whenever
updateCamera is called.
"""
def __init__(self):
self.items = {}
self.labels = []
self.t = 0
self.timeCallback = None
self.startTime = None
self.animating = True
self.currentAnimationTime = 0
self.doRefresh = False
self.cameraController = None
[docs] def getItem(self,item_name):
"""Returns an VisAppearance according to the given name or path"""
if isinstance(item_name,(list,tuple)):
components = item_name
if len(components)==1:
return self.getItem(components[0])
if components[0] not in self.items:
raise ValueError("Invalid top-level item specified: "+str(item_name))
return self.items[components[0]].getSubItem(components[1:])
if item_name in self.items:
return self.items[item_name]
[docs] def dirty(self,item_name='all'):
"""Marks an item or everything as dirty, forcing a deep redraw."""
global _globalLock
with _globalLock:
if item_name == 'all':
for (name,itemvis) in self.items.items():
itemvis.markChanged()
if hasattr(itemvis,'appearance'):
itemvis.appearance.refresh(True)
else:
item = self.getItem(item_name)
item.markChanged()
if hasattr(item,'appearance'):
item.appearance.refresh(True)
[docs] def clear(self):
"""Clears the visualization world"""
global _globalLock
with _globalLock:
for (name,itemvis) in self.items.items():
itemvis.destroy()
self.items = {}
self.currentAnimationTime = 0
self.doRefresh = True
[docs] def clearText(self):
"""Clears all text in the visualization."""
global _globalLock
with _globalLock:
del_items = []
for (name,itemvis) in self.items.items():
if isinstance(itemvis.item,str):
itemvis.destroy()
del_items.append(name)
for n in del_items:
del self.items[n]
[docs] def listItems(self,root=None,indent=0):
"""Prints out all items in the visualization world."""
if root is None:
for name,value in self.items.items():
self.listItems(value,indent)
else:
if isinstance(root,str):
root = self.getItem(root)
if indent > 0:
print(" "*(indent-1), end=' ')
print(root.name)
for n,v in root.subAppearances.items():
self.listItems(v,indent+2)
[docs] def getItemName(self,object):
"""Retrieves a str or tuple of strs giving the path to an object"""
name = None
if hasattr(object,'getName'):
name = object.getName()
elif hasattr(object,'name'):
if callable(object.name):
name = object.name()
else:
name = object.name
if name is not None and name in self.items:
if self.items[name].item is object:
return name
if isinstance(object,RobotModelLink): #a link?
robot = object.robot()
robpath = self.getItemName(robot)
if robpath:
return robpath + (name,)
else:
warnings.warn("Couldnt find link {} under robot".format(name))
def objectPath(app,obj,name):
if app.item == object:
return ()
if name is not None and app.name == name:
return ()
for (subitem,subapp) in app.subAppearances.items():
subpath = objectPath(subapp,obj,name)
if subpath is not None:
return (subapp.name,)+subpath
return None
if hasattr(object,'world'):
#look through the world for the object
world = self.items.get('world',None)
if world is not None:
p = objectPath(self.items['world'],object,name)
if p is not None:
return ('world',)+p
else:
warnings.warn("Couldnt find object {} under world".format(name))
for (k,v) in self.items.items():
p = objectPath(v,object,name)
if p is not None:
if len(p)==0: #top level item
return k
return (k,)+p
else:
warnings.warn("Couldnt find object {} under {}".format(name,k))
return None
[docs] def add(self,name,item,keepAppearance=False,**kwargs):
"""Adds a named item to the visualization world. If the item already
exists, the appearance information will be reinitialized if keepAppearance=False
(default) or be kept if keepAppearance=True."""
global _globalLock
assert not isinstance(name,(list,tuple)),"Cannot add sub-path items"
with _globalLock:
if keepAppearance and name in self.items:
self.items[name].setItem(item)
else:
#need to erase prior item visualizer
if name in self.items:
self.items[name].destroy()
type = kwargs.get('type',None)
if type is None:
world = self.items.get('world',None)
if world is not None: world = world.item
try:
type = objectToVisType(item,world)
except Exception:
type = None
app = VisAppearance(item,name,type)
self.items[name] = app
item = self.items[name]
for (attr,value) in kwargs.items():
self._setAttribute(item,attr,value)
#self.refresh()
[docs] def addText(self,name,text,**kwargs):
self.add(name,text,True,**kwargs)
[docs] def animate(self,name,animation,speed=1.0,endBehavior='loop'):
global _globalLock
with _globalLock:
if hasattr(animation,'__iter__'):
#a list of milestones -- loop through them with 1s delay
print("visualization.animate(): Making a Trajectory with unit durations between",len(animation),"milestones")
animation = Trajectory(list(range(len(animation))),animation)
if isinstance(animation,MultiPath):
world = self.items.get('world',None)
if world is not None:
world=world.item
if world.numRobots() > 0:
#discretize multipath
robot = world.robot(0)
animation = animation.getTrajectory(robot,0.1)
else:
animation = animation.getTrajectory()
else:
animation = animation.getTrajectory()
assert isinstance(animation,Trajectory) or animation is None,"Must animate() with a Trajectory object or list of milestones"
item = self.getItem(name)
item.animation = animation
item.animationStartTime = self.currentAnimationTime
item.animationSpeed = speed
item.animationEndBehavior = endBehavior
item.markChanged(config=True,appearance=False)
[docs] def pauseAnimation(self,paused=True):
global _globalLock
with _globalLock:
self.animating = not paused
[docs] def stepAnimation(self,amount):
global _globalLock
with _globalLock:
self.animationTime(self.currentAnimationTime + amount)
[docs] def animationTime(self,newtime=None):
global _globalLock
if newtime is None:
#query mode
return self.currentAnimationTime
#update mode
with _globalLock:
self.currentAnimationTime = newtime
self.doRefresh = True
for (k,v) in self.items.items():
#do animation updates
v.updateAnimation(self.currentAnimationTime)
return
[docs] def remove(self,name):
global _globalLock
with _globalLock:
assert name in self.items,"Can only remove top level objects from visualization, try hide() instead"
item = self.getItem(name)
item.destroy()
del self.items[name]
self.doRefresh = True
[docs] def getItemConfig(self,name):
global _globalLock
with _globalLock:
res = config.getConfig(self.getItem(name).item)
return res
[docs] def setItemConfig(self,name,value):
global _globalLock
with _globalLock:
item = self.getItem(name)
if isinstance(item.item,(list,tuple)):
#TODO: broadcast value to the shape of item
item.item = value
elif isinstance(item.item,str):
item.item = value
else:
config.setConfig(item.item,value)
if item.editor:
item.update_editor(item_to_editor = True)
item.markChanged(config=True,appearance=False)
self.doRefresh = True
[docs] def addLabel(self,text,point,color):
global _globalLock
with _globalLock:
self.labels.append((text,point,color))
[docs] def hideLabel(self,name,hidden=True):
global _globalLock
with _globalLock:
item = self.getItem(name)
item.attributes["hide_label"] = hidden
item.markChanged(config=False,appearance=True)
self.doRefresh = True
[docs] def hide(self,name,hidden=True):
global _globalLock
with _globalLock:
self.getItem(name).attributes['hidden'] = hidden
self.doRefresh = True
[docs] def addPlotItem(self,plotname,itemname):
global _globalLock
with _globalLock:
plot = self.getItem(plotname)
assert plot is not None and isinstance(plot.item,VisPlot),(plotname+" is not a valid plot")
plot = plot.item
for i in plot.items:
assert i.name != itemname,(str(itemname)+" is already in the plot "+plotname)
item = self.getItem(itemname)
assert item is not None,(str(itemname)+" is not a valid item")
plot.items.append(VisPlotItem(itemname,item))
[docs] def logPlot(self,plotname,itemname,value):
global _globalLock
with _globalLock:
customIndex = -1
plot = self.getItem(plotname)
assert plot is not None and isinstance(plot.item,VisPlot),(plotname+" is not a valid plot")
compress = plot.attributes['compress']
plot = plot.item
for i,item in enumerate(plot.items):
if len(item.name)==0:
customIndex = i
if customIndex < 0:
customIndex = len(plot.items)
plot.items.append(VisPlotItem('',None))
t = self.t
if self.startTime is not None:
if self.timeCallback is None:
t = time.time() - self.startTime
else:
t = self.timeCallback() - self.startTime
else:
t = 0
plot.items[customIndex].compressThreshold = compress
plot.items[customIndex].customUpdate(itemname,t,value)
[docs] def logPlotEvent(self,plotname,eventname,color):
global _globalLock
with _globalLock:
plot = self.getItem(plotname)
assert plot is not None and isinstance(plot.item,VisPlot),(plotname+" is not a valid plot")
t = self.t
if self.startTime is not None:
if self.timeCallback is None:
t = time.time() - self.startTime
else:
t = self.timeCallback() - self.startTime
else:
t = 0
plot.item.addEvent(eventname,t,color)
[docs] def hidePlotItem(self,plotname,itemname,hidden=True):
global _globalLock
with _globalLock:
plot = self.getItem(plotname)
assert plot is not None and isinstance(plot.item,VisPlot),plotname+" is not a valid plot"
plot = plot.item
identified = False
if isinstance(itemname,(tuple,list)):
for i in plot.items:
if i.name == itemname[0]:
assert itemname[1] < len(i.hidden),("Invalid component index of item "+str(itemname[0]))
identified = True
i.hidden[itemname] = hidden
else:
for i in plot.items:
if i.name == itemname:
for j in range(len(i.hidden)):
i.hidden[j] = hidden
assert identified,("Invalid item "+str(itemname)+" specified in plot "+plotname)
self.doRefresh = True
[docs] def savePlot(self,plotname,fn):
global _globalLock
with _globalLock:
plot = self.getItem(plotname)
assert plot is not None and isinstance(plot.item,VisPlot),plotname+" is not a valid plot"
plot = plot.item
if fn is not None:
plot.beginSave(fn)
else:
plot.endSave(fn)
[docs] def setAppearance(self,name,appearance):
global _globalLock
with _globalLock:
item = self.getItem(name)
item.useDefaultAppearance = False
item.customAppearance = appearance
item.markChanged(config=False,appearance=True)
self.doRefresh = True
def _setAttribute(self,item,attr,value):
"""Internal use only"""
item.attributes[attr] = value
if value==None:
del item.attributes[attr]
if attr=='color':
item.useDefaultAppearance = False
if len(item.subAppearances) > 0 and attr not in ['label','hidden']:
#some attributes don't get inherited
for n,app in item.subAppearances.items():
self._setAttribute(app,attr,value)
if attr=='type':
#modify the parent attributes
item.attributes.setParent(_default_attributes(item.item,type=value))
item.markChanged(config=False,appearance=True)
[docs] def setAttribute(self,name,attr,value):
global _globalLock
with _globalLock:
item = self.getItem(name)
if item is None:
raise ValueError("Item "+str(name)+" doesn't exist in scene")
self._setAttribute(item,attr,value)
self.doRefresh = True
[docs] def getAttribute(self,name,attr):
global _globalLock
with _globalLock:
item = self.getItem(name)
res = item.attributes[attr]
return res
[docs] def getAttributes(self,name):
global _globalLock
with _globalLock:
item = self.getItem(name)
res = item.getAttributes()
return res
[docs] def revertAppearance(self,name):
global _globalLock
with _globalLock:
item = self.getItem(name)
item.useDefaultAppearance = True
if hasattr(self,'oldAppearance'):
item.appearance().set(self.oldAppearance)
item.markChanged(config=False,appearance=True)
self.doRefresh = True
[docs] def setColor(self,name,r,g,b,a=1.0):
global _globalLock
with _globalLock:
item = self.getItem(name)
self._setAttribute(item,"color",[r,g,b,a])
item.markChanged(config=False,appearance=True)
self.doRefresh = True
[docs] def setDrawFunc(self,name,func):
global _globalLock
with _globalLock:
item = self.getItem(name)
item.customDrawFunc = func
self.doRefresh = True
[docs] def autoFitCamera(self,zoom=True,rotate=True,scale=1.0):
vp = self.getViewport()
try:
autoFitViewport(vp,list(self.items.values()),zoom=zoom,rotate=rotate)
vp.camera.dist /= scale
self.setViewport(vp)
except Exception as e:
warnings.warn("Unable to auto-fit camera")
import traceback
traceback.print_exc()
[docs] def followCamera(self,target,translate,rotate,center):
if target is None:
self.cameraController = None
return
vp = self.getViewport()
if isinstance(target,str) or isinstance(target,(tuple,list)):
try:
target = self.getItem(target)
except KeyError:
raise ValueError("Invalid item "+str(target))
target_center = target.getCenter()
if center:
if translate:
_camera_translate(vp,target_center)
else:
_camera_lookat(vp,target_center)
if translate and rotate:
self.cameraController = _TrackingCameraController(vp,target)
elif translate:
self.cameraController = _TranslatingCameraController(vp,target)
elif rotate:
self.cameraController = _TargetCameraController(vp,target)
else:
self.cameraController = None
elif isinstance(target,Trajectory):
self.cameraController = _TrajectoryCameraController(vp,target)
elif isinstance(target,SimRobotSensor):
self.cameraController = _SimCamCameraController(vp,target)
else:
raise ValueError("Invalid value for target, must either be str or a Trajectory")
[docs] def setTimeCallback(self,cb):
"""Sets a callback in updateTime() to set the current time"""
self.timeCallback = cb
[docs] def updateTime(self,t=None):
"""The backend will call this during an idle loop to update the
visualization time. This may also update animations if currently
animating."""
if t is None:
if self.timeCallback is None:
t = time.time()
else:
t = self.timeCallback()
if self.startTime is None:
self.startTime = t
oldt = self.t
self.t = t-self.startTime
if self.t - oldt < 0:
warnings.warn("Time is going negative?")
if self.animating:
if self.t - oldt > 0:
self.stepAnimation(self.t - oldt)
for (k,v) in self.items.items():
#do other updates
v.updateTime(self.t)
[docs] def updateCamera(self):
"""Updates the camera, if controlled. The backend should call
this whenever the scene is to be drawn."""
if self.cameraController is not None:
vp = self.cameraController.update(self.currentAnimationTime)
if vp is not None:
self.setViewport(vp)
[docs] def edit(self,name,doedit=True):
raise NotImplementedError("Editing not implemented by {}".format(self.__class__.__name__))
[docs] def pick(self,click_callback,hover_callback,highlight_color,filter,tolerance):
raise NotImplementedError("Picking not implemented by {}".format(self.__class__.__name__))
[docs] def getViewport(self):
raise NotImplementedError("Viewport ops not implemented by {}".format(self.__class__.__name__))
[docs] def setViewport(self,viewport):
raise NotImplementedError("Viewport ops not implemented by {}".format(self.__class__.__name__))
[docs] def setBackgroundColor(self,r,g,b,a=1):
raise NotImplementedError("Background color changing not implemented by {}".format(self.__class__.__name__))
[docs] def renderGL(self,view):
"""Renders the scene in OpenGL"""
vp = view.to_viewport()
self.labels = []
world = self.items.get('world',None)
if world is not None: world=world.item
#draw solid items first
delayed = []
for (k,v) in self.items.items():
transparent = v.transparent()
if transparent is not False:
delayed.append(k)
if transparent is True:
continue
v.widget = self
v.swapDrawConfig()
v.drawGL(world,viewport=vp,draw_transparent=False)
v.swapDrawConfig()
#allows garbage collector to delete these objects
v.widget = None
for k in delayed:
v = self.items[k]
v.widget = self
v.swapDrawConfig()
v.drawGL(world,viewport=vp,draw_transparent=True)
v.swapDrawConfig()
#allows garbage collector to delete these objects
v.widget = None
#cluster label points and draw labels
pointTolerance = view.camera.dist*0.03
if pointTolerance > 0:
pointHash = {}
for (text,point,color) in self.labels:
index = tuple([int(x/pointTolerance) for x in point])
try:
pointHash[index][1].append((text,color))
except KeyError:
pointHash[index] = [point,[(text,color)]]
for (p,items) in pointHash.values():
self._renderGLLabelRaw(view,p,*list(zip(*items)))
[docs] def renderScreenGL(self,view,window):
cx = 20
cy = 20
GL.glDisable(GL.GL_LIGHTING)
GL.glDisable(GL.GL_DEPTH_TEST)
for (k,v) in self.items.items():
if isinstance(v.item,VisPlot) and not v.attributes['hidden']:
pos = v.attributes['position']
duration = v.attributes['duration']
vrange = v.attributes['range']
w,h = v.attributes['size']
if pos is None:
v.item.renderGL(window,cx,cy,w,h,duration,vrange[0],vrange[1])
cy += h+18
else:
x = pos[0]
y = pos[1]
if x < 0:
x = view.w + x
if y < 0:
y = view.h + y
v.item.renderGL(window,x,y,w,h,duration,vrange[0],vrange[1])
for (k,v) in self.items.items():
if isinstance(v.item,str) and not v.attributes['hidden']:
pos = v.attributes['position']
col = v.attributes['color']
size = v.attributes['size']
if pos is None:
#draw at console
window.draw_text((cx,cy+size),v.item,size,col)
cy += (size*15)/10
elif len(pos)==2:
x = pos[0]
y = pos[1]
if x < 0:
x = view.w + x
if y < 0:
y = view.h + y
window.draw_text((x,y+size),v.item,size,col)
GL.glEnable(GL.GL_DEPTH_TEST)
def _renderGLLabelRaw(self,view,point,textList,colorList):
#assert not self.makingDisplayList,"drawText must be called outside of display list"
assert self.window is not None
invCameraRot = so3.inv(view.camera.matrix()[0])
for i,(text,c) in enumerate(zip(textList,colorList)):
if i+1 < len(textList): text = text+","
projpt = view.project(point,clip=False)
if projpt[2] > view.clippingplanes[0]:
d = float(12)/float(view.w)*projpt[2]*0.7
point = vectorops.add(point,so3.apply(invCameraRot,(0,-d,0)))
GL.glDisable(GL.GL_LIGHTING)
GL.glDisable(GL.GL_DEPTH_TEST)
GL.glColor3f(*c)
self.draw_text(point,text,size=12)
GL.glEnable(GL.GL_DEPTH_TEST)
[docs] def clearDisplayLists(self):
for i in self.items.values():
i.clearDisplayLists()
[docs] def rayCast(self,x,y,filter=None,tolerance=0.01):
"""Performs ray casting with the scene
Args:
x (float): the screen x-coordinate of the mouse cursor
y (float): the screen y-coordinate of the mouse cursor
filter (callable, optional): if given, a function f(name,item) that
returns True if the item should be considered.
tolerance (float, optional): a tolerance for ray intersection with
non-volumetric items.
Returns:
tuple: a pair ``(visapp,pt)`` with visapp the first intersected
:class:`VisAppearance` and pt the clicked point. If nothing
intersects, then (None,None) is returned.
"""
(s,d) = self.view.click_ray(x,y)
data = [(None,None),float('inf')] #closest and closest distance
def doclick(name,visappearance,data=data):
item = visappearance.item
if filter is None or filter(name,item):
if len(visappearance.subAppearances) != 0:
for sname,app in visappearance.subAppearances.items():
doclick(sname,app)
else:
pt = visappearance.rayCast(s,d,tolerance)
if pt is not None:
dist = vectorops.dot(vectorops.sub(pt,s),d)
if dist < data[1]:
data[0] = (visappearance,pt)
data[1] = dist
for name,item in self.items.items():
doclick(name,item)
return data[0]
[docs] def saveJsonConfig(self,fn=None):
def dumpitem(v):
if len(v.subAppearances) > 0:
items = []
for (k,app) in v.subAppearances.items():
jsapp = dumpitem(app)
if len(jsapp) > 0:
items.append({"name":k,"appearance":jsapp})
return items
else:
return v.attributes.flatten()
out = {}
for (k,v) in self.items.items():
out[k] = dumpitem(v)
if fn is None:
return out
else:
import json
f = open(fn,'w')
json.dump(out,f)
f.close()
return out
[docs] def loadJsonConfig(self,jsonobj_or_file):
if isinstance(jsonobj_or_file,str):
import json
f = open(jsonobj_or_file,'r')
jsonobj = json.load(f)
f.close()
else:
jsonobj = jsonobj_or_file
def parseitem(js,app):
if isinstance(js,dict):
for (attr,value) in js.items():
app.attributes[attr] = value
app.markChanged()
elif isinstance(js,list):
for val in js:
if not isinstance(val,dict) or "name" not in val or "appearance" not in val:
warnings.warn("JSON object {} does not contain a valid subappearance".format(js))
name = val["name"]
jsapp = val["appearance"]
if isinstance(name,list):
name = tuple(name)
if name not in app.subAppearances:
warnings.warn("JSON object {} subappearance {} not in visualization".format(js,name))
else:
parseitem(jsapp,app.subAppearances[name])
else:
warnings.warn("JSON object {} does not contain a dict of attributes or list of sub-appearances".format(js))
parsed = set()
for (k,v) in self.items.items():
if k in jsonobj:
parsed.add(k)
parseitem(jsonobj[k],v)
else:
warnings.warn("Visualization object {} not in JSON object".format(k))
for (k,v) in jsonobj.items():
if k not in parsed:
warnings.warn("JSON object {} not in visualization".format(k))
def _camera_translate(vp,tgt):
vp.camera.tgt = tgt
def _camera_lookat(vp,tgt):
T = vp.get_transform()
vp.camera.tgt = tgt
vp.camera.dist = max(vectorops.distance(T[1],tgt),0.1)
#set R to point at target
zdir = vectorops.unit(vectorops.sub(tgt,T[1]))
xdir = vectorops.unit(vectorops.cross(zdir,[0,0,1]))
ydir = vectorops.unit(vectorops.cross(zdir,xdir))
R = xdir + ydir + zdir
vp.camera.set_orientation(R,'xyz')
class _TrackingCameraController:
def __init__(self,vp,target):
self.vp = vp
T = vp.get_transform()
target.swapDrawConfig()
self.viewportToTarget = se3.mul(se3.inv(target.getTransform()),T)
target.swapDrawConfig()
self.target = target
def update(self,t):
self.target.swapDrawConfig()
T = se3.mul(self.target.getTransform(),self.viewportToTarget)
self.target.swapDrawConfig()
self.vp.set_transform(T)
return self.vp
class _TranslatingCameraController:
def __init__(self,vp,target):
self.vp = vp
target.swapDrawConfig()
self.last_target_pos = target.getCenter()
target.swapDrawConfig()
self.target = target
def update(self,t):
self.target.swapDrawConfig()
t = self.target.getCenter()
self.target.swapDrawConfig()
self.vp.camera.tgt = vectorops.add(self.vp.camera.tgt,vectorops.sub(t,self.last_target_pos))
self.last_target_pos = t
return self.vp
class _TargetCameraController:
def __init__(self,vp,target):
self.vp = vp
target.swapDrawConfig()
self.last_target_pos = target.getCenter()
target.swapDrawConfig()
self.target = target
def update(self,t):
self.target.swapDrawConfig()
t = self.target.getCenter()
self.target.swapDrawConfig()
tgt = vectorops.add(self.vp.camera.tgt,vectorops.sub(t,self.last_target_pos))
self.last_target_pos = t
_camera_lookat(self.vp,tgt)
return self.vp
class _TrajectoryCameraController:
def __init__(self,vp,trajectory):
self.vp = vp
if isinstance(trajectory,SE3Trajectory):
pass
elif isinstance(trajectory,SO3Trajectory):
pass
else:
assert isinstance(trajectory,Trajectory)
pass
self.trajectory = trajectory
def update(self,t):
if isinstance(self.trajectory,(SE3Trajectory,SE3HermiteTrajectory)):
T = self.trajectory.eval(t,'loop')
self.vp.set_transform(T)
elif isinstance(self.trajectory,(SO3Trajectory,SO3HermiteTrajectory)):
R = self.trajectory.eval(t,'loop')
self.vp.camera.set_orientation(R,'xyz')
else:
trans = self.trajectory.eval(t,'loop')
T = self.vp.get_transform()
ofs = vectorops(self.vp.tgt,T[0])
self.vp.camera.tgt = vectorops.add(trans,ofs)
return self.vp
class _SimCamCameraController:
def __init__(self,vp,target):
self.vp = vp
self.target = target
def update(self,t):
from ..model import sensing
T = sensing.get_sensor_xform(self.target,self.target.robot())
self.vp.set_transform(T)
return self.vp
class _WindowManager:
def reset(self):
raise NotImplementedError()
def frontend(self):
raise NotImplementedError()
def createWindow(self,title):
raise NotImplementedError()
def setWindow(self,id):
raise NotImplementedError()
def getWindow(self):
raise NotImplementedError()
def scene(self):
raise NotImplementedError()
def getWindowName(self):
raise NotImplementedError()
def setWindowName(self,name):
raise NotImplementedError()
def resizeWindow(self,w,h):
raise NotImplementedError()
def setPlugin(self,plugin):
raise NotImplementedError()
def pushPlugin(self,plugin):
raise NotImplementedError()
def popPlugin(self):
raise NotImplementedError()
def splitView(self,plugin):
raise NotImplementedError()
def multithreaded(self):
return False
def run(self):
raise NotImplementedError()
def loop(self,setup,callback,cleanup):
raise NotImplementedError()
def spin(self,duration):
raise NotImplementedError()
def show(self):
raise NotImplementedError()
def shown(self):
raise NotImplementedError()
def hide(self):
raise NotImplementedError()
def dialog(self):
raise NotImplementedError()
def lock(self):
global _globalLock
_globalLock.acquire()
def unlock(self):
global _globalLock
_globalLock.release()
def update(self):
pass
def cleanup(self):
pass
def kill(self):
pass
def threadCall(self,func):
func()
def screenshot(self,*args):
raise NotImplementedError()
def screenshotCallback(self,fn,*args):
raise NotImplementedError()
class _ThreadedWindowManager(_WindowManager):
def __init__(self):
#signals to visualization thread
self.quit = False
self.in_vis_loop = False
self.vis_thread_running = False
self.vis_thread = None
self.in_app_thread = False
self.threadcalls = []
def reset(self):
self.kill()
self.in_app_thread = False
self.threadcalls = []
def run_app_thread(self,callback):
raise NotImplementedError()
def multithreaded(self):
return (True if sys.platform != 'darwin' else False)
def kill(self):
self.quit = True
if self.in_vis_loop:
#if the thread is running, _quit=True just signals to the vis loop to quit.
#otherwise, the program needs to be killed and cleaned up
if not self.vis_thread_running:
#need to clean up Qt resources
self.cleanup()
return
if self.vis_thread_running:
if self.vis_thread is not None:
self.vis_thread.join()
self.vis_thread = None
else:
#should we be tolerant to weird things that happen with kill?
self.vis_thread_running = False
assert self.vis_thread_running == False
self.quit = False
def loop(self,setup,callback,cleanup):
if self.vis_thread_running or self.in_vis_loop:
if setup is not None:
setup()
self.show()
dt = 1.0/30.0
while self.shown():
t0 = time.time()
if callback is not None:
self.lock()
callback()
self.unlock()
t1 = time.time()
time.sleep(max(0,dt-(t1-t0)))
if cleanup is not None:
cleanup()
return
raise RuntimeError("Cannot call loop() after show(), inside dialog(), or inside loop() callbacks")
self.in_vis_loop = True
try:
if setup is not None:
setup()
self.quit = False
self.run_app_thread(callback)
if cleanup is not None:
cleanup()
finally:
self.in_vis_loop = False
def spin(self,duration):
if self.in_vis_loop:
raise RuntimeError("spin() cannot be used inside loop()")
if self.multithreaded():
#use existing thread
self.show()
t = 0
while t < duration:
if not self.shown(): break
time.sleep(min(0.04,duration-t))
t += 0.04
self.hide()
else:
#use single thread
t0 = time.time()
def timed_break():
t1 = time.time()
if t1 - t0 >= duration:
self.hide()
self.loop(callback=timed_break,setup=lambda:self.show(),cleanup=None)
return
def run(self):
if self.vis_thread_running:
#already multithreaded, can't go back to single thread
self.show()
while self.shown():
time.sleep(0.1)
else:
#run in a single thread
self.loop(setup=None,callback=None,cleanup=None)
def _start_app_thread(self):
signal.signal(signal.SIGINT, signal.SIG_DFL)
self.vis_thread = threading.Thread(target=self.run_app_thread)
self.vis_thread.setDaemon(True)
self.vis_thread.start()
time.sleep(0.1)
def threadCall(self,func):
self.threadcalls.append(func)