rational-linkages 2.1.0__cp312-cp312-win_arm64.whl → 2.2.3__cp312-cp312-win_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.
@@ -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
- # PyQt and Pyqtgraph imports
8
- from PyQt6 import QtCore, QtWidgets
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
- .. testcode:: [motiondesigner_example1]
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:: [motiondesigner_example1]
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:: [motiondesigner_example1]
60
+ .. testcleanup:: [motiondesigner_ex1]
48
61
 
49
62
  del d, MotionDesigner
50
63
 
51
- .. testcode:: [motiondesigner_example2]
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
- class MotionDesignerWidget(QtWidgets.QWidget):
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
- 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.
148
142
  """
149
- super().__init__(parent)
150
- self.setMinimumSize(900, 600)
151
-
152
- self.white_background = white_background
153
- self.points = self._initialize_points(method, initial_pts)
154
- self.method = method
155
- self.arrows_length = arrows_length
156
- self.mi = MotionInterpolation()
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
- Create a horizontal line separator (QFrame).
152
+ Initialize the motion designer widget.
210
153
  """
211
- separator = QtWidgets.QFrame()
212
- separator.setFrameShape(QtWidgets.QFrame.Shape.HLine)
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.slider_roll_prev = 0
266
- self.slider_pitch_prev = 0
267
- 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()
268
162
 
269
- # slider range
270
- for slider, textbox in [(self.slider_roll, self.textbox_roll),
271
- (self.slider_pitch, self.textbox_pitch),
272
- (self.slider_yaw, self.textbox_yaw)]:
273
- slider.setMinimum(int(-np.pi * 100))
274
- slider.setMaximum(int(np.pi * 100))
275
- slider.setSingleStep(1)
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
- # slider for lambda of cubic curve with textbox
279
- if method == 'cubic_from_poses':
280
- self.slider_lambda = QtWidgets.QSlider(QtCore.Qt.Orientation.Horizontal)
281
- self.textbox_lambda = QtWidgets.QLineEdit()
171
+ if self.white_background:
172
+ self.render_mode = 'opaque'
173
+ else:
174
+ self.render_mode = 'additive'
282
175
 
283
- self.slider_lambda.setMinimum(int(-500))
284
- self.slider_lambda.setMaximum(int(500))
285
- self.slider_lambda.setSingleStep(1)
176
+ self.previous_rpy_sliders_values = []
286
177
 
287
- self.slider_lambda.valueChanged.connect(self.on_lambda_slider_value_changed)
288
- self.textbox_lambda.editingFinished.connect(
289
- lambda: self.on_lambda_textbox_changed(self.textbox_lambda.text(),
290
- 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])
291
182
 
292
- # add button for swapping family
293
- 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
294
207
  self.motion_family_idx = 0
295
- self.swap_family_check_box.stateChanged.connect(self.on_swap_family_check_box_changed)
296
- else:
297
- self.slider_lambda = None
298
- self.swap_family_check_box = None
299
- self.textbox_lambda = None
300
-
301
- # add button for mechanism synthesis
302
- self.synthesize_button = QtWidgets.QPushButton("Mechanism")
303
- self.synthesize_button.clicked.connect(self.on_synthesize_button_clicked)
304
-
305
- # initially for the first point
306
- self.set_sliders_for_point(0)
307
-
308
- # --- layout the 3D view and control panel ---
309
- main_layout = QtWidgets.QHBoxLayout(self)
310
- # add plotter (stored in self.plotter.widget)
311
- main_layout.addWidget(self.plotter.widget, stretch=1)
312
-
313
- # Build a vertical control panel.
314
- control_panel = QtWidgets.QWidget()
315
- cp_layout = QtWidgets.QVBoxLayout(control_panel)
316
-
317
- cp_layout.addWidget(QtWidgets.QLabel("Select control point:"))
318
- cp_layout.addWidget(self.point_combo)
319
- cp_layout.addSpacing(10)
320
-
321
- cp_layout.addWidget(QtWidgets.QLabel("Adjust X:"))
322
- cp_layout.addWidget(self.slider_x)
323
- cp_layout.addWidget(self.textbox_x)
324
- cp_layout.addWidget(QtWidgets.QLabel("Adjust Y:"))
325
- cp_layout.addWidget(self.slider_y)
326
- cp_layout.addWidget(self.textbox_y)
327
- cp_layout.addWidget(QtWidgets.QLabel("Adjust Z:"))
328
- cp_layout.addWidget(self.slider_z)
329
- cp_layout.addWidget(self.textbox_z)
330
- if method == 'quadratic_from_poses' or method == 'cubic_from_poses':
331
- cp_layout.addSpacing(10) # Add 10 pixels of space before the separator
332
- cp_layout.addWidget(create_separator())
333
- cp_layout.addWidget(QtWidgets.QLabel("Rotate X:"))
334
- cp_layout.addWidget(self.slider_roll)
335
- cp_layout.addWidget(self.textbox_roll)
336
- cp_layout.addWidget(QtWidgets.QLabel("Rotate Y:"))
337
- cp_layout.addWidget(self.slider_pitch)
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
- def on_slider_value_changed(self, value):
483
- """
484
- Called when any of the sliders change their value. Update the currently
485
- selected control point’s x, y, or z coordinate based on the slider values,
486
- update the control point markers, and then recalculate the motion curve.
487
- """
488
- index = self.point_combo.currentIndex() + 1
489
- # Convert slider values (integers) to floating‑point coordinates.
490
- new_x = self.slider_x.value() / 100.0
491
- new_y = self.slider_y.value() / 100.0
492
- new_z = self.slider_z.value() / 100.0
493
-
494
- self.textbox_x.setText(str(new_x))
495
- self.textbox_y.setText(str(new_y))
496
- self.textbox_z.setText(str(new_z))
497
-
498
- if self.method == 'quadratic_from_poses' or self.method == 'cubic_from_poses':
499
- if self.slider_roll.value() != self.slider_roll_prev:
500
- new_roll = (self.slider_roll.value() - self.slider_roll_prev) / 100.0
501
- new_mat = TransfMatrix.from_rotation('x', new_roll)
502
- new_tr = self.plotted_poses[index].tr * new_mat
503
- self.slider_roll_prev = self.slider_roll.value()
504
- self.textbox_roll.setText(str(self.slider_roll.value() / 100.0))
505
-
506
- elif self.slider_pitch.value() != self.slider_pitch_prev:
507
- new_pitch = (self.slider_pitch.value() - self.slider_pitch_prev) / 100.0
508
- new_mat = TransfMatrix.from_rotation('y', new_pitch)
509
- new_tr = self.plotted_poses[index].tr * new_mat
510
- self.slider_pitch_prev = self.slider_pitch.value()
511
- self.textbox_pitch.setText(str(self.slider_pitch.value() / 100.0))
512
-
513
- elif self.slider_yaw.value() != self.slider_yaw_prev:
514
- new_yaw = (self.slider_yaw.value() - self.slider_yaw_prev) / 100.0
515
- new_mat = TransfMatrix.from_rotation('z', new_yaw)
516
- new_tr = self.plotted_poses[index].tr * new_mat
517
- self.slider_yaw_prev = self.slider_yaw.value()
518
- 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)
519
301
  else:
520
- new_tr = TransfMatrix.from_rpy_xyz(self.plotted_poses[index].tr.rpy(),
521
- [new_x, new_y, new_z])
302
+ self.slider_lambda = None
303
+ self.swap_family_check_box = None
304
+ self.textbox_lambda = None
522
305
 
523
- self.previous_rpy_sliders_values[index][0] = self.slider_roll.value()
524
- self.previous_rpy_sliders_values[index][1] = self.slider_pitch.value()
525
- 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)
526
309
 
527
- new_dq = DualQuaternion(new_tr.matrix2dq())
528
- self.points[index] = new_dq
529
- self.plotted_poses[index].setData(new_tr)
310
+ # initially for the first point
311
+ self.set_sliders_for_point(0)
530
312
 
531
- else:
532
- # update the selected control point
533
- self.points[index] = PointHomogeneous.from_3d_point([new_x, new_y, new_z])
534
- self.plotted_points[index] = np.array([new_x, new_y, new_z])
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
- # Recalculate and update the motion curve.
539
- self.update_curve_vis()
318
+ # Build a vertical control panel.
319
+ control_panel = QtWidgets.QWidget()
320
+ cp_layout = QtWidgets.QVBoxLayout(control_panel)
540
321
 
541
- def on_lambda_slider_value_changed(self, value):
542
- """
543
- Called when the lambda slider changes its value. Update the lambda value
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
- def on_swap_family_check_box_changed(self, state):
552
- """
553
- Called when the swap family checkbox changes its state. Update the
554
- motion curve to reflect the new motion family.
555
- """
556
- if state == 2:
557
- self.motion_family_idx = 1
558
- else:
559
- 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)
560
456
 
561
- 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)
562
486
 
563
- def on_lambda_textbox_changed(self, text, slider):
564
- """
565
- 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)
566
535
 
567
- :param str text: The text input from the textbox. Should be a number.
568
- :param slider: The slider to update with the new value.
569
- """
570
- if text is not None:
571
- try:
572
- value = float(text)
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
- if abs(value - 1.0) < 1e-10:
578
- value = 1.00000001 # avoid numerical issues with 1.0
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
- def on_textbox_changed(self, text, slider):
586
- """
587
- Update the given slider with the value from the corresponding textbox.
588
- """
589
- if text is not None:
590
- try:
591
- value = float(text)
592
- slider.blockSignals(True)
593
- slider.setValue(int(value * 100))
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
- self.on_slider_value_changed(value)
597
- except ValueError:
598
- raise ValueError(f"Invalid input for slider: {text}")
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
- def update_curve_vis(self):
601
- """
602
- Recalculate the motion curve using the current control points. The
603
- interpolation is performed by MotionInterpolation. Then update the curve
604
- line in the GLViewWidget.
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
- # get the numeric coefficients from interpolation
608
- if self.method == 'cubic_from_points':
609
- coeffs = self.mi.interpolate_points_cubic(self.points,
610
- return_numeric=True)
611
- elif self.method == 'quadratic_from_points':
612
- 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,
613
615
  return_numeric=True)
614
- elif self.method == 'quadratic_from_poses':
615
- coeffs = self.mi.interpolate_quadratic_numerically(self.points)
616
- elif self.method == 'cubic_from_poses':
617
- coeffs = self.mi.interpolate_cubic_numerically(self.points,
618
- lambda_val=self.lambda_val,
619
- k_idx=self.motion_family_idx)
620
-
621
- # create numpy polynomial objects
622
- curve = [np.polynomial.Polynomial(c[::-1]) for c in coeffs]
623
-
624
- # parameter values using a tangent substitution
625
- t_space = np.tan(np.linspace(-np.pi / 2, np.pi / 2, self.plotter.steps + 1))
626
- curve_points = []
627
- for t in t_space:
628
- dq = DualQuaternion([poly(t) for poly in curve]) # evaluate fot each t
629
- pt = dq.dq2point_via_matrix()
630
- curve_points.append(pt)
631
- curve_points = np.array(curve_points)
632
-
633
- t_space_frames = np.tan(np.linspace(-np.pi / 2, np.pi / 2, 51))
634
- curve_frames = []
635
- for t in t_space_frames:
636
- dq = DualQuaternion([poly(t) for poly in curve])
637
- curve_frames.append(TransfMatrix(dq.dq2matrix()))
638
-
639
- # if the curve line has not yet been created
640
- if self.curve_path_vis is None:
641
- self.curve_path_vis = gl.GLLinePlotItem(pos=curve_points,
642
- color=(0.5, 0.5, 0.5, 1),
643
- glOptions=self.render_mode,
644
- width=2,
645
- antialias=True)
646
- self.plotter.widget.addItem(self.curve_path_vis)
647
-
648
- self.curve_frames_vis = [FramePlotHelper(transform=tr,
649
- length=self.plotter.arrows_length)
650
- for tr in curve_frames]
651
- for frame in self.curve_frames_vis:
652
- frame.addToView(self.plotter.widget)
653
- else: # update the existing curve visuals
654
- self.curve_path_vis.setData(pos=curve_points)
655
- for i, frame in enumerate(self.curve_frames_vis):
656
- frame.setData(curve_frames[i])
657
-
658
- def closeEvent(self, event):
659
- """
660
- Called when the window is closed. Ensure that the Qt application exits.
661
- """
662
- print("Closing the window... generated points for interpolation:")
663
- for pt in self.points:
664
- print(pt)
665
- if self.slider_lambda:
666
- print(f"Lambda: {self.slider_lambda.value() / 100.0}")
667
- if self.swap_family_check_box:
668
- print(f"Motion family index: {self.motion_family_idx}")
669
- 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