rational-linkages 2.1.0__cp310-cp310-win_amd64.whl → 2.2.1__cp310-cp310-win_amd64.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- rational_linkages/CollisionAnalyser.py +12 -7
- rational_linkages/CollisionFreeOptimization.py +4 -1
- rational_linkages/DualQuaternion.py +5 -2
- rational_linkages/FactorizationProvider.py +6 -5
- rational_linkages/MiniBall.py +8 -1
- rational_linkages/MotionApproximation.py +5 -1
- rational_linkages/MotionDesigner.py +549 -541
- rational_linkages/MotionFactorization.py +6 -5
- rational_linkages/MotionInterpolation.py +1 -1
- rational_linkages/PlotterMatplotlib.py +16 -4
- rational_linkages/PlotterPyqtgraph.py +487 -470
- rational_linkages/RationalBezier.py +2 -3
- rational_linkages/RationalCurve.py +12 -3
- rational_linkages/RationalDualQuaternion.py +5 -4
- rational_linkages/RationalMechanism.py +4 -4
- rational_linkages/SingularityAnalysis.py +2 -3
- rational_linkages/utils.py +60 -3
- rational_linkages/utils_rust.cp310-win_amd64.pyd +0 -0
- {rational_linkages-2.1.0.dist-info → rational_linkages-2.2.1.dist-info}/METADATA +32 -18
- rational_linkages-2.2.1.dist-info/RECORD +40 -0
- rational_linkages-2.1.0.dist-info/RECORD +0 -40
- {rational_linkages-2.1.0.dist-info → rational_linkages-2.2.1.dist-info}/WHEEL +0 -0
- {rational_linkages-2.1.0.dist-info → rational_linkages-2.2.1.dist-info}/licenses/LICENSE +0 -0
- {rational_linkages-2.1.0.dist-info → rational_linkages-2.2.1.dist-info}/top_level.txt +0 -0
@@ -1,25 +1,36 @@
|
|
1
1
|
import sys
|
2
|
-
from typing import Union
|
3
|
-
|
4
2
|
import numpy as np
|
5
|
-
import pyqtgraph.opengl as gl
|
6
3
|
|
7
|
-
|
8
|
-
from
|
4
|
+
from typing import Union
|
5
|
+
from warnings import warn
|
9
6
|
|
10
|
-
# Import your custom classes (adjust the import paths as needed)
|
11
7
|
from .DualQuaternion import DualQuaternion
|
12
8
|
from .MotionInterpolation import MotionInterpolation
|
13
|
-
from .PlotterPyqtgraph import (
|
14
|
-
FramePlotHelper,
|
15
|
-
InteractivePlotterWidget,
|
16
|
-
PlotterPyqtgraph,
|
17
|
-
)
|
18
9
|
from .PointHomogeneous import PointHomogeneous
|
19
10
|
from .RationalCurve import RationalCurve
|
20
11
|
from .RationalMechanism import RationalMechanism
|
21
12
|
from .TransfMatrix import TransfMatrix
|
22
13
|
|
14
|
+
# Try importing GUI components
|
15
|
+
try:
|
16
|
+
import pyqtgraph.opengl as gl
|
17
|
+
from PyQt6 import QtCore, QtWidgets
|
18
|
+
from .PlotterPyqtgraph import (
|
19
|
+
FramePlotHelper,
|
20
|
+
InteractivePlotterWidget,
|
21
|
+
PlotterPyqtgraph,
|
22
|
+
)
|
23
|
+
except (ImportError, OSError):
|
24
|
+
warn("Failed to import OpenGL or PyQt6. If you expect interactive GUI to work, "
|
25
|
+
"please check the package installation.")
|
26
|
+
|
27
|
+
gl = None
|
28
|
+
QtCore = None
|
29
|
+
QtWidgets = None
|
30
|
+
FramePlotHelper = None
|
31
|
+
InteractivePlotterWidget = None
|
32
|
+
PlotterPyqtgraph = None
|
33
|
+
|
23
34
|
|
24
35
|
class MotionDesigner:
|
25
36
|
"""
|
@@ -29,14 +40,16 @@ class MotionDesigner:
|
|
29
40
|
|
30
41
|
:examples:
|
31
42
|
|
32
|
-
|
43
|
+
Run motion designer without initial points or poses:
|
44
|
+
|
45
|
+
.. testcode:: [motiondesigner_ex1]
|
33
46
|
|
34
47
|
from rational_linkages import MotionDesigner
|
35
48
|
|
36
49
|
d = MotionDesigner(method='quadratic_from_poses')
|
37
50
|
d.show()
|
38
51
|
|
39
|
-
.. testoutput:: [
|
52
|
+
.. testoutput:: [motiondesigner_ex1]
|
40
53
|
:hide:
|
41
54
|
|
42
55
|
Closing the window... generated points for interpolation:
|
@@ -44,11 +57,15 @@ class MotionDesigner:
|
|
44
57
|
[ 1. , -0.207522406 , -0.0333866662, -0.0691741237, -0.0625113682, -0.141265791 , -0.4478576802, -0.2637268902]
|
45
58
|
[ 1. , 0.2333739522, -0.0427838517, 0.0777914503, -0.0839342318, 0.2991396249, 0.2980046603, 0.345444421 ]
|
46
59
|
|
47
|
-
.. testcleanup:: [
|
60
|
+
.. testcleanup:: [motiondesigner_ex1]
|
48
61
|
|
49
62
|
del d, MotionDesigner
|
50
63
|
|
51
|
-
|
64
|
+
Run motion designer with initial points:
|
65
|
+
|
66
|
+
.. code-block:: python
|
67
|
+
|
68
|
+
# NOT TESTED
|
52
69
|
|
53
70
|
from rational_linkages import MotionDesigner, PointHomogeneous
|
54
71
|
|
@@ -65,19 +82,6 @@ class MotionDesigner:
|
|
65
82
|
d = MotionDesigner(method='quadratic_from_points', initial_points_or_poses=chosen_points)
|
66
83
|
d.show()
|
67
84
|
|
68
|
-
.. testoutput:: [motiondesigner_example2]
|
69
|
-
:hide:
|
70
|
-
|
71
|
-
Closing the window... generated points for interpolation:
|
72
|
-
[ 1. , -0.2 , 0. , 1.76]
|
73
|
-
[1., 1., 1., 2.]
|
74
|
-
[ 1., 3., -3., 1.]
|
75
|
-
[ 1., 2., -4., 1.]
|
76
|
-
[ 1., -2., -2., 2.]
|
77
|
-
|
78
|
-
.. testcleanup:: [motiondesigner_example2]
|
79
|
-
|
80
|
-
del d, MotionDesigner, PointHomogeneous, chosen_points
|
81
85
|
|
82
86
|
"""
|
83
87
|
def __init__(self,
|
@@ -126,544 +130,548 @@ class MotionDesigner:
|
|
126
130
|
except SystemExit:
|
127
131
|
pass
|
128
132
|
|
129
|
-
|
130
|
-
|
131
|
-
Interactive plotting widget for designing motion curves with interpolated points.
|
132
|
-
|
133
|
-
A widget that displays a 3D view of a motion curve and control points,
|
134
|
-
plus a side panel with controls for selecting and modifying one of the
|
135
|
-
control points (p0 to p6). Moving the sliders adjusts the x, y, and z
|
136
|
-
coordinates of the selected control point, which then updates the curve.
|
137
|
-
"""
|
138
|
-
def __init__(self,
|
139
|
-
method: str = 'cubic_from_points',
|
140
|
-
initial_pts: Union[list[PointHomogeneous], list[DualQuaternion]] = None,
|
141
|
-
parent = None,
|
142
|
-
steps: int = 1000,
|
143
|
-
interval: tuple = (0, 1),
|
144
|
-
arrows_length: float = 1.0,
|
145
|
-
white_background: bool = False):
|
133
|
+
if QtWidgets is not None:
|
134
|
+
class MotionDesignerWidget(QtWidgets.QWidget):
|
146
135
|
"""
|
147
|
-
|
136
|
+
Interactive plotting widget for designing motion curves with interpolated points.
|
137
|
+
|
138
|
+
A widget that displays a 3D view of a motion curve and control points,
|
139
|
+
plus a side panel with controls for selecting and modifying one of the
|
140
|
+
control points (p0 to p6). Moving the sliders adjusts the x, y, and z
|
141
|
+
coordinates of the selected control point, which then updates the curve.
|
148
142
|
"""
|
149
|
-
|
150
|
-
|
151
|
-
|
152
|
-
|
153
|
-
|
154
|
-
|
155
|
-
|
156
|
-
|
157
|
-
|
158
|
-
# an instance of Pyqtgraph-based plotter
|
159
|
-
self.plotter = PlotterPyqtgraph(steps=steps,
|
160
|
-
interval=interval,
|
161
|
-
arrows_length=self.arrows_length,
|
162
|
-
white_background=self.white_background)
|
163
|
-
|
164
|
-
self.mechanism_plotter = []
|
165
|
-
|
166
|
-
if self.white_background:
|
167
|
-
self.render_mode = 'opaque'
|
168
|
-
else:
|
169
|
-
self.render_mode = 'additive'
|
170
|
-
|
171
|
-
self.previous_rpy_sliders_values = []
|
172
|
-
|
173
|
-
# array of control point coordinates (in 3D)
|
174
|
-
if method == 'quadratic_from_points' or method == 'cubic_from_points':
|
175
|
-
self.plotted_points = np.array([pt.normalized_in_3d()
|
176
|
-
for pt in self.points])
|
177
|
-
|
178
|
-
# interpolated points markers
|
179
|
-
self.markers = gl.GLScatterPlotItem(pos=self.plotted_points,
|
180
|
-
color=(1, 0, 1, 1),
|
181
|
-
glOptions=self.render_mode,
|
182
|
-
size=10)
|
183
|
-
self.plotter.widget.addItem(self.markers)
|
184
|
-
|
185
|
-
for i, pt in enumerate(self.plotted_points):
|
186
|
-
self.plotter.widget.add_label(pt, f"p{i}")
|
187
|
-
|
188
|
-
elif method == 'quadratic_from_poses' or method == 'cubic_from_poses':
|
189
|
-
poses_arrays = [TransfMatrix(pt.dq2matrix()) for pt in self.points]
|
190
|
-
self.plotted_poses = [FramePlotHelper(transform=tr,
|
191
|
-
width=10,
|
192
|
-
length=2 * self.arrows_length)
|
193
|
-
for tr in poses_arrays]
|
194
|
-
for i, pose in enumerate(self.plotted_poses):
|
195
|
-
pose.addToView(self.plotter.widget)
|
196
|
-
self.plotter.widget.add_label(pose, f"p{i}")
|
197
|
-
self.previous_rpy_sliders_values.append(pose.tr.rpy() * 100)
|
198
|
-
|
199
|
-
self.curve_path_vis = None # path of motion curve
|
200
|
-
self.curve_frames_vis = None # poses of motion curve
|
201
|
-
self.lambda_val = 0.0
|
202
|
-
self.motion_family_idx = 0
|
203
|
-
self.update_curve_vis() # initial curve update
|
204
|
-
|
205
|
-
###################################
|
206
|
-
# --- build the Control Panel --- #
|
207
|
-
def create_separator():
|
143
|
+
def __init__(self,
|
144
|
+
method: str = 'cubic_from_points',
|
145
|
+
initial_pts: Union[list[PointHomogeneous], list[DualQuaternion]] = None,
|
146
|
+
parent = None,
|
147
|
+
steps: int = 1000,
|
148
|
+
interval: tuple = (0, 1),
|
149
|
+
arrows_length: float = 1.0,
|
150
|
+
white_background: bool = False):
|
208
151
|
"""
|
209
|
-
|
152
|
+
Initialize the motion designer widget.
|
210
153
|
"""
|
211
|
-
|
212
|
-
|
213
|
-
separator.setFrameShadow(QtWidgets.QFrame.Shadow.Sunken)
|
214
|
-
return separator
|
215
|
-
|
216
|
-
# combo box to select one of the points
|
217
|
-
self.point_combo = QtWidgets.QComboBox()
|
218
|
-
for i in range(1, len(self.points)):
|
219
|
-
self.point_combo.addItem(f"Point {i}")
|
220
|
-
self.point_combo.currentIndexChanged.connect(self.on_point_selection_changed)
|
221
|
-
|
222
|
-
# sliders for adjusting x, y, and z
|
223
|
-
self.slider_x = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
224
|
-
self.textbox_x = QtWidgets.QLineEdit()
|
225
|
-
self.textbox_x.editingFinished.connect(
|
226
|
-
lambda: self.on_textbox_changed(self.textbox_x.text(), self.slider_x)
|
227
|
-
)
|
228
|
-
self.slider_y = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
229
|
-
self.textbox_y = QtWidgets.QLineEdit()
|
230
|
-
self.textbox_y.editingFinished.connect(
|
231
|
-
lambda: self.on_textbox_changed(self.textbox_y.text(), self.slider_y)
|
232
|
-
)
|
233
|
-
self.slider_z = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
234
|
-
self.textbox_z = QtWidgets.QLineEdit()
|
235
|
-
self.textbox_z.editingFinished.connect(
|
236
|
-
lambda: self.on_textbox_changed(self.textbox_z.text(), self.slider_z)
|
237
|
-
)
|
238
|
-
# slider range
|
239
|
-
for slider, textbox in [(self.slider_x, self.textbox_x),
|
240
|
-
(self.slider_y, self.textbox_y),
|
241
|
-
(self.slider_z, self.textbox_z)]:
|
242
|
-
slider.setMinimum(-1000)
|
243
|
-
slider.setMaximum(1000)
|
244
|
-
slider.setSingleStep(1)
|
245
|
-
slider.valueChanged.connect(self.on_slider_value_changed)
|
246
|
-
|
247
|
-
if method == 'quadratic_from_poses' or method == 'cubic_from_poses':
|
248
|
-
# sliders for adjusting roll, pitch, and yaw with textboxes
|
249
|
-
self.slider_roll = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
250
|
-
self.textbox_roll = QtWidgets.QLineEdit()
|
251
|
-
self.textbox_roll.editingFinished.connect(
|
252
|
-
lambda: self.on_textbox_changed(self.textbox_roll.text(), self.slider_roll)
|
253
|
-
)
|
254
|
-
self.slider_pitch = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
255
|
-
self.textbox_pitch = QtWidgets.QLineEdit()
|
256
|
-
self.textbox_pitch.editingFinished.connect(
|
257
|
-
lambda: self.on_textbox_changed(self.textbox_pitch.text(), self.slider_pitch)
|
258
|
-
)
|
259
|
-
self.slider_yaw = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
260
|
-
self.textbox_yaw = QtWidgets.QLineEdit()
|
261
|
-
self.textbox_yaw.editingFinished.connect(
|
262
|
-
lambda: self.on_textbox_changed(self.textbox_yaw.text(), self.slider_yaw)
|
263
|
-
)
|
154
|
+
super().__init__(parent)
|
155
|
+
self.setMinimumSize(900, 600)
|
264
156
|
|
265
|
-
self.
|
266
|
-
self.
|
267
|
-
self.
|
157
|
+
self.white_background = white_background
|
158
|
+
self.points = self._initialize_points(method, initial_pts)
|
159
|
+
self.method = method
|
160
|
+
self.arrows_length = arrows_length
|
161
|
+
self.mi = MotionInterpolation()
|
268
162
|
|
269
|
-
#
|
270
|
-
|
271
|
-
|
272
|
-
|
273
|
-
|
274
|
-
|
275
|
-
|
276
|
-
slider.valueChanged.connect(self.on_slider_value_changed)
|
163
|
+
# an instance of Pyqtgraph-based plotter
|
164
|
+
self.plotter = PlotterPyqtgraph(steps=steps,
|
165
|
+
interval=interval,
|
166
|
+
arrows_length=self.arrows_length,
|
167
|
+
white_background=self.white_background)
|
168
|
+
|
169
|
+
self.mechanism_plotter = []
|
277
170
|
|
278
|
-
|
279
|
-
|
280
|
-
|
281
|
-
|
171
|
+
if self.white_background:
|
172
|
+
self.render_mode = 'opaque'
|
173
|
+
else:
|
174
|
+
self.render_mode = 'additive'
|
282
175
|
|
283
|
-
self.
|
284
|
-
self.slider_lambda.setMaximum(int(500))
|
285
|
-
self.slider_lambda.setSingleStep(1)
|
176
|
+
self.previous_rpy_sliders_values = []
|
286
177
|
|
287
|
-
|
288
|
-
|
289
|
-
|
290
|
-
|
178
|
+
# array of control point coordinates (in 3D)
|
179
|
+
if method == 'quadratic_from_points' or method == 'cubic_from_points':
|
180
|
+
self.plotted_points = np.array([pt.normalized_in_3d()
|
181
|
+
for pt in self.points])
|
291
182
|
|
292
|
-
|
293
|
-
|
183
|
+
# interpolated points markers
|
184
|
+
self.markers = gl.GLScatterPlotItem(pos=self.plotted_points,
|
185
|
+
color=(1, 0, 1, 1),
|
186
|
+
glOptions=self.render_mode,
|
187
|
+
size=10)
|
188
|
+
self.plotter.widget.addItem(self.markers)
|
189
|
+
|
190
|
+
for i, pt in enumerate(self.plotted_points):
|
191
|
+
self.plotter.widget.add_label(pt, f"p{i}")
|
192
|
+
|
193
|
+
elif method == 'quadratic_from_poses' or method == 'cubic_from_poses':
|
194
|
+
poses_arrays = [TransfMatrix(pt.dq2matrix()) for pt in self.points]
|
195
|
+
self.plotted_poses = [FramePlotHelper(transform=tr,
|
196
|
+
width=10,
|
197
|
+
length=2 * self.arrows_length)
|
198
|
+
for tr in poses_arrays]
|
199
|
+
for i, pose in enumerate(self.plotted_poses):
|
200
|
+
pose.addToView(self.plotter.widget)
|
201
|
+
self.plotter.widget.add_label(pose, f"p{i}")
|
202
|
+
self.previous_rpy_sliders_values.append(pose.tr.rpy() * 100)
|
203
|
+
|
204
|
+
self.curve_path_vis = None # path of motion curve
|
205
|
+
self.curve_frames_vis = None # poses of motion curve
|
206
|
+
self.lambda_val = 0.0
|
294
207
|
self.motion_family_idx = 0
|
295
|
-
self.
|
296
|
-
|
297
|
-
|
298
|
-
|
299
|
-
|
300
|
-
|
301
|
-
|
302
|
-
|
303
|
-
|
304
|
-
|
305
|
-
|
306
|
-
|
307
|
-
|
308
|
-
|
309
|
-
|
310
|
-
|
311
|
-
|
312
|
-
|
313
|
-
|
314
|
-
|
315
|
-
|
316
|
-
|
317
|
-
|
318
|
-
|
319
|
-
|
320
|
-
|
321
|
-
|
322
|
-
|
323
|
-
|
324
|
-
|
325
|
-
|
326
|
-
|
327
|
-
|
328
|
-
|
329
|
-
|
330
|
-
|
331
|
-
|
332
|
-
|
333
|
-
|
334
|
-
|
335
|
-
|
336
|
-
|
337
|
-
|
338
|
-
cp_layout.addWidget(self.textbox_pitch)
|
339
|
-
cp_layout.addWidget(QtWidgets.QLabel("Rotate Z:"))
|
340
|
-
cp_layout.addWidget(self.slider_yaw)
|
341
|
-
cp_layout.addWidget(self.textbox_yaw)
|
342
|
-
if method == 'cubic_from_poses':
|
343
|
-
cp_layout.addSpacing(10) # Add 10 pixels of space before the separator
|
344
|
-
cp_layout.addWidget(create_separator())
|
345
|
-
cp_layout.addSpacing(10)
|
346
|
-
cp_layout.addWidget(self.swap_family_check_box)
|
347
|
-
cp_layout.addWidget(QtWidgets.QLabel("Lambda:"))
|
348
|
-
cp_layout.addWidget(self.slider_lambda)
|
349
|
-
cp_layout.addWidget(self.textbox_lambda)
|
350
|
-
|
351
|
-
cp_layout.addSpacing(20)
|
352
|
-
cp_layout.addWidget(self.synthesize_button)
|
353
|
-
|
354
|
-
cp_layout.addStretch(1)
|
355
|
-
|
356
|
-
main_layout.addWidget(control_panel)
|
357
|
-
self.setLayout(main_layout)
|
358
|
-
self.setWindowTitle("Motion Designer")
|
359
|
-
|
360
|
-
def _initialize_points(self, method, initial_pts):
|
361
|
-
predefined_points = {
|
362
|
-
'cubic_from_points': [
|
363
|
-
PointHomogeneous(),
|
364
|
-
PointHomogeneous([1, 1, 1, 0.3]),
|
365
|
-
PointHomogeneous([1, 3, -3, 0.5]),
|
366
|
-
PointHomogeneous([1, 0.5, -7, 1]),
|
367
|
-
PointHomogeneous([1, -3.2, -7, 4]),
|
368
|
-
PointHomogeneous([1, -7, -3, 2]),
|
369
|
-
PointHomogeneous([1, -8, 3, 0.5])
|
370
|
-
],
|
371
|
-
'cubic_from_poses': [
|
372
|
-
DualQuaternion(),
|
373
|
-
DualQuaternion([0, 0, 0, 1, 1, 0, 1, 0]),
|
374
|
-
DualQuaternion([1, 2, 0, 0, -2, 1, 0, 0]),
|
375
|
-
DualQuaternion([3, 0, 1, 0, 1, 0, -3, 0])
|
376
|
-
],
|
377
|
-
'quadratic_from_points': [
|
378
|
-
PointHomogeneous(),
|
379
|
-
PointHomogeneous([1, 1, 1, 2]),
|
380
|
-
PointHomogeneous([1, 3, -3, 1]),
|
381
|
-
PointHomogeneous([1, 2, -4, 1]),
|
382
|
-
PointHomogeneous([1, -2, -2, 2])
|
383
|
-
],
|
384
|
-
'quadratic_from_poses': [
|
385
|
-
DualQuaternion(),
|
386
|
-
DualQuaternion(
|
387
|
-
TransfMatrix.from_vectors(
|
388
|
-
approach_z=[-0.0362862, 0.400074, 0.915764],
|
389
|
-
normal_x=[0.988751, -0.118680, 0.0910266],
|
390
|
-
origin=[0.33635718, 0.9436004, 0.3428654]).matrix2dq()),
|
391
|
-
DualQuaternion(
|
392
|
-
TransfMatrix.from_vectors(
|
393
|
-
approach_z=[-0.0463679, -0.445622, 0.894020],
|
394
|
-
normal_x=[0.985161, 0.127655, 0.114724],
|
395
|
-
origin=[-0.52857769, -0.4463076, -0.81766]).matrix2dq()),
|
396
|
-
]
|
397
|
-
}
|
398
|
-
|
399
|
-
required_points = {
|
400
|
-
'cubic_from_points': 7,
|
401
|
-
'cubic_from_poses': 4,
|
402
|
-
'quadratic_from_points': 5,
|
403
|
-
'quadratic_from_poses': 3
|
404
|
-
}
|
405
|
-
|
406
|
-
if method not in predefined_points:
|
407
|
-
raise ValueError(f"Unknown method: {method}")
|
408
|
-
|
409
|
-
if initial_pts is None:
|
410
|
-
return predefined_points[method]
|
411
|
-
|
412
|
-
if len(initial_pts) != required_points[method]:
|
413
|
-
raise ValueError(
|
414
|
-
f"For a {method.replace('_', ' ')}, {required_points[method]} points are needed.")
|
415
|
-
|
416
|
-
return initial_pts
|
417
|
-
|
418
|
-
def set_sliders_for_point(self, index):
|
419
|
-
"""
|
420
|
-
Set the slider positions to reflect the current coordinates of the
|
421
|
-
control point with the given index.
|
422
|
-
(Here we assume that coordinates are in the range roughly –10..10.)
|
423
|
-
"""
|
424
|
-
index = index + 1 # skip the first point/pose
|
425
|
-
sliders = [self.slider_x, self.slider_y, self.slider_z]
|
426
|
-
text_boxes = [self.textbox_x, self.textbox_y, self.textbox_z]
|
427
|
-
if self.method == 'quadratic_from_points' or self.method == 'cubic_from_points':
|
428
|
-
pt = self.plotted_points[index]
|
429
|
-
values = [int(pt[i] * 100) for i in range(3)]
|
430
|
-
else:
|
431
|
-
sliders.extend([self.slider_roll, self.slider_pitch, self.slider_yaw])
|
432
|
-
text_boxes.extend([self.textbox_roll, self.textbox_pitch, self.textbox_yaw])
|
433
|
-
pt = self.plotted_poses[index]
|
434
|
-
rpy = self.previous_rpy_sliders_values[index]
|
435
|
-
values = [
|
436
|
-
int(pt.tr.t[0] * 100),
|
437
|
-
int(pt.tr.t[1] * 100),
|
438
|
-
int(pt.tr.t[2] * 100),
|
439
|
-
int(rpy[0]), # Roll
|
440
|
-
int(rpy[1]), # Pitch
|
441
|
-
int(rpy[2]) # Yaw
|
442
|
-
]
|
443
|
-
(self.slider_roll_prev, self.slider_pitch_prev,
|
444
|
-
self.slider_yaw_prev) = values[3:]
|
445
|
-
#
|
446
|
-
for slider, text_box, value in zip(sliders, text_boxes, values):
|
447
|
-
slider.blockSignals(True)
|
448
|
-
slider.setValue(value)
|
449
|
-
text_box.setText(str(value / 100.0))
|
450
|
-
slider.blockSignals(False)
|
451
|
-
|
452
|
-
def on_synthesize_button_clicked(self):
|
453
|
-
"""
|
454
|
-
Called when the "Synthesize mechanism" button is clicked. This method
|
455
|
-
should be implemented to synthesize a mechanism based on the current
|
456
|
-
control points.
|
457
|
-
"""
|
458
|
-
if (self.method == 'quadratic_from_points'
|
459
|
-
or self.method == 'cubic_from_points'
|
460
|
-
or self.method == 'quadratic_from_poses'):
|
461
|
-
c = MotionInterpolation.interpolate(self.points)
|
462
|
-
else:
|
463
|
-
p = MotionInterpolation.interpolate_cubic_numerically(
|
464
|
-
self.points,
|
465
|
-
lambda_val=self.lambda_val,
|
466
|
-
k_idx=self.motion_family_idx)
|
467
|
-
c = RationalCurve.from_coeffs(p)
|
468
|
-
self.mechanism_plotter.append(
|
469
|
-
InteractivePlotterWidget(mechanism=RationalMechanism(c.factorize()),
|
470
|
-
arrows_length=self.arrows_length,
|
471
|
-
parent_app=self.plotter.app))
|
472
|
-
self.mechanism_plotter[-1].show()
|
473
|
-
|
474
|
-
|
475
|
-
def on_point_selection_changed(self, index):
|
476
|
-
"""
|
477
|
-
When a different point is selected in the combo box, update the slider
|
478
|
-
positions to match that point’s coordinates.
|
479
|
-
"""
|
480
|
-
self.set_sliders_for_point(index)
|
208
|
+
self.update_curve_vis() # initial curve update
|
209
|
+
|
210
|
+
###################################
|
211
|
+
# --- build the Control Panel --- #
|
212
|
+
def create_separator():
|
213
|
+
"""
|
214
|
+
Create a horizontal line separator (QFrame).
|
215
|
+
"""
|
216
|
+
separator = QtWidgets.QFrame()
|
217
|
+
separator.setFrameShape(QtWidgets.QFrame.Shape.HLine)
|
218
|
+
separator.setFrameShadow(QtWidgets.QFrame.Shadow.Sunken)
|
219
|
+
return separator
|
220
|
+
|
221
|
+
# combo box to select one of the points
|
222
|
+
self.point_combo = QtWidgets.QComboBox()
|
223
|
+
for i in range(1, len(self.points)):
|
224
|
+
self.point_combo.addItem(f"Point {i}")
|
225
|
+
self.point_combo.currentIndexChanged.connect(self.on_point_selection_changed)
|
226
|
+
|
227
|
+
# sliders for adjusting x, y, and z
|
228
|
+
self.slider_x = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
229
|
+
self.textbox_x = QtWidgets.QLineEdit()
|
230
|
+
self.textbox_x.editingFinished.connect(
|
231
|
+
lambda: self.on_textbox_changed(self.textbox_x.text(), self.slider_x)
|
232
|
+
)
|
233
|
+
self.slider_y = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
234
|
+
self.textbox_y = QtWidgets.QLineEdit()
|
235
|
+
self.textbox_y.editingFinished.connect(
|
236
|
+
lambda: self.on_textbox_changed(self.textbox_y.text(), self.slider_y)
|
237
|
+
)
|
238
|
+
self.slider_z = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
239
|
+
self.textbox_z = QtWidgets.QLineEdit()
|
240
|
+
self.textbox_z.editingFinished.connect(
|
241
|
+
lambda: self.on_textbox_changed(self.textbox_z.text(), self.slider_z)
|
242
|
+
)
|
243
|
+
# slider range
|
244
|
+
for slider, textbox in [(self.slider_x, self.textbox_x),
|
245
|
+
(self.slider_y, self.textbox_y),
|
246
|
+
(self.slider_z, self.textbox_z)]:
|
247
|
+
slider.setMinimum(-1000)
|
248
|
+
slider.setMaximum(1000)
|
249
|
+
slider.setSingleStep(1)
|
250
|
+
slider.valueChanged.connect(self.on_slider_value_changed)
|
481
251
|
|
482
|
-
|
483
|
-
|
484
|
-
|
485
|
-
|
486
|
-
|
487
|
-
|
488
|
-
|
489
|
-
|
490
|
-
|
491
|
-
|
492
|
-
|
493
|
-
|
494
|
-
|
495
|
-
|
496
|
-
|
497
|
-
|
498
|
-
|
499
|
-
|
500
|
-
|
501
|
-
|
502
|
-
|
503
|
-
|
504
|
-
|
505
|
-
|
506
|
-
|
507
|
-
|
508
|
-
|
509
|
-
|
510
|
-
|
511
|
-
|
512
|
-
|
513
|
-
|
514
|
-
|
515
|
-
|
516
|
-
|
517
|
-
|
518
|
-
self.
|
252
|
+
if method == 'quadratic_from_poses' or method == 'cubic_from_poses':
|
253
|
+
# sliders for adjusting roll, pitch, and yaw with textboxes
|
254
|
+
self.slider_roll = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
255
|
+
self.textbox_roll = QtWidgets.QLineEdit()
|
256
|
+
self.textbox_roll.editingFinished.connect(
|
257
|
+
lambda: self.on_textbox_changed(self.textbox_roll.text(), self.slider_roll)
|
258
|
+
)
|
259
|
+
self.slider_pitch = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
260
|
+
self.textbox_pitch = QtWidgets.QLineEdit()
|
261
|
+
self.textbox_pitch.editingFinished.connect(
|
262
|
+
lambda: self.on_textbox_changed(self.textbox_pitch.text(), self.slider_pitch)
|
263
|
+
)
|
264
|
+
self.slider_yaw = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
265
|
+
self.textbox_yaw = QtWidgets.QLineEdit()
|
266
|
+
self.textbox_yaw.editingFinished.connect(
|
267
|
+
lambda: self.on_textbox_changed(self.textbox_yaw.text(), self.slider_yaw)
|
268
|
+
)
|
269
|
+
|
270
|
+
self.slider_roll_prev = 0
|
271
|
+
self.slider_pitch_prev = 0
|
272
|
+
self.slider_yaw_prev = 0
|
273
|
+
|
274
|
+
# slider range
|
275
|
+
for slider, textbox in [(self.slider_roll, self.textbox_roll),
|
276
|
+
(self.slider_pitch, self.textbox_pitch),
|
277
|
+
(self.slider_yaw, self.textbox_yaw)]:
|
278
|
+
slider.setMinimum(int(-np.pi * 100))
|
279
|
+
slider.setMaximum(int(np.pi * 100))
|
280
|
+
slider.setSingleStep(1)
|
281
|
+
slider.valueChanged.connect(self.on_slider_value_changed)
|
282
|
+
|
283
|
+
# slider for lambda of cubic curve with textbox
|
284
|
+
if method == 'cubic_from_poses':
|
285
|
+
self.slider_lambda = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
|
286
|
+
self.textbox_lambda = QtWidgets.QLineEdit()
|
287
|
+
|
288
|
+
self.slider_lambda.setMinimum(int(-500))
|
289
|
+
self.slider_lambda.setMaximum(int(500))
|
290
|
+
self.slider_lambda.setSingleStep(1)
|
291
|
+
|
292
|
+
self.slider_lambda.valueChanged.connect(self.on_lambda_slider_value_changed)
|
293
|
+
self.textbox_lambda.editingFinished.connect(
|
294
|
+
lambda: self.on_lambda_textbox_changed(self.textbox_lambda.text(),
|
295
|
+
self.slider_lambda))
|
296
|
+
|
297
|
+
# add button for swapping family
|
298
|
+
self.swap_family_check_box = QtWidgets.QCheckBox(text="Swap family")
|
299
|
+
self.motion_family_idx = 0
|
300
|
+
self.swap_family_check_box.stateChanged.connect(self.on_swap_family_check_box_changed)
|
519
301
|
else:
|
520
|
-
|
521
|
-
|
302
|
+
self.slider_lambda = None
|
303
|
+
self.swap_family_check_box = None
|
304
|
+
self.textbox_lambda = None
|
522
305
|
|
523
|
-
|
524
|
-
self.
|
525
|
-
self.
|
306
|
+
# add button for mechanism synthesis
|
307
|
+
self.synthesize_button = QtWidgets.QPushButton("Mechanism")
|
308
|
+
self.synthesize_button.clicked.connect(self.on_synthesize_button_clicked)
|
526
309
|
|
527
|
-
|
528
|
-
self.
|
529
|
-
self.plotted_poses[index].setData(new_tr)
|
310
|
+
# initially for the first point
|
311
|
+
self.set_sliders_for_point(0)
|
530
312
|
|
531
|
-
|
532
|
-
|
533
|
-
|
534
|
-
self.
|
535
|
-
# update the visual markers
|
536
|
-
self.markers.setData(pos=self.plotted_points)
|
313
|
+
# --- layout the 3D view and control panel ---
|
314
|
+
main_layout = QtWidgets.QHBoxLayout(self)
|
315
|
+
# add plotter (stored in self.plotter.widget)
|
316
|
+
main_layout.addWidget(self.plotter.widget, stretch=1)
|
537
317
|
|
538
|
-
|
539
|
-
|
318
|
+
# Build a vertical control panel.
|
319
|
+
control_panel = QtWidgets.QWidget()
|
320
|
+
cp_layout = QtWidgets.QVBoxLayout(control_panel)
|
540
321
|
|
541
|
-
|
542
|
-
|
543
|
-
|
544
|
-
of the cubic curve, update the control point markers, and then recalculate
|
545
|
-
the motion curve.
|
546
|
-
"""
|
547
|
-
self.lambda_val = self.slider_lambda.value() / 100.0
|
548
|
-
self.textbox_lambda.setText(str(self.lambda_val))
|
549
|
-
self.update_curve_vis()
|
322
|
+
cp_layout.addWidget(QtWidgets.QLabel("Select control point:"))
|
323
|
+
cp_layout.addWidget(self.point_combo)
|
324
|
+
cp_layout.addSpacing(10)
|
550
325
|
|
551
|
-
|
552
|
-
|
553
|
-
|
554
|
-
|
555
|
-
|
556
|
-
|
557
|
-
|
558
|
-
|
559
|
-
self.
|
326
|
+
cp_layout.addWidget(QtWidgets.QLabel("Adjust X:"))
|
327
|
+
cp_layout.addWidget(self.slider_x)
|
328
|
+
cp_layout.addWidget(self.textbox_x)
|
329
|
+
cp_layout.addWidget(QtWidgets.QLabel("Adjust Y:"))
|
330
|
+
cp_layout.addWidget(self.slider_y)
|
331
|
+
cp_layout.addWidget(self.textbox_y)
|
332
|
+
cp_layout.addWidget(QtWidgets.QLabel("Adjust Z:"))
|
333
|
+
cp_layout.addWidget(self.slider_z)
|
334
|
+
cp_layout.addWidget(self.textbox_z)
|
335
|
+
if method == 'quadratic_from_poses' or method == 'cubic_from_poses':
|
336
|
+
cp_layout.addSpacing(10) # Add 10 pixels of space before the separator
|
337
|
+
cp_layout.addWidget(create_separator())
|
338
|
+
cp_layout.addWidget(QtWidgets.QLabel("Rotate X:"))
|
339
|
+
cp_layout.addWidget(self.slider_roll)
|
340
|
+
cp_layout.addWidget(self.textbox_roll)
|
341
|
+
cp_layout.addWidget(QtWidgets.QLabel("Rotate Y:"))
|
342
|
+
cp_layout.addWidget(self.slider_pitch)
|
343
|
+
cp_layout.addWidget(self.textbox_pitch)
|
344
|
+
cp_layout.addWidget(QtWidgets.QLabel("Rotate Z:"))
|
345
|
+
cp_layout.addWidget(self.slider_yaw)
|
346
|
+
cp_layout.addWidget(self.textbox_yaw)
|
347
|
+
if method == 'cubic_from_poses':
|
348
|
+
cp_layout.addSpacing(10) # Add 10 pixels of space before the separator
|
349
|
+
cp_layout.addWidget(create_separator())
|
350
|
+
cp_layout.addSpacing(10)
|
351
|
+
cp_layout.addWidget(self.swap_family_check_box)
|
352
|
+
cp_layout.addWidget(QtWidgets.QLabel("Lambda:"))
|
353
|
+
cp_layout.addWidget(self.slider_lambda)
|
354
|
+
cp_layout.addWidget(self.textbox_lambda)
|
355
|
+
|
356
|
+
cp_layout.addSpacing(20)
|
357
|
+
cp_layout.addWidget(self.synthesize_button)
|
358
|
+
|
359
|
+
cp_layout.addStretch(1)
|
360
|
+
|
361
|
+
main_layout.addWidget(control_panel)
|
362
|
+
self.setLayout(main_layout)
|
363
|
+
self.setWindowTitle("Motion Designer")
|
364
|
+
|
365
|
+
def _initialize_points(self, method, initial_pts):
|
366
|
+
predefined_points = {
|
367
|
+
'cubic_from_points': [
|
368
|
+
PointHomogeneous(),
|
369
|
+
PointHomogeneous([1, 1, 1, 0.3]),
|
370
|
+
PointHomogeneous([1, 3, -3, 0.5]),
|
371
|
+
PointHomogeneous([1, 0.5, -7, 1]),
|
372
|
+
PointHomogeneous([1, -3.2, -7, 4]),
|
373
|
+
PointHomogeneous([1, -7, -3, 2]),
|
374
|
+
PointHomogeneous([1, -8, 3, 0.5])
|
375
|
+
],
|
376
|
+
'cubic_from_poses': [
|
377
|
+
DualQuaternion(),
|
378
|
+
DualQuaternion([0, 0, 0, 1, 1, 0, 1, 0]),
|
379
|
+
DualQuaternion([1, 2, 0, 0, -2, 1, 0, 0]),
|
380
|
+
DualQuaternion([3, 0, 1, 0, 1, 0, -3, 0])
|
381
|
+
],
|
382
|
+
'quadratic_from_points': [
|
383
|
+
PointHomogeneous(),
|
384
|
+
PointHomogeneous([1, 1, 1, 2]),
|
385
|
+
PointHomogeneous([1, 3, -3, 1]),
|
386
|
+
PointHomogeneous([1, 2, -4, 1]),
|
387
|
+
PointHomogeneous([1, -2, -2, 2])
|
388
|
+
],
|
389
|
+
'quadratic_from_poses': [
|
390
|
+
DualQuaternion(),
|
391
|
+
DualQuaternion(
|
392
|
+
TransfMatrix.from_vectors(
|
393
|
+
approach_z=[-0.0362862, 0.400074, 0.915764],
|
394
|
+
normal_x=[0.988751, -0.118680, 0.0910266],
|
395
|
+
origin=[0.33635718, 0.9436004, 0.3428654]).matrix2dq()),
|
396
|
+
DualQuaternion(
|
397
|
+
TransfMatrix.from_vectors(
|
398
|
+
approach_z=[-0.0463679, -0.445622, 0.894020],
|
399
|
+
normal_x=[0.985161, 0.127655, 0.114724],
|
400
|
+
origin=[-0.52857769, -0.4463076, -0.81766]).matrix2dq()),
|
401
|
+
]
|
402
|
+
}
|
403
|
+
|
404
|
+
required_points = {
|
405
|
+
'cubic_from_points': 7,
|
406
|
+
'cubic_from_poses': 4,
|
407
|
+
'quadratic_from_points': 5,
|
408
|
+
'quadratic_from_poses': 3
|
409
|
+
}
|
410
|
+
|
411
|
+
if method not in predefined_points:
|
412
|
+
raise ValueError(f"Unknown method: {method}")
|
413
|
+
|
414
|
+
if initial_pts is None:
|
415
|
+
return predefined_points[method]
|
416
|
+
|
417
|
+
if len(initial_pts) != required_points[method]:
|
418
|
+
raise ValueError(
|
419
|
+
f"For a {method.replace('_', ' ')}, {required_points[method]} points are needed.")
|
420
|
+
|
421
|
+
return initial_pts
|
422
|
+
|
423
|
+
def set_sliders_for_point(self, index):
|
424
|
+
"""
|
425
|
+
Set the slider positions to reflect the current coordinates of the
|
426
|
+
control point with the given index.
|
427
|
+
(Here we assume that coordinates are in the range roughly –10..10.)
|
428
|
+
"""
|
429
|
+
index = index + 1 # skip the first point/pose
|
430
|
+
sliders = [self.slider_x, self.slider_y, self.slider_z]
|
431
|
+
text_boxes = [self.textbox_x, self.textbox_y, self.textbox_z]
|
432
|
+
if self.method == 'quadratic_from_points' or self.method == 'cubic_from_points':
|
433
|
+
pt = self.plotted_points[index]
|
434
|
+
values = [int(pt[i] * 100) for i in range(3)]
|
435
|
+
else:
|
436
|
+
sliders.extend([self.slider_roll, self.slider_pitch, self.slider_yaw])
|
437
|
+
text_boxes.extend([self.textbox_roll, self.textbox_pitch, self.textbox_yaw])
|
438
|
+
pt = self.plotted_poses[index]
|
439
|
+
rpy = self.previous_rpy_sliders_values[index]
|
440
|
+
values = [
|
441
|
+
int(pt.tr.t[0] * 100),
|
442
|
+
int(pt.tr.t[1] * 100),
|
443
|
+
int(pt.tr.t[2] * 100),
|
444
|
+
int(rpy[0]), # Roll
|
445
|
+
int(rpy[1]), # Pitch
|
446
|
+
int(rpy[2]) # Yaw
|
447
|
+
]
|
448
|
+
(self.slider_roll_prev, self.slider_pitch_prev,
|
449
|
+
self.slider_yaw_prev) = values[3:]
|
450
|
+
#
|
451
|
+
for slider, text_box, value in zip(sliders, text_boxes, values):
|
452
|
+
slider.blockSignals(True)
|
453
|
+
slider.setValue(value)
|
454
|
+
text_box.setText(str(value / 100.0))
|
455
|
+
slider.blockSignals(False)
|
560
456
|
|
561
|
-
self
|
457
|
+
def on_synthesize_button_clicked(self):
|
458
|
+
"""
|
459
|
+
Called when the "Synthesize mechanism" button is clicked. This method
|
460
|
+
should be implemented to synthesize a mechanism based on the current
|
461
|
+
control points.
|
462
|
+
"""
|
463
|
+
if (self.method == 'quadratic_from_points'
|
464
|
+
or self.method == 'cubic_from_points'
|
465
|
+
or self.method == 'quadratic_from_poses'):
|
466
|
+
c = MotionInterpolation.interpolate(self.points)
|
467
|
+
else:
|
468
|
+
p = MotionInterpolation.interpolate_cubic_numerically(
|
469
|
+
self.points,
|
470
|
+
lambda_val=self.lambda_val,
|
471
|
+
k_idx=self.motion_family_idx)
|
472
|
+
c = RationalCurve.from_coeffs(p)
|
473
|
+
self.mechanism_plotter.append(
|
474
|
+
InteractivePlotterWidget(mechanism=RationalMechanism(c.factorize()),
|
475
|
+
arrows_length=self.arrows_length,
|
476
|
+
parent_app=self.plotter.app))
|
477
|
+
self.mechanism_plotter[-1].show()
|
478
|
+
|
479
|
+
|
480
|
+
def on_point_selection_changed(self, index):
|
481
|
+
"""
|
482
|
+
When a different point is selected in the combo box, update the slider
|
483
|
+
positions to match that point’s coordinates.
|
484
|
+
"""
|
485
|
+
self.set_sliders_for_point(index)
|
562
486
|
|
563
|
-
|
564
|
-
|
565
|
-
|
487
|
+
def on_slider_value_changed(self, value):
|
488
|
+
"""
|
489
|
+
Called when any of the sliders change their value. Update the currently
|
490
|
+
selected control point’s x, y, or z coordinate based on the slider values,
|
491
|
+
update the control point markers, and then recalculate the motion curve.
|
492
|
+
"""
|
493
|
+
index = self.point_combo.currentIndex() + 1
|
494
|
+
# Convert slider values (integers) to floating‑point coordinates.
|
495
|
+
new_x = self.slider_x.value() / 100.0
|
496
|
+
new_y = self.slider_y.value() / 100.0
|
497
|
+
new_z = self.slider_z.value() / 100.0
|
498
|
+
|
499
|
+
self.textbox_x.setText(str(new_x))
|
500
|
+
self.textbox_y.setText(str(new_y))
|
501
|
+
self.textbox_z.setText(str(new_z))
|
502
|
+
|
503
|
+
if self.method == 'quadratic_from_poses' or self.method == 'cubic_from_poses':
|
504
|
+
if self.slider_roll.value() != self.slider_roll_prev:
|
505
|
+
new_roll = (self.slider_roll.value() - self.slider_roll_prev) / 100.0
|
506
|
+
new_mat = TransfMatrix.from_rotation('x', new_roll)
|
507
|
+
new_tr = self.plotted_poses[index].tr * new_mat
|
508
|
+
self.slider_roll_prev = self.slider_roll.value()
|
509
|
+
self.textbox_roll.setText(str(self.slider_roll.value() / 100.0))
|
510
|
+
|
511
|
+
elif self.slider_pitch.value() != self.slider_pitch_prev:
|
512
|
+
new_pitch = (self.slider_pitch.value() - self.slider_pitch_prev) / 100.0
|
513
|
+
new_mat = TransfMatrix.from_rotation('y', new_pitch)
|
514
|
+
new_tr = self.plotted_poses[index].tr * new_mat
|
515
|
+
self.slider_pitch_prev = self.slider_pitch.value()
|
516
|
+
self.textbox_pitch.setText(str(self.slider_pitch.value() / 100.0))
|
517
|
+
|
518
|
+
elif self.slider_yaw.value() != self.slider_yaw_prev:
|
519
|
+
new_yaw = (self.slider_yaw.value() - self.slider_yaw_prev) / 100.0
|
520
|
+
new_mat = TransfMatrix.from_rotation('z', new_yaw)
|
521
|
+
new_tr = self.plotted_poses[index].tr * new_mat
|
522
|
+
self.slider_yaw_prev = self.slider_yaw.value()
|
523
|
+
self.textbox_yaw.setText(str(self.slider_yaw.value() / 100.0))
|
524
|
+
else:
|
525
|
+
new_tr = TransfMatrix.from_rpy_xyz(self.plotted_poses[index].tr.rpy(),
|
526
|
+
[new_x, new_y, new_z])
|
527
|
+
|
528
|
+
self.previous_rpy_sliders_values[index][0] = self.slider_roll.value()
|
529
|
+
self.previous_rpy_sliders_values[index][1] = self.slider_pitch.value()
|
530
|
+
self.previous_rpy_sliders_values[index][2] = self.slider_yaw.value()
|
531
|
+
|
532
|
+
new_dq = DualQuaternion(new_tr.matrix2dq())
|
533
|
+
self.points[index] = new_dq
|
534
|
+
self.plotted_poses[index].setData(new_tr)
|
566
535
|
|
567
|
-
|
568
|
-
|
569
|
-
|
570
|
-
|
571
|
-
|
572
|
-
|
573
|
-
slider.blockSignals(True)
|
574
|
-
slider.setValue(int(value * 100))
|
575
|
-
slider.blockSignals(False)
|
536
|
+
else:
|
537
|
+
# update the selected control point
|
538
|
+
self.points[index] = PointHomogeneous.from_3d_point([new_x, new_y, new_z])
|
539
|
+
self.plotted_points[index] = np.array([new_x, new_y, new_z])
|
540
|
+
# update the visual markers
|
541
|
+
self.markers.setData(pos=self.plotted_points)
|
576
542
|
|
577
|
-
|
578
|
-
|
579
|
-
print("Warning: lambda value set to 1.0, using 1.00000001 instead.")
|
580
|
-
self.lambda_val = value
|
581
|
-
self.update_curve_vis()
|
582
|
-
except ValueError:
|
583
|
-
raise ValueError(f"Invalid input for slider: {text}")
|
543
|
+
# Recalculate and update the motion curve.
|
544
|
+
self.update_curve_vis()
|
584
545
|
|
585
|
-
|
586
|
-
|
587
|
-
|
588
|
-
|
589
|
-
|
590
|
-
|
591
|
-
|
592
|
-
|
593
|
-
|
594
|
-
slider.blockSignals(False)
|
546
|
+
def on_lambda_slider_value_changed(self, value):
|
547
|
+
"""
|
548
|
+
Called when the lambda slider changes its value. Update the lambda value
|
549
|
+
of the cubic curve, update the control point markers, and then recalculate
|
550
|
+
the motion curve.
|
551
|
+
"""
|
552
|
+
self.lambda_val = self.slider_lambda.value() / 100.0
|
553
|
+
self.textbox_lambda.setText(str(self.lambda_val))
|
554
|
+
self.update_curve_vis()
|
595
555
|
|
596
|
-
|
597
|
-
|
598
|
-
|
556
|
+
def on_swap_family_check_box_changed(self, state):
|
557
|
+
"""
|
558
|
+
Called when the swap family checkbox changes its state. Update the
|
559
|
+
motion curve to reflect the new motion family.
|
560
|
+
"""
|
561
|
+
if state == 2:
|
562
|
+
self.motion_family_idx = 1
|
563
|
+
else:
|
564
|
+
self.motion_family_idx = 0
|
599
565
|
|
600
|
-
|
601
|
-
|
602
|
-
|
603
|
-
|
604
|
-
|
605
|
-
"""
|
566
|
+
self.update_curve_vis()
|
567
|
+
|
568
|
+
def on_lambda_textbox_changed(self, text, slider):
|
569
|
+
"""
|
570
|
+
Update the given slider with the value from the corresponding textbox.
|
606
571
|
|
607
|
-
|
608
|
-
|
609
|
-
|
610
|
-
|
611
|
-
|
612
|
-
|
572
|
+
:param str text: The text input from the textbox. Should be a number.
|
573
|
+
:param slider: The slider to update with the new value.
|
574
|
+
"""
|
575
|
+
if text is not None:
|
576
|
+
try:
|
577
|
+
value = float(text)
|
578
|
+
slider.blockSignals(True)
|
579
|
+
slider.setValue(int(value * 100))
|
580
|
+
slider.blockSignals(False)
|
581
|
+
|
582
|
+
if abs(value - 1.0) < 1e-10:
|
583
|
+
value = 1.00000001 # avoid numerical issues with 1.0
|
584
|
+
print("Warning: lambda value set to 1.0, using 1.00000001 instead.")
|
585
|
+
self.lambda_val = value
|
586
|
+
self.update_curve_vis()
|
587
|
+
except ValueError:
|
588
|
+
raise ValueError(f"Invalid input for slider: {text}")
|
589
|
+
|
590
|
+
def on_textbox_changed(self, text, slider):
|
591
|
+
"""
|
592
|
+
Update the given slider with the value from the corresponding textbox.
|
593
|
+
"""
|
594
|
+
if text is not None:
|
595
|
+
try:
|
596
|
+
value = float(text)
|
597
|
+
slider.blockSignals(True)
|
598
|
+
slider.setValue(int(value * 100))
|
599
|
+
slider.blockSignals(False)
|
600
|
+
|
601
|
+
self.on_slider_value_changed(value)
|
602
|
+
except ValueError:
|
603
|
+
raise ValueError(f"Invalid input for slider: {text}")
|
604
|
+
|
605
|
+
def update_curve_vis(self):
|
606
|
+
"""
|
607
|
+
Recalculate the motion curve using the current control points. The
|
608
|
+
interpolation is performed by MotionInterpolation. Then update the curve
|
609
|
+
line in the GLViewWidget.
|
610
|
+
"""
|
611
|
+
|
612
|
+
# get the numeric coefficients from interpolation
|
613
|
+
if self.method == 'cubic_from_points':
|
614
|
+
coeffs = self.mi.interpolate_points_cubic(self.points,
|
613
615
|
return_numeric=True)
|
614
|
-
|
615
|
-
|
616
|
-
|
617
|
-
|
618
|
-
|
619
|
-
|
620
|
-
|
621
|
-
|
622
|
-
|
623
|
-
|
624
|
-
|
625
|
-
|
626
|
-
|
627
|
-
|
628
|
-
|
629
|
-
|
630
|
-
|
631
|
-
|
632
|
-
|
633
|
-
|
634
|
-
|
635
|
-
|
636
|
-
|
637
|
-
curve_frames
|
638
|
-
|
639
|
-
|
640
|
-
|
641
|
-
|
642
|
-
|
643
|
-
|
644
|
-
|
645
|
-
|
646
|
-
|
647
|
-
|
648
|
-
|
649
|
-
|
650
|
-
|
651
|
-
|
652
|
-
|
653
|
-
|
654
|
-
|
655
|
-
|
656
|
-
|
657
|
-
|
658
|
-
|
659
|
-
|
660
|
-
|
661
|
-
|
662
|
-
|
663
|
-
|
664
|
-
|
665
|
-
|
666
|
-
|
667
|
-
|
668
|
-
|
669
|
-
|
616
|
+
elif self.method == 'quadratic_from_points':
|
617
|
+
coeffs = self.mi.interpolate_points_quadratic(self.points,
|
618
|
+
return_numeric=True)
|
619
|
+
elif self.method == 'quadratic_from_poses':
|
620
|
+
coeffs = self.mi.interpolate_quadratic_numerically(self.points)
|
621
|
+
elif self.method == 'cubic_from_poses':
|
622
|
+
coeffs = self.mi.interpolate_cubic_numerically(self.points,
|
623
|
+
lambda_val=self.lambda_val,
|
624
|
+
k_idx=self.motion_family_idx)
|
625
|
+
|
626
|
+
# create numpy polynomial objects
|
627
|
+
curve = [np.polynomial.Polynomial(c[::-1]) for c in coeffs]
|
628
|
+
|
629
|
+
# parameter values using a tangent substitution
|
630
|
+
t_space = np.tan(np.linspace(-np.pi / 2, np.pi / 2, self.plotter.steps + 1))
|
631
|
+
curve_points = []
|
632
|
+
for t in t_space:
|
633
|
+
dq = DualQuaternion([poly(t) for poly in curve]) # evaluate fot each t
|
634
|
+
pt = dq.dq2point_via_matrix()
|
635
|
+
curve_points.append(pt)
|
636
|
+
curve_points = np.array(curve_points)
|
637
|
+
|
638
|
+
t_space_frames = np.tan(np.linspace(-np.pi / 2, np.pi / 2, 51))
|
639
|
+
curve_frames = []
|
640
|
+
for t in t_space_frames:
|
641
|
+
dq = DualQuaternion([poly(t) for poly in curve])
|
642
|
+
curve_frames.append(TransfMatrix(dq.dq2matrix()))
|
643
|
+
|
644
|
+
# if the curve line has not yet been created
|
645
|
+
if self.curve_path_vis is None:
|
646
|
+
self.curve_path_vis = gl.GLLinePlotItem(pos=curve_points,
|
647
|
+
color=(0.5, 0.5, 0.5, 1),
|
648
|
+
glOptions=self.render_mode,
|
649
|
+
width=2,
|
650
|
+
antialias=True)
|
651
|
+
self.plotter.widget.addItem(self.curve_path_vis)
|
652
|
+
|
653
|
+
self.curve_frames_vis = [FramePlotHelper(transform=tr,
|
654
|
+
length=self.plotter.arrows_length)
|
655
|
+
for tr in curve_frames]
|
656
|
+
for frame in self.curve_frames_vis:
|
657
|
+
frame.addToView(self.plotter.widget)
|
658
|
+
else: # update the existing curve visuals
|
659
|
+
self.curve_path_vis.setData(pos=curve_points)
|
660
|
+
for i, frame in enumerate(self.curve_frames_vis):
|
661
|
+
frame.setData(curve_frames[i])
|
662
|
+
|
663
|
+
def closeEvent(self, event):
|
664
|
+
"""
|
665
|
+
Called when the window is closed. Ensure that the Qt application exits.
|
666
|
+
"""
|
667
|
+
print("Closing the window... generated points for interpolation:")
|
668
|
+
for pt in self.points:
|
669
|
+
print(pt)
|
670
|
+
if self.slider_lambda:
|
671
|
+
print(f"Lambda: {self.slider_lambda.value() / 100.0}")
|
672
|
+
if self.swap_family_check_box:
|
673
|
+
print(f"Motion family index: {self.motion_family_idx}")
|
674
|
+
self.plotter.app.quit()
|
675
|
+
|
676
|
+
else:
|
677
|
+
MotionDesignerWidget = None
|