Source code for rational_linkages.PlotterPyqtgraph
import sys
import numpy
from warnings import warn
from .DualQuaternion import DualQuaternion
from .Linkage import LineSegment
from .MiniBall import MiniBall
from .MotionFactorization import MotionFactorization
from .NormalizedLine import NormalizedLine
from .NormalizedPlane import NormalizedPlane
from .PointHomogeneous import PointHomogeneous, PointOrbit
from .RationalBezier import RationalBezier, RationalSoo
from .RationalCurve import RationalCurve
from .RationalMechanism import RationalMechanism
from .TransfMatrix import TransfMatrix
def _flat_xyz(vec) -> numpy.ndarray:
"""Convert a 3D vector-like input to a flat shape (3,) float array."""
arr = numpy.asarray(vec, dtype=float)
arr = numpy.squeeze(arr)
if arr.size != 3:
raise ValueError(f"Expected 3D vector, got shape {numpy.asarray(vec).shape}")
return arr.reshape(3,)
# Try importing GUI components
try:
import pyqtgraph.opengl as gl
from PyQt6 import QtCore, QtGui, QtWidgets
from PyQt6.QtWidgets import QApplication
except (ImportError, OSError):
warn("Failed to import OpenGL or PyQt6. If you expect interactive GUI to work, "
"please check the package installation.")
gl = None
QtCore = None
QtGui = None
QtWidgets = None
QApplication = None
[docs]
class PlotterPyqtgraph:
"""
PyQtGraph plotter for 3D visualization of geometric objects.
"""
def __init__(self,
base=None,
steps: int = 1000,
interval: tuple = (-1, 1),
arrows_length: float = 1.0,
white_background: bool = False,
show_grid: bool = True,
grid_size: float = 10.,
parent_app=None
):
"""
Initialize the Pyqtgraph plotter.
This version creates a GLViewWidget, sets a turntable-like camera, adds a grid and coordinate axes.
Parameters
----------
base : TransfMatrix or DualQuaternion, optional
Base transformation for plotting. Must be a TransfMatrix or DualQuaternion instance.
steps : int, optional
Number of steps for plotting.
interval : tuple, optional
Interval for plotting.
arrows_length : float, optional
Length of quiver arrows for poses and frames.
white_background : bool, optional
If True, use a white background for the plot.
show_grid : bool, optional
If True, show a grid in the plot.
grid_size : float, optional
Size of the grid.
parent_app : QApplication, optional
Parent Qt application instance.
"""
# Create a Qt application if one is not already running.
if parent_app is not None:
self.app = parent_app
else:
self.app = QApplication.instance()
if self.app is None:
self.app = QApplication(sys.argv)
# Expose pyqtgraph.opengl for custom scripts that want to construct
# GL items via the plotter instance.
self.gl = gl
if base is not None:
if isinstance(base, TransfMatrix):
if not base.is_rotation():
raise ValueError("Given matrix is not proper rotation.")
self.base = base
self.base_arr = self.base.array()
elif isinstance(base, DualQuaternion):
self.base = TransfMatrix(base.dq2matrix())
self.base_arr = self.base.array()
else:
raise TypeError("Base must be a TransfMatrix or DualQuaternion instance.")
self.white_background = white_background
# Create the GLViewWidget.
self.widget = CustomGLViewWidget(white_background=self.white_background)
self.widget.setWindowTitle('Rational Linkages')
self.widget.opts['distance'] = 10
self.widget.setCameraPosition(
distance=10,
rotation=QtGui.QQuaternion.fromEulerAngles(-70, 0, -30)
)
if self.white_background:
self.widget.setBackgroundColor(255, 255, 255, 255)
self.render_mode = 'translucent'
else:
self.render_mode = 'additive'
self.widget.show()
self.app.processEvents()
# add a grid
if show_grid:
grid = gl.GLGridItem()
if not numpy.isfinite(grid_size) or grid_size <= 0:
warn("Non‑positive or non‑finite grid_size; using default grid size = 10")
grid_size = 10
grid_spacing = 1.
else:
exponent = int(numpy.floor(numpy.log10(grid_size))) - 1
grid_spacing = float(numpy.power(10.0, exponent))
grid.setSize(x=grid_size, y=grid_size)
grid.setSpacing(x=grid_spacing, y=grid_spacing)
if self.white_background:
grid.setColor(QtGui.QColor(QtCore.Qt.GlobalColor.lightGray))
else:
grid.setColor(QtGui.QColor(128,128,128,80))
self.widget.addItem(grid)
# store parameters
self.t_space = numpy.linspace(interval[0], interval[1], steps)
self.steps = steps
self.arrows_length = arrows_length
# add origin coordinates
self.plot(TransfMatrix())
self.labels = []
@staticmethod
def _get_color(color, default):
"""
Convert common color names to RGBA tuples.
If color is already a tuple (or list), it is returned unchanged.
Parameters
----------
color : str or tuple or list
Color name or RGBA tuple/list.
default : tuple
Default RGBA tuple to use if color name is not recognized.
Returns
-------
tuple
RGBA tuple.
"""
if isinstance(color, str):
color_map = {
'white': (1, 1, 1, 1),
'red': (1, 0, 0, 1),
'green': (0, 1, 0, 1),
'blue': (0, 0, 1, 1),
'yellow': (1, 1, 0, 1),
'magenta': (1, 0, 1, 1),
'cyan': (0, 1, 1, 1),
'orange': (1, 0.5, 0, 1),
'lime': (0, 1, 0, 1)
}
return color_map.get(color.lower(), default)
return color
[docs]
def plot(self, objects_to_plot, **kwargs):
"""
Plot one or several objects.
If a list is provided, then (optionally) a list of labels may be provided.
Parameters
----------
objects_to_plot : NormalizedLine, PointHomogeneous, RationalMechanism, MotionFactorization, DualQuaternion, TransfMatrix, RationalCurve, RationalBezier, MiniBall, or list
The object(s) to plot.
**kwargs
Additional plotting options. If 'label' is provided as a list, each object will be labeled accordingly.
"""
if isinstance(objects_to_plot, list):
label_list = kwargs.pop('label', None)
for i, obj in enumerate(objects_to_plot):
if label_list is not None:
kwargs['label'] = label_list[i]
self._plot(obj, **kwargs)
else:
self._plot(objects_to_plot, **kwargs)
self.widget.update()
[docs]
def add_item(self, item):
"""
Add a custom pyqtgraph OpenGL item to the current 3D view.
Parameters
----------
item : object
Any GL item instance compatible with ``GLViewWidget.addItem``
(e.g. ``gl.GLSurfacePlotItem``).
"""
self.widget.addItem(item)
self.widget.update()
def _plot(self, object_to_plot, **kwargs):
"""
Dispatch to the proper plotting method based on the object type.
Parameters
----------
object_to_plot : object
The object to plot.
**kwargs
Additional plotting options.
"""
type_to_plot = self.analyze_object(object_to_plot)
if type_to_plot == "is_line":
self._plot_line(object_to_plot, **kwargs)
elif type_to_plot == "is_point":
self._plot_point(object_to_plot, **kwargs)
elif type_to_plot == "is_motion_factorization":
self._plot_motion_factorization(object_to_plot, **kwargs)
elif type_to_plot == "is_dq":
self._plot_dual_quaternion(object_to_plot, **kwargs)
elif type_to_plot == "is_transf_matrix":
self._plot_transf_matrix(object_to_plot, **kwargs)
elif type_to_plot == "is_gauss_legendre":
self._plot_gauss_legendre(object_to_plot, **kwargs)
elif type_to_plot == "is_rational_curve":
self._plot_rational_curve(object_to_plot, **kwargs)
elif type_to_plot == "is_rational_bezier":
self._plot_rational_bezier(object_to_plot, **kwargs)
elif type_to_plot == "is_rational_mechanism":
self._plot_rational_mechanism(object_to_plot, **kwargs)
elif type_to_plot == "is_miniball":
self._plot_miniball(object_to_plot, **kwargs)
elif type_to_plot == "is_line_segment":
self._plot_line_segment(object_to_plot, **kwargs)
elif type_to_plot == "is_point_orbit":
self._plot_point_orbit(object_to_plot, **kwargs)
elif type_to_plot == "is_plane":
self._plot_plane(object_to_plot, **kwargs)
else:
raise TypeError("Unsupported type for plotting.")
[docs]
def analyze_object(self, object_to_plot):
"""
Analyze the object type so that the proper plotting method is called.
Parameters
----------
object_to_plot : object
The object to analyze.
Returns
-------
str
A string indicating the type of object for plotting.
"""
if isinstance(object_to_plot, RationalMechanism):
return "is_rational_mechanism"
elif isinstance(object_to_plot, MotionFactorization):
return "is_motion_factorization"
elif isinstance(object_to_plot, NormalizedLine):
return "is_line"
elif isinstance(object_to_plot, PointHomogeneous):
return "is_point"
elif isinstance(object_to_plot, RationalSoo):
return "is_gauss_legendre"
elif isinstance(object_to_plot, RationalBezier):
return "is_rational_bezier"
elif isinstance(object_to_plot, RationalCurve):
return "is_rational_curve"
elif isinstance(object_to_plot, DualQuaternion):
return "is_dq"
elif isinstance(object_to_plot, TransfMatrix):
return "is_transf_matrix"
elif isinstance(object_to_plot, MiniBall):
return "is_miniball"
elif isinstance(object_to_plot, LineSegment):
return "is_line_segment"
elif isinstance(object_to_plot, PointOrbit):
return "is_point_orbit"
elif isinstance(object_to_plot, NormalizedPlane):
return "is_plane"
else:
raise TypeError("Unsupported type for plotting.")
[docs]
def plot_axis_between_two_points(self,
p0: PointHomogeneous,
p1: PointHomogeneous,
**kwargs):
"""
Plot an arrow (here as a simple line) from p0 to p1.
Parameters
----------
p0 : PointHomogeneous
Start point of the arrow.
p1 : PointHomogeneous
End point of the arrow.
**kwargs
Additional plotting options.
"""
pos0 = _flat_xyz(p0.normalized_euclidean())
pos1 = _flat_xyz(p1.normalized_euclidean())
pts = numpy.array([pos0, pos1])
color = self._get_color(kwargs.get('color', 'magenta'), (1, 1, 1, 1))
line = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line)
scatter = gl.GLScatterPlotItem(pos=numpy.array([pos1]),
color=color,
glOptions=self.render_mode,
size=5)
self.widget.addItem(scatter)
if 'label' in kwargs:
mid = (pos0 + pos1) / 2
self.widget.add_label(mid, kwargs['label'])
[docs]
def plot_line_segments_between_points(self,
points: list,
**kwargs):
"""
Plot a connected line (polyline) through a list of points.
Parameters
----------
points : list of PointHomogeneous
List of points through which the polyline is drawn.
**kwargs
Additional plotting options.
"""
pts = numpy.array([_flat_xyz(p.normalized_euclidean()) for p in points])
color = self._get_color(kwargs.get('color', 'green'), (1, 1, 1, 1))
line = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line)
def _plot_plane(self,
plane: NormalizedPlane,
xlim: tuple = (-1, 1),
ylim: tuple = (-1, 1),
**kwargs):
"""
Plot a plane as a semi-transparent mesh.
Parameters
----------
plane : NormalizedPlane
The plane to plot.
xlim : tuple, optional
The x-axis limits.
ylim : tuple, optional
The y-axis limits.
**kwargs
Additional plotting options.
"""
grid_points = plane.data_to_plot(xlim, ylim)
vertices, faces = self._create_mesh_from_grid(grid_points)
surface = gl.GLMeshItem(vertexes=vertices,
faces=faces,
color=self._get_color(
kwargs.get('color', (0.8, 0.2, 0.2, 0.2)),
(0.8, 0.2, 0.2, 0.2)),
smooth=False,
drawEdges=True,
edgeColor=(0.5, 0.5, 0.5, 1))
self.widget.addItem(surface)
@staticmethod
def _create_mesh_from_grid(grid_points: tuple):
"""
Create vertices and faces for a mesh given grid data.
Parameters
----------
grid_points : tuple of numpy.ndarray
Tuple of (x, y, z) grid arrays.
Returns
-------
vertices : numpy.ndarray
Array of mesh vertices.
faces : numpy.ndarray
Array of mesh faces (triangles).
"""
x, y, z = grid_points
m, n = x.shape
vertices = numpy.column_stack((x.flatten(), y.flatten(), z.flatten()))
faces = []
for i in range(m - 1):
for j in range(n - 1):
idx = i * n + j
faces.append([idx, idx + 1, idx + n])
faces.append([idx + 1, idx + n + 1, idx + n])
faces = numpy.array(faces)
return vertices, faces
def _plot_line(self, line: NormalizedLine, **kwargs):
"""
Plot a line as an arrow (here a simple line). The method
get_plot_data() is assumed to return a 6‑element array
[x0, y0, z0, dx, dy, dz].
"""
interval = kwargs.pop('interval', (-1, 1))
data = line.get_plot_data(interval)
start_pt = _flat_xyz(data[:3])
direction = _flat_xyz(data[3:])
end_pt = start_pt + direction
pts = numpy.array([start_pt, end_pt])
color = self._get_color(kwargs.get('color', 'magenta'), (1, 1, 1, 1))
line_item = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
tip_point = gl.GLScatterPlotItem(pos=numpy.array([end_pt]),
color=color,
glOptions=self.render_mode,
size=5)
self.widget.addItem(tip_point)
if 'label' in kwargs:
mid = start_pt + direction / 2
self.widget.add_label(mid, kwargs['label'])
def _plot_point(self, point: PointHomogeneous, **kwargs):
"""
Plot a point as a marker.
"""
size = kwargs.pop('size', 4)
pos = _flat_xyz(point.get_plot_data())
color = self._get_color(kwargs.get('color', 'red'), (1, 0, 0, 1))
scatter = gl.GLScatterPlotItem(pos=numpy.array([pos]),
color=color,
glOptions=self.render_mode,
size=size)
self.widget.addItem(scatter)
if 'label' in kwargs:
self.widget.add_label(pos, kwargs['label'])
def _plot_dual_quaternion(self, dq: DualQuaternion, **kwargs):
"""
Plot a dual quaternion by converting it to a transformation matrix.
"""
matrix = TransfMatrix(dq.dq2matrix())
self._plot_transf_matrix(matrix, **kwargs)
def _plot_transf_matrix(self, matrix: TransfMatrix, **kwargs):
"""
Plot a transformation matrix as three arrows (x, y, and z axes).
"""
origin = _flat_xyz(matrix.t)
x_axis = numpy.array([origin, origin + self.arrows_length * _flat_xyz(matrix.n)])
y_axis = numpy.array([origin, origin + self.arrows_length * _flat_xyz(matrix.o)])
z_axis = numpy.array([origin, origin + self.arrows_length * _flat_xyz(matrix.a)])
x_line = gl.GLLinePlotItem(pos=x_axis,
color=(1, 0, 0, 1),
glOptions=self.render_mode,
width=2,
antialias=True)
y_line = gl.GLLinePlotItem(pos=y_axis,
color=(0, 1, 0, 1),
glOptions=self.render_mode,
width=2,
antialias=True)
z_line = gl.GLLinePlotItem(pos=z_axis,
color=(0, 0, 1, 1),
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(x_line)
self.widget.addItem(y_line)
self.widget.addItem(z_line)
if 'label' in kwargs:
self.widget.add_label(origin, kwargs['label'])
def _plot_rational_curve(self, curve: RationalCurve, **kwargs):
"""
Plot a rational curve as a line. Optionally, plot poses along the curve.
"""
interval = kwargs.pop('interval', (0, 1))
if kwargs.pop('with_poses', False):
if interval == 'closed':
t_space = numpy.tan(numpy.linspace(-numpy.pi / 2, numpy.pi / 2, 51))
else:
t_space = numpy.linspace(interval[0], interval[1], 50)
for t in t_space:
pose_dq = DualQuaternion(curve.evaluate(t))
self._plot_dual_quaternion(pose_dq)
x, y, z = curve.get_plot_data(interval, self.steps)
pts = numpy.column_stack((x, y, z))
color = self._get_color(kwargs.get('color', 'orange'), (1, 1, 0, 1))
line_item = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
def _plot_rational_bezier(self,
bezier: RationalBezier,
plot_control_points: bool = True,
**kwargs):
"""
Plot a rational Bézier curve along with its control points.
"""
interval = kwargs.pop('interval', (0, 1))
x, y, z, x_cp, y_cp, z_cp = bezier.get_plot_data(interval, self.steps)
pts = numpy.column_stack((x, y, z))
color = self._get_color(kwargs.get('color', 'yellow'), (1, 0, 1, 1))
line_item = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
if plot_control_points:
cp = numpy.column_stack((x_cp, y_cp, z_cp))
scatter = gl.GLScatterPlotItem(pos=cp,
color=(1, 0, 0, 1),
glOptions=self.render_mode,
size=8)
self.widget.addItem(scatter)
cp_line = gl.GLLinePlotItem(pos=cp,
color=(1, 0, 0, 1),
glOptions=self.render_mode,
width=1,
antialias=True)
self.widget.addItem(cp_line)
def _plot_gauss_legendre(self, curve: RationalSoo, plot_control_points: bool = True, **kwargs):
"""
Plot a Gauss-Legendre rational curve along with its control points.
Similar to plot Bezier, but specifically for Gauss-Legendre curves.
Parameters
----------
curve : RationalSoo
The Gauss-Legendre curve to plot.
plot_control_points : bool, optional
Whether to plot the control points (default is True).
**kwargs
Additional keyword arguments for customization. Common options include:
- color : tuple or str, optional
Color for the curve (default is yellow).
- interval : tuple, optional
Parameter interval for plotting (default is (-1, 1)).
Returns
-------
None
"""
interval = kwargs.pop('interval', (-1, 1))
x, y, z, x_cp, y_cp, z_cp = curve.get_plot_data(interval, self.steps)
pts = numpy.column_stack((x, y, z))
color = self._get_color(kwargs.get('color', 'yellow'), (1, 0, 1, 1))
line_item = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
if plot_control_points:
cp = numpy.column_stack((x_cp, y_cp, z_cp))
scatter = gl.GLScatterPlotItem(pos=cp,
color=(1, 0, 0, 1),
glOptions=self.render_mode,
size=8)
self.widget.addItem(scatter)
cp_line = gl.GLLinePlotItem(pos=cp,
color=(1, 0, 0, 1),
glOptions=self.render_mode,
width=1,
antialias=True)
self.widget.addItem(cp_line)
def _plot_motion_factorization(self, factorization: MotionFactorization, **kwargs):
"""
Plot the motion factorization as a 3D line.
"""
t = kwargs.pop('t', 0)
points = factorization.direct_kinematics(t)
pts = numpy.array(points)
color = self._get_color(kwargs.get('color', 'orange'), (1, 0.5, 0, 1))
line_item = gl.GLLinePlotItem(pos=pts,
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
def _plot_rational_mechanism(self, mechanism: RationalMechanism, **kwargs):
"""
Plot a rational mechanism by plotting its factorizations and the tool path.
"""
t = kwargs.pop('t', 0)
for factorization in mechanism.factorizations:
self._plot_motion_factorization(factorization, t=t, **kwargs)
pts0 = mechanism.factorizations[0].direct_kinematics_of_tool_with_link(
t, mechanism.tool_frame.dq2point_via_matrix())
pts1 = mechanism.factorizations[1].direct_kinematics_of_tool_with_link(
t, mechanism.tool_frame.dq2point_via_matrix())[::-1]
ee_points = numpy.concatenate((pts0, pts1))
color = self._get_color(kwargs.get('color', 'cyan'), (0, 1, 1, 1))
line_item = gl.GLLinePlotItem(pos=numpy.array(ee_points),
color=color,
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
self._plot_tool_path(mechanism, **kwargs)
def _plot_tool_path(self, mechanism: RationalMechanism, **kwargs):
"""
Plot the path of the tool.
"""
t_lin = numpy.linspace(0, 2 * numpy.pi, self.steps)
t_vals = [mechanism.factorizations[0].joint_angle_to_t_param(t_lin[i])
for i in range(self.steps)]
ee_points = [mechanism.factorizations[0].direct_kinematics_of_tool(
t_vals[i], mechanism.tool_frame.dq2point_via_matrix())
for i in range(self.steps)]
pts = numpy.array(ee_points)
line_item = gl.GLLinePlotItem(pos=pts,
color=(1, 0, 1, 1),
glOptions=self.render_mode,
width=2,
antialias=True)
self.widget.addItem(line_item)
def _plot_miniball(self, ball: MiniBall, **kwargs):
"""
Plot a MiniBall as a semi‑transparent mesh.
"""
raise NotImplementedError("TODO, make as point orbit")
def _plot_point_orbit(self, orbit: PointOrbit, **kwargs):
"""
Plot a point orbit as a semi‑transparent mesh.
"""
if 'color' in kwargs:
coloring = kwargs.pop('color')
else:
coloring = (1, 0.5, 0, 0.15)
center, radius = orbit.get_plot_data()
mesh = gl.MeshData.sphere(rows=8, cols=8, radius=radius)
sphere = gl.GLMeshItem(meshdata=mesh,
color=coloring,
glOptions='translucent',
drawFaces=True,
drawEdges=False)
sphere.translate(*center)
self.widget.addItem(sphere)
def _plot_line_segment(self, segment: LineSegment, **kwargs):
"""
Plot a line segment as a surface mesh.
"""
raise NotImplementedError("TODO, see matplotlib version")
[docs]
def animate_rotation(self,
save_images: bool = True,
number_of_frames: int = 20):
"""
Rotate the view around the z-axis to create a turntable effect.
If save_images is True, it will save images of each frame.
Parameters
----------
save_images : bool, optional
If True, save images of each frame (default is True).
number_of_frames : int, optional
Number of frames to generate (default is 20).
Returns
-------
None
"""
if save_images:
azimuth_step = 360 / number_of_frames
else:
azimuth_step = 5 # Default step if not saving images
img_counter = 0
def rotate():
nonlocal img_counter
# Check if we've completed the full rotation
if img_counter >= number_of_frames:
# Animation complete, show message and stop recursion
if save_images:
QtWidgets.QMessageBox.information(
self.widget, "Save Images",
f"{number_of_frames} images were saved as frame_XXX.png "
f"files."
)
return
self.widget.opts['azimuth'] += azimuth_step
self.widget.update()
if save_images:
filename = f"frame_{img_counter:03d}.png"
self.widget.grabFramebuffer().save(filename)
img_counter += 1
QtCore.QTimer.singleShot(50, rotate)
# Start the rotation
rotate()
[docs]
def show(self):
"""
Start the Qt event loop and display the plot window.
This method shows the main plotting widget and starts the Qt event loop,
blocking until the window is closed.
Returns
-------
None
"""
self.widget.show()
self.app.exec()
[docs]
def closeEvent(self, event):
"""
Handle the window close event and ensure the Qt application exits.
Parameters
----------
event : QCloseEvent
The close event triggered by the window manager.
Returns
-------
None
"""
self.app.quit()
event.accept()
if gl is not None:
class CustomGLViewWidget(gl.GLViewWidget):
"""
Custom GLViewWidget for 3D visualization with label overlay support.
Parameters
----------
white_background : bool, optional
If True, use a white background for the widget.
*args
Additional positional arguments for GLViewWidget.
**kwargs
Additional keyword arguments for GLViewWidget.
"""
def __init__(self, white_background=False, *args, **kwargs):
"""
Initialize the CustomGLViewWidget.
Parameters
----------
white_background : bool, optional
If True, use a white background for the widget.
*args
Additional positional arguments for GLViewWidget.
**kwargs
Additional keyword arguments for GLViewWidget.
"""
kwargs['rotationMethod'] = 'quaternion'
super().__init__(*args, **kwargs)
self.labels = []
self.white_background = white_background
# Create an overlay widget for displaying text
self.text_overlay = QtWidgets.QWidget(self)
self.text_overlay.setAttribute(
QtCore.Qt.WidgetAttribute.WA_TransparentForMouseEvents)
self.text_overlay.setStyleSheet("background:transparent;")
self.text_overlay.resize(self.size())
self.text_overlay.show()
def resizeEvent(self, event):
"""
Handle widget resize event and resize the text overlay accordingly.
Parameters
----------
event : QResizeEvent
The resize event.
"""
super().resizeEvent(event)
if hasattr(self, 'text_overlay'):
self.text_overlay.resize(self.size())
def add_label(self, point, text):
"""
Add a label for a 3D point.
Parameters
----------
point : object
The 3D point to label.
text : str
The label text.
"""
self.labels.append({'point': point, 'text': text})
self.update()
def paintEvent(self, event):
"""
Handle the paint event for the widget and schedule label overlay update.
Parameters
----------
event : QPaintEvent
The paint event.
"""
# Only handle standard OpenGL rendering here - no mixing with QPainter
super().paintEvent(event)
# Schedule label painting as a separate operation
QtCore.QTimer.singleShot(0, self.update_text_overlay)
def projectionMatrix(self, region=None):
if region is None:
region = (0, 0, self.deviceWidth(), self.deviceHeight())
x0, y0, w, h = self.getViewport()
dist = self.opts['distance']
fov = self.opts['fov']
from math import tan, radians
r = dist * tan(0.5 * radians(fov))
t = r * h / w
tr = QtGui.QMatrix4x4()
tr.ortho(-r, r, -t, t, -dist * 1000., dist * 1000.)
return tr
def update_text_overlay(self):
"""
Update the text overlay with current labels.
"""
# Create a new painter for the overlay widget
self.text_overlay.update()
def _obtain_label_vec(self, pt):
"""
Obtain the label vector for a 3D point.
Parameters
----------
pt : object
The point object (various supported types).
Returns
-------
QVector4D
The homogeneous 4D vector for the label position.
"""
# Convert the 3D point to homogeneous coordinates
if isinstance(pt, numpy.ndarray):
point_vec = pt
elif isinstance(pt, PointHomogeneous):
point_vec = [pt.coordinates_normalized[1],
pt.coordinates_normalized[2],
pt.coordinates_normalized[3]]
elif isinstance(pt, TransfMatrix):
point_vec = [pt.t[0], pt.t[1], pt.t[2]]
elif isinstance(pt, FramePlotHelper):
point_vec = [pt.tr.t[0], pt.tr.t[1], pt.tr.t[2]]
else: # is pyqtgraph marker (scatter)
point_vec = [pt.pos[0][0], pt.pos[0][1], pt.pos[0][2]]
return QtGui.QVector4D(point_vec[0], point_vec[1], point_vec[2], 1.0)
# This method renders text on the overlay
def paintOverlay(self, event):
"""
Render text labels on the overlay widget.
Parameters
----------
event : QPaintEvent
The paint event.
"""
painter = QtGui.QPainter(self.text_overlay)
painter.setRenderHint(QtGui.QPainter.RenderHint.Antialiasing)
if self.white_background:
painter.setPen(QtGui.QColor(QtCore.Qt.GlobalColor.black))
else:
painter.setPen(QtGui.QColor(QtCore.Qt.GlobalColor.white))
# Get the Model-View-Projection matrix
# pyqtgraph 0.14+ uses a projection stack (currentProjection method)
# pyqtgraph 0.13 and earlier use projectionMatrix() with no args
try:
# Try 0.13 API
projection_matrix = self.projectionMatrix()
except AttributeError:
# Fall back for 0.14+ API
# TODO: check in the future
projection_matrix = self.currentProjection()
view_matrix = self.viewMatrix()
mvp = projection_matrix * view_matrix
# Draw all labels
for entry in self.labels:
point = entry['point']
text = entry['text']
projected = mvp.map(self._obtain_label_vec(point))
if projected.w() != 0:
ndc_x = projected.x() / projected.w()
ndc_y = projected.y() / projected.w()
# Check if the point is in front of the camera
if projected.z() / projected.w() < 1.0:
x = int((ndc_x * 0.5 + 0.5) * self.width())
y = int((1 - (ndc_y * 0.5 + 0.5)) * self.height())
painter.drawText(x, y, text)
painter.end()
def showEvent(self, event):
"""
Handle the show event and install the event filter for the text overlay.
Parameters
----------
event : QShowEvent
The show event.
"""
super().showEvent(event)
self.text_overlay.installEventFilter(self)
def eventFilter(self, obj, event):
"""
Event filter for handling paint events on the text overlay.
Parameters
----------
obj : QObject
The object being filtered.
event : QEvent
The event to filter.
Returns
-------
bool
True if the event was handled, False otherwise.
"""
if obj is self.text_overlay and event.type() == QtCore.QEvent.Type.Paint:
self.paintOverlay(event)
return True
return super().eventFilter(obj, event)
else:
CustomGLViewWidget = None
if gl is not None:
[docs]
class FramePlotHelper:
"""
Helper class to create a coordinate frame using three GLLinePlotItems.
Parameters
----------
transform : TransfMatrix, optional
The initial transformation matrix.
width : float, optional
The width of the lines.
length : float, optional
The length of the axes.
antialias : bool, optional
Whether to use antialiasing.
"""
def __init__(self,
transform: TransfMatrix = TransfMatrix(),
width: float = 2.,
length: float = 1.,
antialias: bool = True):
"""
Initialize the FramePlotHelper.
Parameters
----------
transform : TransfMatrix, optional
The initial transformation matrix.
width : float, optional
The width of the lines.
length : float, optional
The length of the axes.
antialias : bool, optional
Whether to use antialiasing.
"""
# Create GLLinePlotItems for the three axes.
# The initial positions are placeholders; they will be set properly in setData().
self.x_axis = gl.GLLinePlotItem(pos=numpy.zeros((2, 3)),
color=(1, 0, 0, 0.5),
glOptions='translucent',
width=width,
antialias=antialias)
self.y_axis = gl.GLLinePlotItem(pos=numpy.zeros((2, 3)),
color=(0, 1, 0, 0.5),
glOptions='translucent',
width=width,
antialias=antialias)
self.z_axis = gl.GLLinePlotItem(pos=numpy.zeros((2, 3)),
color=(0, 0, 1, 0.5),
glOptions='translucent',
width=width,
antialias=antialias)
# Set the initial transformation
self.tr = transform
self.length = length
self.setData(transform)
[docs]
def setData(self, transform: TransfMatrix):
"""
Update the coordinate frame using a new 4x4 transformation matrix.
Parameters
----------
transform : TransfMatrix
The new transformation matrix.
"""
self.tr = transform
# Update the positions for each axis.
t = _flat_xyz(transform.t)
n = _flat_xyz(transform.n)
o = _flat_xyz(transform.o)
a = _flat_xyz(transform.a)
self.x_axis.setData(pos=numpy.array([t, t + self.length * n]))
self.y_axis.setData(pos=numpy.array([t, t + self.length * o]))
self.z_axis.setData(pos=numpy.array([t, t + self.length * a]))
[docs]
def addToView(self, view: gl.GLViewWidget):
"""
Add all three axes to a GLViewWidget.
Parameters
----------
view : gl.GLViewWidget
The view to add the axes to.
"""
view.addItem(self.x_axis)
view.addItem(self.y_axis)
view.addItem(self.z_axis)
else:
FramePlotHelper = None
if QtWidgets is not None:
[docs]
class InteractivePlotterWidget(QtWidgets.QWidget):
"""
A QWidget that contains a PlotterPyqtgraph 3D view and interactive controls.
Contains sliders and text boxes for plotting and manipulating a mechanism.
Parameters
----------
mechanism : RationalMechanism
The mechanism to plot and manipulate.
base : TransfMatrix or DualQuaternion, optional
The base frame for plotting.
show_tool : bool, optional
Whether to show the tool (end-effector) frame.
steps : int, optional
Number of discrete steps for the curve path.
joint_sliders_lim : float, optional
Limit for the joint sliders.
arrows_length : float, optional
Length of the arrows of plotted frames.
white_background : bool, optional
If True, use a white background for the plot.
parent : QWidget, optional
Parent widget.
parent_app : QApplication, optional
Parent Qt application instance.
"""
def __init__(self,
mechanism: RationalMechanism,
base=None,
show_tool: bool = True,
steps: int = 1000,
joint_sliders_lim: float = 1.0,
arrows_length: float = 1.0,
white_background: bool = False,
parent=None,
parent_app=None):
"""
Initialize the InteractivePlotterWidget.
Parameters
----------
mechanism : RationalMechanism
The mechanism to plot and manipulate.
base : TransfMatrix or DualQuaternion, optional
The base frame for plotting.
show_tool : bool, optional
Whether to show the tool (end-effector) frame.
steps : int, optional
Number of discrete steps for the curve path.
joint_sliders_lim : float, optional
Limit for the joint sliders.
arrows_length : float, optional
Length of the arrows of plotted frames.
white_background : bool, optional
If True, use a white background for the plot.
parent : QWidget, optional
Parent widget.
parent_app : QApplication, optional
Parent Qt application instance.
"""
super().__init__(parent)
self.setMinimumSize(800, 600)
self.mechanism = mechanism
self.show_tool = show_tool
self.steps = steps
self.joint_sliders_lim = joint_sliders_lim
self.arrows_length = arrows_length
if base is not None:
if isinstance(base, TransfMatrix):
if not base.is_rotation():
raise ValueError("Given matrix is not proper rotation.")
self.base = base
self.base_arr = self.base.array()
elif isinstance(base, DualQuaternion):
self.base = TransfMatrix(base.dq2matrix())
self.base_arr = self.base.array()
else:
raise TypeError("Base must be a TransfMatrix or DualQuaternion instance.")
else:
self.base = None
self.base_arr = None
self.white_background = white_background
if self.white_background:
self.render_mode = 'translucent'
else:
self.render_mode = 'additive'
# Create the PlotterPyqtgraph instance.
self.plotter = PlotterPyqtgraph(base=None,
steps=self.steps,
arrows_length=self.arrows_length,
white_background=self.white_background,
parent_app=parent_app)
# Optionally adjust the camera.
self.plotter.widget.setCameraPosition(
distance=10,
rotation=QtGui.QQuaternion.fromEulerAngles(-70, 0, -30))
# Main layout: split between the 3D view and a control panel.
main_layout = QtWidgets.QHBoxLayout(self)
# Add the 3D view (PlotterPyqtgraph’s widget) to the layout.
main_layout.addWidget(self.plotter.widget, stretch=5)
# Create the control panel (on the right).
control_panel = QtWidgets.QWidget()
control_layout = QtWidgets.QVBoxLayout(control_panel)
# --- Driving joint angle slider ---
control_layout.addWidget(QtWidgets.QLabel("Joint angle [rad]:"))
self.move_slider = self.create_float_slider(0.0, 2 * numpy.pi, 0.0,
orientation=QtCore.Qt.Orientation.Horizontal)
control_layout.addWidget(self.move_slider)
# --- Text boxes ---
self.text_box_angle = QtWidgets.QLineEdit()
self.text_box_angle.setPlaceholderText("Set angle [rad]:")
control_layout.addWidget(self.text_box_angle)
self.text_box_param = QtWidgets.QLineEdit()
self.text_box_param.setPlaceholderText("Set parameter t [-]:")
control_layout.addWidget(self.text_box_param)
self.save_mech_pkl = QtWidgets.QLineEdit()
self.save_mech_pkl.setPlaceholderText("Save mechanism PKL, filename:")
control_layout.addWidget(self.save_mech_pkl)
self.save_figure_box = QtWidgets.QLineEdit()
self.save_figure_box.setPlaceholderText("Save figure PNG, filename:")
control_layout.addWidget(self.save_figure_box)
# --- Joint connection sliders ---
joint_sliders_layout = QtWidgets.QHBoxLayout()
self.joint_sliders = []
# Initialize sliders for each joint
for i in range(self.mechanism.num_joints):
slider0, slider1 = self._init_joint_sliders(i, self.joint_sliders_lim)
self.joint_sliders.append(slider0)
self.joint_sliders.append(slider1)
# Arrange sliders vertically for each joint
joint_layout = QtWidgets.QVBoxLayout()
joint_layout.addWidget(QtWidgets.QLabel(f"j{i}cp0"))
joint_layout.addWidget(slider0)
joint_layout.addWidget(QtWidgets.QLabel(f"j{i}cp1"))
joint_layout.addWidget(slider1)
joint_sliders_layout.addLayout(joint_layout)
control_layout.addLayout(joint_sliders_layout)
# Set default values for the first factorization
for i in range(self.mechanism.factorizations[0].number_of_factors):
default_val0 = self.mechanism.factorizations[0].linkage[i].points_params[0]
default_val1 = self.mechanism.factorizations[0].linkage[i].points_params[1]
self.joint_sliders[2 * i].setValue(int(default_val0 * 100))
self.joint_sliders[2 * i + 1].setValue(int(default_val1 * 100))
# Set default values for the second factorization
offset = 2 * self.mechanism.factorizations[0].number_of_factors
for i in range(self.mechanism.factorizations[1].number_of_factors):
default_val0 = self.mechanism.factorizations[1].linkage[i].points_params[0]
default_val1 = self.mechanism.factorizations[1].linkage[i].points_params[1]
self.joint_sliders[offset + 2 * i].setValue(int(default_val0 * 100))
self.joint_sliders[offset + 2 * i + 1].setValue(int(default_val1 * 100))
main_layout.addWidget(control_panel, stretch=1)
# --- Initialize plot items for the mechanism links ---
self.lines = []
num_lines = self.mechanism.num_joints * 2
# base link in orange
line_item = gl.GLLinePlotItem(pos=numpy.zeros((2, 3)),
color=(1, 0.5, 0, 1),
glOptions=self.render_mode,
width=5,
antialias=True)
self.lines.append(line_item)
self.plotter.widget.addItem(line_item)
for i in range(1, num_lines):
# if i is even, make the link color green, and joints red
if i % 2 == 0:
line_item = gl.GLLinePlotItem(pos=numpy.zeros((2, 3)),
color=(1, 1, 0, 1), # yellow (links)
glOptions=self.render_mode,
width=5,
antialias=True)
else:
line_item = gl.GLLinePlotItem(pos=numpy.zeros((2, 3)),
color=(1, 0, 1, 1), # magenta (joints)
glOptions=self.render_mode,
width=5,
antialias=True)
self.lines.append(line_item)
self.plotter.widget.addItem(line_item)
# --- If desired, initialize tool plot and tool frame ---
if self.show_tool:
self.tool_link = gl.GLLinePlotItem(pos=numpy.zeros((3, 3)),
color=(1, 1, 0, 0.5), # 50% yellow
glOptions=self.render_mode,
width=5,
antialias=True)
self.plotter.widget.addItem(self.tool_link)
self.tool_frame = FramePlotHelper(
transform=TransfMatrix(self.mechanism.tool_frame.dq2matrix()),
length=self.arrows_length)
self.tool_frame.addToView(self.plotter.widget)
# --- Plot the tool path ---
self._plot_tool_path()
# --- Connect signals to slots ---
self.move_slider.valueChanged.connect(self.on_move_slider_changed)
self.text_box_angle.returnPressed.connect(self.on_angle_text_entered)
self.text_box_param.returnPressed.connect(self.on_param_text_entered)
self.save_mech_pkl.returnPressed.connect(self.on_save_save_mech_pkl)
self.save_figure_box.returnPressed.connect(self.on_save_figure_box)
for slider in self.joint_sliders:
slider.valueChanged.connect(self.on_joint_slider_changed)
# Set initial configuration (home position)
self.move_slider.setValue(self.move_slider.minimum())
self.plot_slider_update(self.move_slider.value() / 100.0)
self.setWindowTitle('Rational Linkages')
# --- Helper to create a “float slider” (using integer scaling) ---
[docs]
def create_float_slider(self, min_val, max_val, init_val,
orientation=QtCore.Qt.Orientation.Horizontal):
"""
Create a float slider using integer scaling.
Parameters
----------
min_val : float
Minimum value for the slider.
max_val : float
Maximum value for the slider.
init_val : float
Initial value for the slider.
orientation : QtCore.Qt.Orientation, optional
Orientation of the slider (horizontal or vertical).
Returns
-------
QSlider
The created slider.
"""
slider = QtWidgets.QSlider(orientation)
slider.setMinimum(int(min_val * 100))
slider.setMaximum(int(max_val * 100))
slider.setValue(int(init_val * 100))
slider.setTickPosition(QtWidgets.QSlider.TickPosition.TicksBelow)
slider.setTickInterval(10)
return slider
def _init_joint_sliders(self, idx, slider_limit):
"""
Create a pair of vertical sliders for joint connection parameters.
Parameters
----------
idx : int
Index of the joint.
slider_limit : float
Limit for the slider values (scaled by 100).
Returns
-------
tuple of QSlider
The created pair of sliders.
"""
slider0 = self.create_float_slider(-slider_limit,
slider_limit,
0.0,
orientation=QtCore.Qt.Orientation.Vertical)
slider1 = self.create_float_slider(-slider_limit,
slider_limit,
0.0,
orientation=QtCore.Qt.Orientation.Vertical)
return slider0, slider1
def _plot_tool_path(self):
"""
Plot the tool path (as a continuous line) using a set of computed points.
"""
t_lin = numpy.linspace(0, 2 * numpy.pi, self.steps)
t_vals = [self.mechanism.factorizations[0].joint_angle_to_t_param(t)
for t in t_lin]
ee_points = [self.mechanism.factorizations[0].direct_kinematics_of_tool(
t, self.mechanism.tool_frame.dq2point_via_matrix())
for t in t_vals]
if self.base_arr is not None:
# transform the end-effector points by the base transformation
ee_points = [self.base_arr @ numpy.insert(p, 0, 1) for p in ee_points]
# normalize
ee_points = [p[1:4]/p[0] for p in ee_points]
pts = numpy.array(ee_points)
tool_path = gl.GLLinePlotItem(pos=pts,
color=(0.5, 0.5, 0.5, 1),
glOptions=self.render_mode,
width=2,
antialias=True)
self.plotter.widget.addItem(tool_path)
# --- Slots for interactive control events ---
[docs]
def on_move_slider_changed(self, value):
"""
Called when the driving joint angle slider is moved.
Parameters
----------
value : int
The slider value (scaled by 100).
"""
angle = value / 100.0 # Convert back to a float value.
self.plot_slider_update(angle)
[docs]
def on_angle_text_entered(self):
"""
Called when the angle text box is submitted.
"""
try:
val = float(self.text_box_angle.text())
# Normalize angle to [0, 2*pi]
if val >= 0:
val = val % (2 * numpy.pi)
else:
val = (val % (2 * numpy.pi)) - numpy.pi
self.move_slider.setValue(int(val * 100))
except ValueError:
pass
[docs]
def on_param_text_entered(self):
"""
Called when the t-parameter text box is submitted.
"""
try:
val = float(self.text_box_param.text())
self.plot_slider_update(val, t_param=val)
joint_angle = self.mechanism.factorizations[0].t_param_to_joint_angle(val)
self.move_slider.setValue(int(joint_angle * 100))
except ValueError:
pass
[docs]
def on_save_save_mech_pkl(self):
"""
Called when the save text box is submitted.
"""
filename = self.save_mech_pkl.text()
self.mechanism.save(filename=filename)
QtWidgets.QMessageBox.information(self,
"Success",
f"Mechanism saved as {filename}.pkl")
[docs]
def on_save_figure_box(self):
"""
Called when the filesave text box is submitted.
Saves the current figure in the specified format.
"""
filename = self.save_figure_box.text()
# better quality but does not save the text overlay
#self.plotter.widget.readQImage().save(filename + "_old.png")
#self.plotter.widget.readQImage().save(filename + "_old.png", quality=100)
image = QtGui.QImage(self.plotter.widget.size(),
QtGui.QImage.Format.Format_ARGB32_Premultiplied)
image.fill(QtCore.Qt.GlobalColor.transparent)
# Create a painter and render the widget into the image
painter = QtGui.QPainter(image)
self.plotter.widget.render(painter)
painter.end()
# Save the image
image.save(filename + ".png", "PNG", 80)
QtWidgets.QMessageBox.information(self,
"Success",
f"Figure saved as {filename}.png")
[docs]
def on_joint_slider_changed(self, value):
"""
Called when any joint slider is changed.
Updates the joint connection parameters and refreshes the plot.
Parameters
----------
value : int
The slider value (scaled by 100).
"""
num_of_factors = self.mechanism.factorizations[0].number_of_factors
# Update first factorization's linkage parameters.
for i in range(num_of_factors):
self.mechanism.factorizations[0].linkage[i].set_point_by_param(
0, self.joint_sliders[2 * i].value() / 100.0)
self.mechanism.factorizations[0].linkage[i].set_point_by_param(
1, self.joint_sliders[2 * i + 1].value() / 100.0)
# Update second factorization's linkage parameters.
for i in range(num_of_factors):
self.mechanism.factorizations[1].linkage[i].set_point_by_param(
0, self.joint_sliders[2 * num_of_factors + 2 * i].value() / 100.0)
self.mechanism.factorizations[1].linkage[i].set_point_by_param(
1, self.joint_sliders[2 * num_of_factors + 1 + 2 * i].value() / 100.0)
self.plot_slider_update(self.move_slider.value() / 100.0)
[docs]
def plot_slider_update(self, angle, t_param=None):
"""
Update the mechanism plot based on the current joint angle or t parameter.
Parameters
----------
angle : float
The joint angle value.
t_param : float, optional
The t parameter for the driving joint, if provided.
"""
if t_param is not None:
t = t_param
else:
t = self.mechanism.factorizations[0].joint_angle_to_t_param(angle)
# Compute link positions.
links = (self.mechanism.factorizations[0].direct_kinematics(t) +
self.mechanism.factorizations[1].direct_kinematics(t)[::-1])
links.insert(0, links[-1])
if self.base is not None:
# Transform the links by the base transformation.
links = [self.base_arr @ numpy.insert(p, 0, 1) for p in links]
# Normalize the homogeneous coordinates.
links = [p[1:4] / p[0] for p in links]
# Update each line segment.
for i, line in enumerate(self.lines):
pt1 = links[i]
pt2 = links[i+1]
pts = numpy.array([pt1, pt2])
line.setData(pos=pts)
if self.show_tool:
pts0 = self.mechanism.factorizations[0].direct_kinematics(t)[-1]
pts1 = self.mechanism.factorizations[0].direct_kinematics_of_tool(
t, self.mechanism.tool_frame.dq2point_via_matrix())
pts2 = self.mechanism.factorizations[1].direct_kinematics(t)[-1]
# shift points to thirds
mid_pt = (pts0 + pts2) / 2
pts0_3 = (mid_pt + pts0) / 2
pts2_3 = (mid_pt + pts2) / 2
# tool_triangle = [pts0, pts1, pts2]
tool_triangle = [pts0_3, pts1, pts2_3]
if self.base is not None:
# Transform the tool triangle by the base transformation.
tool_triangle = [self.base_arr @ numpy.insert(p, 0, 1)
for p in tool_triangle]
# Normalize the homogeneous coordinates.
tool_triangle = [p[1:4] / p[0] for p in tool_triangle]
self.tool_link.setData(pos=numpy.array(tool_triangle))
# Update tool frame (pose) arrows.
pose_dq = DualQuaternion(self.mechanism.evaluate(t))
# Compute the pose matrix by composing the mechanism’s pose and tool frame.
pose_matrix = TransfMatrix(pose_dq.dq2matrix()) * TransfMatrix(
self.mechanism.tool_frame.dq2matrix())
if self.base is not None:
# Transform the pose matrix by the base transformation.
pose_matrix = self.base * pose_matrix
self.tool_frame.setData(pose_matrix)
self.plotter.widget.update()
else:
InteractivePlotterWidget = None
[docs]
class InteractivePlotter:
"""
Main application class for the interactive plotting of mechanisms.
Encapsulates the QApplication and the InteractivePlotter widget.
Parameters
----------
mechanism : RationalMechanism
The mechanism to be plotted.
base : TransfMatrix or DualQuaternion, optional
The base frame for plotting.
show_tool : bool, optional
Whether to show the tool (end-effector) frame.
steps : int, optional
Number of discrete steps for the curve path.
joint_sliders_lim : float, optional
Limit for the joint sliders.
arrows_length : float, optional
Length of the arrows of plotted frames.
white_background : bool, optional
If True, use a white background for the plot.
"""
def __init__(self,
mechanism: RationalMechanism,
base=None,
show_tool=True,
steps=1000,
joint_sliders_lim=1.0,
arrows_length=1.0,
white_background: bool = False):
"""
Initialize the InteractivePlotter application.
Parameters
----------
mechanism : RationalMechanism
The mechanism to be plotted.
base : TransfMatrix or DualQuaternion, optional
The base frame for plotting.
show_tool : bool, optional
Whether to show the tool (end-effector) frame.
steps : int, optional
Number of discrete steps for the curve path.
joint_sliders_lim : float, optional
Limit for the joint sliders.
arrows_length : float, optional
Length of the arrows of plotted frames.
white_background : bool, optional
If True, use a white background for the plot.
"""
self.app = QApplication.instance()
if self.app is None:
self.app = QApplication(sys.argv)
self.window = InteractivePlotterWidget(mechanism=mechanism,
base=base,
show_tool=show_tool,
steps=steps,
joint_sliders_lim=joint_sliders_lim,
arrows_length=arrows_length,
white_background=white_background,
parent_app=self.app)
self.gl = self.window.plotter.gl
# self.window.show()
# self.app.processEvents()
# self.window.hide()
[docs]
def plot(self, *args, **kwargs):
"""
Plot the given objects in the motion designer widget.
Parameters
----------
*args
The objects to plot.
**kwargs
Additional keyword arguments for the plotter.
"""
# self.window.show()
# self.app.processEvents()
# self.window.hide()
self.window.plotter.plot(*args, **kwargs)
[docs]
def plot_axis_between_two_points(self,
p0: PointHomogeneous,
p1: PointHomogeneous,
**kwargs):
"""
Plot an axis between two points in the motion designer widget.
Parameters
----------
p0 : PointHomogeneous
Foot point of the axis.
p1 : PointHomogeneous
Tip of the axis.
**kwargs
Additional keyword arguments for the plotter.
"""
self.window.plotter.plot_axis_between_two_points(p0, p1, **kwargs)
[docs]
def plot_line_segments_between_points(self, points: list, **kwargs):
"""
Plot line segments between two points in the motion designer widget.
Parameters
----------
points : list
The list of points of polyline segments to plot.
**kwargs
Additional keyword arguments for the plotter.
"""
self.window.plotter.plot_line_segments_between_points(points, **kwargs)
[docs]
def show(self):
"""
Run the application and display the motion designer widget.
This method shows the main interactive plotting window and starts the Qt event loop,
blocking until the window is closed.
Returns
-------
None
"""
self.window.show()
self.app.exec()
[docs]
def closeEvent(self, event):
"""
Handle the close event for the application and ensure all windows are closed.
Parameters
----------
event : QCloseEvent
The close event triggered by the window manager.
Returns
-------
None
"""
self.app.closeAllWindows()
self.app.quit()
event.accept()