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