rational-linkages 2.2.0__cp312-cp312-win_amd64.whl → 2.2.1__cp312-cp312-win_amd64.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -611,99 +611,102 @@ class PlotterPyqtgraph:
611
611
  event.accept()
612
612
 
613
613
 
614
- class CustomGLViewWidget(gl.GLViewWidget):
615
- def __init__(self, white_background=False, *args, **kwargs):
616
- super().__init__(*args, **kwargs)
617
- self.labels = []
618
- self.white_background = white_background
619
- # Create an overlay widget for displaying text
620
- self.text_overlay = QtWidgets.QWidget(self)
621
- self.text_overlay.setAttribute(
622
- QtCore.Qt.WidgetAttribute.WA_TransparentForMouseEvents)
623
- self.text_overlay.setStyleSheet("background:transparent;")
624
- self.text_overlay.resize(self.size())
625
- self.text_overlay.show()
626
-
627
- def resizeEvent(self, event):
628
- super().resizeEvent(event)
629
- if hasattr(self, 'text_overlay'):
614
+ if gl is not None:
615
+ class CustomGLViewWidget(gl.GLViewWidget):
616
+ def __init__(self, white_background=False, *args, **kwargs):
617
+ super().__init__(*args, **kwargs)
618
+ self.labels = []
619
+ self.white_background = white_background
620
+ # Create an overlay widget for displaying text
621
+ self.text_overlay = QtWidgets.QWidget(self)
622
+ self.text_overlay.setAttribute(
623
+ QtCore.Qt.WidgetAttribute.WA_TransparentForMouseEvents)
624
+ self.text_overlay.setStyleSheet("background:transparent;")
630
625
  self.text_overlay.resize(self.size())
631
-
632
- def add_label(self, point, text):
633
- """Adds a label for a 3D point."""
634
- self.labels.append({'point': point, 'text': text})
635
- self.update()
636
-
637
- def paintEvent(self, event):
638
- # Only handle standard OpenGL rendering here - no mixing with QPainter
639
- super().paintEvent(event)
640
-
641
- # Schedule label painting as a separate operation
642
- QtCore.QTimer.singleShot(0, self.update_text_overlay)
643
-
644
- def update_text_overlay(self):
645
- """Update the text overlay with current labels"""
646
- # Create a new painter for the overlay widget
647
- self.text_overlay.update()
648
-
649
- def _obtain_label_vec(self, pt):
650
- """Obtain the label vector."""
651
- # Convert the 3D point to homogeneous coordinates
652
- if isinstance(pt, np.ndarray):
653
- point_vec = pt
654
- elif isinstance(pt, PointHomogeneous):
655
- point_vec = [pt.coordinates_normalized[1],
656
- pt.coordinates_normalized[2],
657
- pt.coordinates_normalized[3]]
658
- elif isinstance(pt, TransfMatrix):
659
- point_vec = [pt.t[0], pt.t[1], pt.t[2]]
660
- elif isinstance(pt, FramePlotHelper):
661
- point_vec = [pt.tr.t[0], pt.tr.t[1], pt.tr.t[2]]
662
- else: # is pyqtgraph marker (scatter)
663
- point_vec = [pt.pos[0][0], pt.pos[0][1], pt.pos[0][2]]
664
-
665
- return QtGui.QVector4D(point_vec[0], point_vec[1], point_vec[2], 1.0)
666
-
667
- # This method renders text on the overlay
668
- def paintOverlay(self, event):
669
- painter = QtGui.QPainter(self.text_overlay)
670
- painter.setRenderHint(QtGui.QPainter.RenderHint.Antialiasing)
671
- if self.white_background:
672
- painter.setPen(QtGui.QColor(QtCore.Qt.GlobalColor.black))
673
- else:
674
- painter.setPen(QtGui.QColor(QtCore.Qt.GlobalColor.white))
675
-
676
- # Get the Model-View-Projection matrix
677
- projection_matrix = self.projectionMatrix()
678
- view_matrix = self.viewMatrix()
679
- mvp = projection_matrix * view_matrix
680
-
681
- # Draw all labels
682
- for entry in self.labels:
683
- point = entry['point']
684
- text = entry['text']
685
-
686
- projected = mvp.map(self._obtain_label_vec(point))
687
- if projected.w() != 0:
688
- ndc_x = projected.x() / projected.w()
689
- ndc_y = projected.y() / projected.w()
690
- # Check if the point is in front of the camera
691
- if projected.z() / projected.w() < 1.0:
692
- x = int((ndc_x * 0.5 + 0.5) * self.width())
693
- y = int((1 - (ndc_y * 0.5 + 0.5)) * self.height())
694
- painter.drawText(x, y, text)
695
-
696
- painter.end()
697
-
698
- def showEvent(self, event):
699
- super().showEvent(event)
700
- self.text_overlay.installEventFilter(self)
701
-
702
- def eventFilter(self, obj, event):
703
- if obj is self.text_overlay and event.type() == QtCore.QEvent.Type.Paint:
704
- self.paintOverlay(event)
705
- return True
706
- return super().eventFilter(obj, event)
626
+ self.text_overlay.show()
627
+
628
+ def resizeEvent(self, event):
629
+ super().resizeEvent(event)
630
+ if hasattr(self, 'text_overlay'):
631
+ self.text_overlay.resize(self.size())
632
+
633
+ def add_label(self, point, text):
634
+ """Adds a label for a 3D point."""
635
+ self.labels.append({'point': point, 'text': text})
636
+ self.update()
637
+
638
+ def paintEvent(self, event):
639
+ # Only handle standard OpenGL rendering here - no mixing with QPainter
640
+ super().paintEvent(event)
641
+
642
+ # Schedule label painting as a separate operation
643
+ QtCore.QTimer.singleShot(0, self.update_text_overlay)
644
+
645
+ def update_text_overlay(self):
646
+ """Update the text overlay with current labels"""
647
+ # Create a new painter for the overlay widget
648
+ self.text_overlay.update()
649
+
650
+ def _obtain_label_vec(self, pt):
651
+ """Obtain the label vector."""
652
+ # Convert the 3D point to homogeneous coordinates
653
+ if isinstance(pt, np.ndarray):
654
+ point_vec = pt
655
+ elif isinstance(pt, PointHomogeneous):
656
+ point_vec = [pt.coordinates_normalized[1],
657
+ pt.coordinates_normalized[2],
658
+ pt.coordinates_normalized[3]]
659
+ elif isinstance(pt, TransfMatrix):
660
+ point_vec = [pt.t[0], pt.t[1], pt.t[2]]
661
+ elif isinstance(pt, FramePlotHelper):
662
+ point_vec = [pt.tr.t[0], pt.tr.t[1], pt.tr.t[2]]
663
+ else: # is pyqtgraph marker (scatter)
664
+ point_vec = [pt.pos[0][0], pt.pos[0][1], pt.pos[0][2]]
665
+
666
+ return QtGui.QVector4D(point_vec[0], point_vec[1], point_vec[2], 1.0)
667
+
668
+ # This method renders text on the overlay
669
+ def paintOverlay(self, event):
670
+ painter = QtGui.QPainter(self.text_overlay)
671
+ painter.setRenderHint(QtGui.QPainter.RenderHint.Antialiasing)
672
+ if self.white_background:
673
+ painter.setPen(QtGui.QColor(QtCore.Qt.GlobalColor.black))
674
+ else:
675
+ painter.setPen(QtGui.QColor(QtCore.Qt.GlobalColor.white))
676
+
677
+ # Get the Model-View-Projection matrix
678
+ projection_matrix = self.projectionMatrix()
679
+ view_matrix = self.viewMatrix()
680
+ mvp = projection_matrix * view_matrix
681
+
682
+ # Draw all labels
683
+ for entry in self.labels:
684
+ point = entry['point']
685
+ text = entry['text']
686
+
687
+ projected = mvp.map(self._obtain_label_vec(point))
688
+ if projected.w() != 0:
689
+ ndc_x = projected.x() / projected.w()
690
+ ndc_y = projected.y() / projected.w()
691
+ # Check if the point is in front of the camera
692
+ if projected.z() / projected.w() < 1.0:
693
+ x = int((ndc_x * 0.5 + 0.5) * self.width())
694
+ y = int((1 - (ndc_y * 0.5 + 0.5)) * self.height())
695
+ painter.drawText(x, y, text)
696
+
697
+ painter.end()
698
+
699
+ def showEvent(self, event):
700
+ super().showEvent(event)
701
+ self.text_overlay.installEventFilter(self)
702
+
703
+ def eventFilter(self, obj, event):
704
+ if obj is self.text_overlay and event.type() == QtCore.QEvent.Type.Paint:
705
+ self.paintOverlay(event)
706
+ return True
707
+ return super().eventFilter(obj, event)
708
+ else:
709
+ CustomGLViewWidget = None
707
710
 
708
711
 
709
712
  class FramePlotHelper:
@@ -766,388 +769,389 @@ class FramePlotHelper:
766
769
  view.addItem(self.y_axis)
767
770
  view.addItem(self.z_axis)
768
771
 
769
-
770
- class InteractivePlotterWidget(QtWidgets.QWidget):
771
- """
772
- A QWidget that contains a PlotterPyqtgraph 3D view and interactive controls.
773
-
774
- Containts (sliders and text boxes) for plotting and manipulating a mechanism.
775
- """
776
- def __init__(self,
777
- mechanism: RationalMechanism,
778
- base=None,
779
- show_tool: bool = True,
780
- steps: int = 1000,
781
- joint_sliders_lim: float = 1.0,
782
- arrows_length: float = 1.0,
783
- white_background: bool = False,
784
- parent=None,
785
- parent_app=None):
786
- super().__init__(parent)
787
- self.setMinimumSize(800, 600)
788
-
789
- self.mechanism = mechanism
790
- self.show_tool = show_tool
791
- self.steps = steps
792
- self.joint_sliders_lim = joint_sliders_lim
793
- self.arrows_length = arrows_length
794
-
795
- if base is not None:
796
- if isinstance(base, TransfMatrix):
797
- if not base.is_rotation():
798
- raise ValueError("Given matrix is not proper rotation.")
799
- self.base = base
800
- self.base_arr = self.base.array()
801
- elif isinstance(base, DualQuaternion):
802
- self.base = TransfMatrix(base.dq2matrix())
803
- self.base_arr = self.base.array()
772
+ if QtWidgets is not None:
773
+ class InteractivePlotterWidget(QtWidgets.QWidget):
774
+ """
775
+ A QWidget that contains a PlotterPyqtgraph 3D view and interactive controls.
776
+
777
+ Containts (sliders and text boxes) for plotting and manipulating a mechanism.
778
+ """
779
+ def __init__(self,
780
+ mechanism: RationalMechanism,
781
+ base=None,
782
+ show_tool: bool = True,
783
+ steps: int = 1000,
784
+ joint_sliders_lim: float = 1.0,
785
+ arrows_length: float = 1.0,
786
+ white_background: bool = False,
787
+ parent=None,
788
+ parent_app=None):
789
+ super().__init__(parent)
790
+ self.setMinimumSize(800, 600)
791
+
792
+ self.mechanism = mechanism
793
+ self.show_tool = show_tool
794
+ self.steps = steps
795
+ self.joint_sliders_lim = joint_sliders_lim
796
+ self.arrows_length = arrows_length
797
+
798
+ if base is not None:
799
+ if isinstance(base, TransfMatrix):
800
+ if not base.is_rotation():
801
+ raise ValueError("Given matrix is not proper rotation.")
802
+ self.base = base
803
+ self.base_arr = self.base.array()
804
+ elif isinstance(base, DualQuaternion):
805
+ self.base = TransfMatrix(base.dq2matrix())
806
+ self.base_arr = self.base.array()
807
+ else:
808
+ raise TypeError("Base must be a TransfMatrix or DualQuaternion instance.")
804
809
  else:
805
- raise TypeError("Base must be a TransfMatrix or DualQuaternion instance.")
806
- else:
807
- self.base = None
808
- self.base_arr = None
809
-
810
- self.white_background = white_background
811
- if self.white_background:
812
- self.render_mode = 'translucent'
813
- else:
814
- self.render_mode = 'additive'
810
+ self.base = None
811
+ self.base_arr = None
815
812
 
816
- # Create the PlotterPyqtgraph instance.
817
- self.plotter = PlotterPyqtgraph(base=None,
818
- steps=self.steps,
819
- arrows_length=self.arrows_length,
820
- white_background=self.white_background,
821
- parent_app=parent_app)
822
- # Optionally adjust the camera.
823
- self.plotter.widget.setCameraPosition(distance=10, azimuth=30, elevation=30)
824
-
825
- # Main layout: split between the 3D view and a control panel.
826
- main_layout = QtWidgets.QHBoxLayout(self)
827
-
828
- # Add the 3D view (PlotterPyqtgraph’s widget) to the layout.
829
- main_layout.addWidget(self.plotter.widget, stretch=5)
830
-
831
- # Create the control panel (on the right).
832
- control_panel = QtWidgets.QWidget()
833
- control_layout = QtWidgets.QVBoxLayout(control_panel)
834
-
835
- # --- Driving joint angle slider ---
836
- control_layout.addWidget(QtWidgets.QLabel("Joint angle [rad]:"))
837
- self.move_slider = self.create_float_slider(0.0, 2 * np.pi, 0.0,
838
- orientation=QtCore.Qt.Orientation.Horizontal)
839
- control_layout.addWidget(self.move_slider)
840
-
841
- # --- Text boxes ---
842
- self.text_box_angle = QtWidgets.QLineEdit()
843
- self.text_box_angle.setPlaceholderText("Set angle [rad]:")
844
- control_layout.addWidget(self.text_box_angle)
845
-
846
- self.text_box_param = QtWidgets.QLineEdit()
847
- self.text_box_param.setPlaceholderText("Set parameter t [-]:")
848
- control_layout.addWidget(self.text_box_param)
849
-
850
- self.save_mech_pkl = QtWidgets.QLineEdit()
851
- self.save_mech_pkl.setPlaceholderText("Save mechanism PKL, filename:")
852
- control_layout.addWidget(self.save_mech_pkl)
853
-
854
- self.save_figure_box = QtWidgets.QLineEdit()
855
- self.save_figure_box.setPlaceholderText("Save figure PNG, filename:")
856
- control_layout.addWidget(self.save_figure_box)
857
-
858
- # --- Joint connection sliders ---
859
- joint_sliders_layout = QtWidgets.QHBoxLayout()
860
- self.joint_sliders = []
861
-
862
- # Initialize sliders for each joint
863
- for i in range(self.mechanism.num_joints):
864
- slider0, slider1 = self._init_joint_sliders(i, self.joint_sliders_lim)
865
- self.joint_sliders.append(slider0)
866
- self.joint_sliders.append(slider1)
867
-
868
- # Arrange sliders vertically for each joint
869
- joint_layout = QtWidgets.QVBoxLayout()
870
-
871
- joint_layout.addWidget(QtWidgets.QLabel(f"j{i}cp0"))
872
- joint_layout.addWidget(slider0)
873
- joint_layout.addWidget(QtWidgets.QLabel(f"j{i}cp1"))
874
- joint_layout.addWidget(slider1)
875
-
876
- joint_sliders_layout.addLayout(joint_layout)
877
-
878
- control_layout.addLayout(joint_sliders_layout)
879
-
880
- # Set default values for the first factorization
881
- for i in range(self.mechanism.factorizations[0].number_of_factors):
882
- default_val0 = self.mechanism.factorizations[0].linkage[i].points_params[0]
883
- default_val1 = self.mechanism.factorizations[0].linkage[i].points_params[1]
884
- self.joint_sliders[2 * i].setValue(int(default_val0 * 100))
885
- self.joint_sliders[2 * i + 1].setValue(int(default_val1 * 100))
886
-
887
- # Set default values for the second factorization
888
- offset = 2 * self.mechanism.factorizations[0].number_of_factors
889
- for i in range(self.mechanism.factorizations[1].number_of_factors):
890
- default_val0 = self.mechanism.factorizations[1].linkage[i].points_params[0]
891
- default_val1 = self.mechanism.factorizations[1].linkage[i].points_params[1]
892
- self.joint_sliders[offset + 2 * i].setValue(int(default_val0 * 100))
893
- self.joint_sliders[offset + 2 * i + 1].setValue(int(default_val1 * 100))
894
-
895
- main_layout.addWidget(control_panel, stretch=1)
896
-
897
- # --- Initialize plot items for the mechanism links ---
898
- self.lines = []
899
- num_lines = self.mechanism.num_joints * 2
900
- for i in range(num_lines):
901
- # if i is even, make the link color green, and joints red
902
- if i % 2 == 0:
903
- line_item = gl.GLLinePlotItem(pos=np.zeros((2, 3)),
904
- color=(0, 1, 0, 1),
905
- glOptions=self.render_mode,
906
- width=5,
907
- antialias=True)
813
+ self.white_background = white_background
814
+ if self.white_background:
815
+ self.render_mode = 'translucent'
908
816
  else:
909
- line_item = gl.GLLinePlotItem(pos=np.zeros((2, 3)),
910
- color=(1, 0, 0, 1),
911
- glOptions=self.render_mode,
912
- width=5,
913
- antialias=True)
914
- self.lines.append(line_item)
915
- self.plotter.widget.addItem(line_item)
916
-
917
- # --- If desired, initialize tool plot and tool frame ---
918
- if self.show_tool:
919
- self.tool_link = gl.GLLinePlotItem(pos=np.zeros((3, 3)),
920
- color=(0, 1, 0, 0.5),
921
- glOptions=self.render_mode,
922
- width=5,
923
- antialias=True)
924
- self.plotter.widget.addItem(self.tool_link)
925
- self.tool_frame = FramePlotHelper(
926
- transform=TransfMatrix(self.mechanism.tool_frame.dq2matrix()),
927
- length=self.arrows_length)
928
- self.tool_frame.addToView(self.plotter.widget)
929
-
930
- # --- Plot the tool path ---
931
- self._plot_tool_path()
932
-
933
- # --- Connect signals to slots ---
934
- self.move_slider.valueChanged.connect(self.on_move_slider_changed)
935
- self.text_box_angle.returnPressed.connect(self.on_angle_text_entered)
936
- self.text_box_param.returnPressed.connect(self.on_param_text_entered)
937
- self.save_mech_pkl.returnPressed.connect(self.on_save_save_mech_pkl)
938
- self.save_figure_box.returnPressed.connect(self.on_save_figure_box)
939
- for slider in self.joint_sliders:
940
- slider.valueChanged.connect(self.on_joint_slider_changed)
941
-
942
- # Set initial configuration (home position)
943
- self.move_slider.setValue(self.move_slider.minimum())
944
- self.plot_slider_update(self.move_slider.value() / 100.0)
945
-
946
- self.setWindowTitle('Rational Linkages')
947
-
948
- # --- Helper to create a “float slider” (using integer scaling) ---
949
- def create_float_slider(self, min_val, max_val, init_val,
950
- orientation=QtCore.Qt.Orientation.Horizontal):
951
- slider = QtWidgets.QSlider(orientation)
952
- slider.setMinimum(int(min_val * 100))
953
- slider.setMaximum(int(max_val * 100))
954
- slider.setValue(int(init_val * 100))
955
- slider.setTickPosition(QtWidgets.QSlider.TickPosition.TicksBelow)
956
- slider.setTickInterval(10)
957
- return slider
958
-
959
- def _init_joint_sliders(self, idx, slider_limit):
960
- """
961
- Create a pair of vertical sliders for joint connection parameters.
962
- (The slider values are scaled by 100.)
963
- """
964
- slider0 = self.create_float_slider(-slider_limit,
965
- slider_limit,
966
- 0.0,
967
- orientation=QtCore.Qt.Orientation.Vertical)
968
- slider1 = self.create_float_slider(-slider_limit,
969
- slider_limit,
970
- 0.0,
971
- orientation=QtCore.Qt.Orientation.Vertical)
972
- return slider0, slider1
973
-
974
- def _plot_tool_path(self):
975
- """
976
- Plot the tool path (as a continuous line) using a set of computed points.
977
- """
978
- t_lin = np.linspace(0, 2 * np.pi, self.steps)
979
- t_vals = [self.mechanism.factorizations[0].joint_angle_to_t_param(t)
980
- for t in t_lin]
981
- ee_points = [self.mechanism.factorizations[0].direct_kinematics_of_tool(
982
- t, self.mechanism.tool_frame.dq2point_via_matrix())
983
- for t in t_vals]
984
-
985
- if self.base_arr is not None:
986
- # transform the end-effector points by the base transformation
987
- ee_points = [self.base_arr @ np.insert(p, 0, 1) for p in ee_points]
988
- # normalize
989
- ee_points = [p[1:4]/p[0] for p in ee_points]
990
-
991
- pts = np.array(ee_points)
992
- tool_path = gl.GLLinePlotItem(pos=pts,
993
- color=(0.5, 0.5, 0.5, 1),
994
- glOptions=self.render_mode,
995
- width=2,
996
- antialias=True)
997
- self.plotter.widget.addItem(tool_path)
998
-
999
- # --- Slots for interactive control events ---
1000
- def on_move_slider_changed(self, value):
1001
- """
1002
- Called when the driving joint angle slider is moved.
1003
- """
1004
- angle = value / 100.0 # Convert back to a float value.
1005
- self.plot_slider_update(angle)
1006
-
1007
- def on_angle_text_entered(self):
1008
- """
1009
- Called when the angle text box is submitted.
1010
- """
1011
- try:
1012
- val = float(self.text_box_angle.text())
1013
- # Normalize angle to [0, 2*pi]
1014
- if val >= 0:
1015
- val = val % (2 * np.pi)
817
+ self.render_mode = 'additive'
818
+
819
+ # Create the PlotterPyqtgraph instance.
820
+ self.plotter = PlotterPyqtgraph(base=None,
821
+ steps=self.steps,
822
+ arrows_length=self.arrows_length,
823
+ white_background=self.white_background,
824
+ parent_app=parent_app)
825
+ # Optionally adjust the camera.
826
+ self.plotter.widget.setCameraPosition(distance=10, azimuth=30, elevation=30)
827
+
828
+ # Main layout: split between the 3D view and a control panel.
829
+ main_layout = QtWidgets.QHBoxLayout(self)
830
+
831
+ # Add the 3D view (PlotterPyqtgraph’s widget) to the layout.
832
+ main_layout.addWidget(self.plotter.widget, stretch=5)
833
+
834
+ # Create the control panel (on the right).
835
+ control_panel = QtWidgets.QWidget()
836
+ control_layout = QtWidgets.QVBoxLayout(control_panel)
837
+
838
+ # --- Driving joint angle slider ---
839
+ control_layout.addWidget(QtWidgets.QLabel("Joint angle [rad]:"))
840
+ self.move_slider = self.create_float_slider(0.0, 2 * np.pi, 0.0,
841
+ orientation=QtCore.Qt.Orientation.Horizontal)
842
+ control_layout.addWidget(self.move_slider)
843
+
844
+ # --- Text boxes ---
845
+ self.text_box_angle = QtWidgets.QLineEdit()
846
+ self.text_box_angle.setPlaceholderText("Set angle [rad]:")
847
+ control_layout.addWidget(self.text_box_angle)
848
+
849
+ self.text_box_param = QtWidgets.QLineEdit()
850
+ self.text_box_param.setPlaceholderText("Set parameter t [-]:")
851
+ control_layout.addWidget(self.text_box_param)
852
+
853
+ self.save_mech_pkl = QtWidgets.QLineEdit()
854
+ self.save_mech_pkl.setPlaceholderText("Save mechanism PKL, filename:")
855
+ control_layout.addWidget(self.save_mech_pkl)
856
+
857
+ self.save_figure_box = QtWidgets.QLineEdit()
858
+ self.save_figure_box.setPlaceholderText("Save figure PNG, filename:")
859
+ control_layout.addWidget(self.save_figure_box)
860
+
861
+ # --- Joint connection sliders ---
862
+ joint_sliders_layout = QtWidgets.QHBoxLayout()
863
+ self.joint_sliders = []
864
+
865
+ # Initialize sliders for each joint
866
+ for i in range(self.mechanism.num_joints):
867
+ slider0, slider1 = self._init_joint_sliders(i, self.joint_sliders_lim)
868
+ self.joint_sliders.append(slider0)
869
+ self.joint_sliders.append(slider1)
870
+
871
+ # Arrange sliders vertically for each joint
872
+ joint_layout = QtWidgets.QVBoxLayout()
873
+
874
+ joint_layout.addWidget(QtWidgets.QLabel(f"j{i}cp0"))
875
+ joint_layout.addWidget(slider0)
876
+ joint_layout.addWidget(QtWidgets.QLabel(f"j{i}cp1"))
877
+ joint_layout.addWidget(slider1)
878
+
879
+ joint_sliders_layout.addLayout(joint_layout)
880
+
881
+ control_layout.addLayout(joint_sliders_layout)
882
+
883
+ # Set default values for the first factorization
884
+ for i in range(self.mechanism.factorizations[0].number_of_factors):
885
+ default_val0 = self.mechanism.factorizations[0].linkage[i].points_params[0]
886
+ default_val1 = self.mechanism.factorizations[0].linkage[i].points_params[1]
887
+ self.joint_sliders[2 * i].setValue(int(default_val0 * 100))
888
+ self.joint_sliders[2 * i + 1].setValue(int(default_val1 * 100))
889
+
890
+ # Set default values for the second factorization
891
+ offset = 2 * self.mechanism.factorizations[0].number_of_factors
892
+ for i in range(self.mechanism.factorizations[1].number_of_factors):
893
+ default_val0 = self.mechanism.factorizations[1].linkage[i].points_params[0]
894
+ default_val1 = self.mechanism.factorizations[1].linkage[i].points_params[1]
895
+ self.joint_sliders[offset + 2 * i].setValue(int(default_val0 * 100))
896
+ self.joint_sliders[offset + 2 * i + 1].setValue(int(default_val1 * 100))
897
+
898
+ main_layout.addWidget(control_panel, stretch=1)
899
+
900
+ # --- Initialize plot items for the mechanism links ---
901
+ self.lines = []
902
+ num_lines = self.mechanism.num_joints * 2
903
+ for i in range(num_lines):
904
+ # if i is even, make the link color green, and joints red
905
+ if i % 2 == 0:
906
+ line_item = gl.GLLinePlotItem(pos=np.zeros((2, 3)),
907
+ color=(0, 1, 0, 1),
908
+ glOptions=self.render_mode,
909
+ width=5,
910
+ antialias=True)
911
+ else:
912
+ line_item = gl.GLLinePlotItem(pos=np.zeros((2, 3)),
913
+ color=(1, 0, 0, 1),
914
+ glOptions=self.render_mode,
915
+ width=5,
916
+ antialias=True)
917
+ self.lines.append(line_item)
918
+ self.plotter.widget.addItem(line_item)
919
+
920
+ # --- If desired, initialize tool plot and tool frame ---
921
+ if self.show_tool:
922
+ self.tool_link = gl.GLLinePlotItem(pos=np.zeros((3, 3)),
923
+ color=(0, 1, 0, 0.5),
924
+ glOptions=self.render_mode,
925
+ width=5,
926
+ antialias=True)
927
+ self.plotter.widget.addItem(self.tool_link)
928
+ self.tool_frame = FramePlotHelper(
929
+ transform=TransfMatrix(self.mechanism.tool_frame.dq2matrix()),
930
+ length=self.arrows_length)
931
+ self.tool_frame.addToView(self.plotter.widget)
932
+
933
+ # --- Plot the tool path ---
934
+ self._plot_tool_path()
935
+
936
+ # --- Connect signals to slots ---
937
+ self.move_slider.valueChanged.connect(self.on_move_slider_changed)
938
+ self.text_box_angle.returnPressed.connect(self.on_angle_text_entered)
939
+ self.text_box_param.returnPressed.connect(self.on_param_text_entered)
940
+ self.save_mech_pkl.returnPressed.connect(self.on_save_save_mech_pkl)
941
+ self.save_figure_box.returnPressed.connect(self.on_save_figure_box)
942
+ for slider in self.joint_sliders:
943
+ slider.valueChanged.connect(self.on_joint_slider_changed)
944
+
945
+ # Set initial configuration (home position)
946
+ self.move_slider.setValue(self.move_slider.minimum())
947
+ self.plot_slider_update(self.move_slider.value() / 100.0)
948
+
949
+ self.setWindowTitle('Rational Linkages')
950
+
951
+ # --- Helper to create a “float slider” (using integer scaling) ---
952
+ def create_float_slider(self, min_val, max_val, init_val,
953
+ orientation=QtCore.Qt.Orientation.Horizontal):
954
+ slider = QtWidgets.QSlider(orientation)
955
+ slider.setMinimum(int(min_val * 100))
956
+ slider.setMaximum(int(max_val * 100))
957
+ slider.setValue(int(init_val * 100))
958
+ slider.setTickPosition(QtWidgets.QSlider.TickPosition.TicksBelow)
959
+ slider.setTickInterval(10)
960
+ return slider
961
+
962
+ def _init_joint_sliders(self, idx, slider_limit):
963
+ """
964
+ Create a pair of vertical sliders for joint connection parameters.
965
+ (The slider values are scaled by 100.)
966
+ """
967
+ slider0 = self.create_float_slider(-slider_limit,
968
+ slider_limit,
969
+ 0.0,
970
+ orientation=QtCore.Qt.Orientation.Vertical)
971
+ slider1 = self.create_float_slider(-slider_limit,
972
+ slider_limit,
973
+ 0.0,
974
+ orientation=QtCore.Qt.Orientation.Vertical)
975
+ return slider0, slider1
976
+
977
+ def _plot_tool_path(self):
978
+ """
979
+ Plot the tool path (as a continuous line) using a set of computed points.
980
+ """
981
+ t_lin = np.linspace(0, 2 * np.pi, self.steps)
982
+ t_vals = [self.mechanism.factorizations[0].joint_angle_to_t_param(t)
983
+ for t in t_lin]
984
+ ee_points = [self.mechanism.factorizations[0].direct_kinematics_of_tool(
985
+ t, self.mechanism.tool_frame.dq2point_via_matrix())
986
+ for t in t_vals]
987
+
988
+ if self.base_arr is not None:
989
+ # transform the end-effector points by the base transformation
990
+ ee_points = [self.base_arr @ np.insert(p, 0, 1) for p in ee_points]
991
+ # normalize
992
+ ee_points = [p[1:4]/p[0] for p in ee_points]
993
+
994
+ pts = np.array(ee_points)
995
+ tool_path = gl.GLLinePlotItem(pos=pts,
996
+ color=(0.5, 0.5, 0.5, 1),
997
+ glOptions=self.render_mode,
998
+ width=2,
999
+ antialias=True)
1000
+ self.plotter.widget.addItem(tool_path)
1001
+
1002
+ # --- Slots for interactive control events ---
1003
+ def on_move_slider_changed(self, value):
1004
+ """
1005
+ Called when the driving joint angle slider is moved.
1006
+ """
1007
+ angle = value / 100.0 # Convert back to a float value.
1008
+ self.plot_slider_update(angle)
1009
+
1010
+ def on_angle_text_entered(self):
1011
+ """
1012
+ Called when the angle text box is submitted.
1013
+ """
1014
+ try:
1015
+ val = float(self.text_box_angle.text())
1016
+ # Normalize angle to [0, 2*pi]
1017
+ if val >= 0:
1018
+ val = val % (2 * np.pi)
1019
+ else:
1020
+ val = (val % (2 * np.pi)) - np.pi
1021
+ self.move_slider.setValue(int(val * 100))
1022
+ except ValueError:
1023
+ pass
1024
+
1025
+ def on_param_text_entered(self):
1026
+ """
1027
+ Called when the t-parameter text box is submitted.
1028
+ """
1029
+ try:
1030
+ val = float(self.text_box_param.text())
1031
+ self.plot_slider_update(val, t_param=val)
1032
+ joint_angle = self.mechanism.factorizations[0].t_param_to_joint_angle(val)
1033
+ self.move_slider.setValue(int(joint_angle * 100))
1034
+ except ValueError:
1035
+ pass
1036
+
1037
+ def on_save_save_mech_pkl(self):
1038
+ """
1039
+ Called when the save text box is submitted.
1040
+ """
1041
+ filename = self.save_mech_pkl.text()
1042
+ self.mechanism.save(filename=filename)
1043
+
1044
+ QtWidgets.QMessageBox.information(self,
1045
+ "Success",
1046
+ f"Mechanism saved as {filename}.pkl")
1047
+
1048
+ def on_save_figure_box(self):
1049
+ """
1050
+ Called when the filesave text box is submitted.
1051
+
1052
+ Saves the current figure in the specified format.
1053
+ """
1054
+ filename = self.save_figure_box.text()
1055
+
1056
+ # better quality but does not save the text overlay
1057
+ #self.plotter.widget.readQImage().save(filename + "_old.png")
1058
+ #self.plotter.widget.readQImage().save(filename + "_old.png", quality=100)
1059
+
1060
+ image = QtGui.QImage(self.plotter.widget.size(),
1061
+ QtGui.QImage.Format.Format_ARGB32_Premultiplied)
1062
+ image.fill(QtCore.Qt.GlobalColor.transparent)
1063
+
1064
+ # Create a painter and render the widget into the image
1065
+ painter = QtGui.QPainter(image)
1066
+ self.plotter.widget.render(painter)
1067
+ painter.end()
1068
+
1069
+ # Save the image
1070
+ image.save(filename + ".png", "PNG", 80)
1071
+
1072
+ QtWidgets.QMessageBox.information(self,
1073
+ "Success",
1074
+ f"Figure saved as {filename}.png")
1075
+
1076
+ def on_joint_slider_changed(self, value):
1077
+ """
1078
+ Called when any joint slider is changed.
1079
+ Updates the joint connection parameters and refreshes the plot.
1080
+ """
1081
+ num_of_factors = self.mechanism.factorizations[0].number_of_factors
1082
+ # Update first factorization's linkage parameters.
1083
+ for i in range(num_of_factors):
1084
+ self.mechanism.factorizations[0].linkage[i].set_point_by_param(
1085
+ 0, self.joint_sliders[2 * i].value() / 100.0)
1086
+ self.mechanism.factorizations[0].linkage[i].set_point_by_param(
1087
+ 1, self.joint_sliders[2 * i + 1].value() / 100.0)
1088
+ # Update second factorization's linkage parameters.
1089
+ for i in range(num_of_factors):
1090
+ self.mechanism.factorizations[1].linkage[i].set_point_by_param(
1091
+ 0, self.joint_sliders[2 * num_of_factors + 2 * i].value() / 100.0)
1092
+ self.mechanism.factorizations[1].linkage[i].set_point_by_param(
1093
+ 1, self.joint_sliders[2 * num_of_factors + 1 + 2 * i].value() / 100.0)
1094
+ self.plot_slider_update(self.move_slider.value() / 100.0)
1095
+
1096
+ def plot_slider_update(self, angle, t_param=None):
1097
+ """
1098
+ Update the mechanism plot based on the current joint angle or t parameter.
1099
+ """
1100
+ if t_param is not None:
1101
+ t = t_param
1016
1102
  else:
1017
- val = (val % (2 * np.pi)) - np.pi
1018
- self.move_slider.setValue(int(val * 100))
1019
- except ValueError:
1020
- pass
1021
-
1022
- def on_param_text_entered(self):
1023
- """
1024
- Called when the t-parameter text box is submitted.
1025
- """
1026
- try:
1027
- val = float(self.text_box_param.text())
1028
- self.plot_slider_update(val, t_param=val)
1029
- joint_angle = self.mechanism.factorizations[0].t_param_to_joint_angle(val)
1030
- self.move_slider.setValue(int(joint_angle * 100))
1031
- except ValueError:
1032
- pass
1033
-
1034
- def on_save_save_mech_pkl(self):
1035
- """
1036
- Called when the save text box is submitted.
1037
- """
1038
- filename = self.save_mech_pkl.text()
1039
- self.mechanism.save(filename=filename)
1040
-
1041
- QtWidgets.QMessageBox.information(self,
1042
- "Success",
1043
- f"Mechanism saved as {filename}.pkl")
1044
-
1045
- def on_save_figure_box(self):
1046
- """
1047
- Called when the filesave text box is submitted.
1048
-
1049
- Saves the current figure in the specified format.
1050
- """
1051
- filename = self.save_figure_box.text()
1052
-
1053
- # better quality but does not save the text overlay
1054
- #self.plotter.widget.readQImage().save(filename + "_old.png")
1055
- #self.plotter.widget.readQImage().save(filename + "_old.png", quality=100)
1056
-
1057
- image = QtGui.QImage(self.plotter.widget.size(),
1058
- QtGui.QImage.Format.Format_ARGB32_Premultiplied)
1059
- image.fill(QtCore.Qt.GlobalColor.transparent)
1060
-
1061
- # Create a painter and render the widget into the image
1062
- painter = QtGui.QPainter(image)
1063
- self.plotter.widget.render(painter)
1064
- painter.end()
1103
+ t = self.mechanism.factorizations[0].joint_angle_to_t_param(angle)
1065
1104
 
1066
- # Save the image
1067
- image.save(filename + ".png", "PNG", 80)
1068
-
1069
- QtWidgets.QMessageBox.information(self,
1070
- "Success",
1071
- f"Figure saved as {filename}.png")
1072
-
1073
- def on_joint_slider_changed(self, value):
1074
- """
1075
- Called when any joint slider is changed.
1076
- Updates the joint connection parameters and refreshes the plot.
1077
- """
1078
- num_of_factors = self.mechanism.factorizations[0].number_of_factors
1079
- # Update first factorization's linkage parameters.
1080
- for i in range(num_of_factors):
1081
- self.mechanism.factorizations[0].linkage[i].set_point_by_param(
1082
- 0, self.joint_sliders[2 * i].value() / 100.0)
1083
- self.mechanism.factorizations[0].linkage[i].set_point_by_param(
1084
- 1, self.joint_sliders[2 * i + 1].value() / 100.0)
1085
- # Update second factorization's linkage parameters.
1086
- for i in range(num_of_factors):
1087
- self.mechanism.factorizations[1].linkage[i].set_point_by_param(
1088
- 0, self.joint_sliders[2 * num_of_factors + 2 * i].value() / 100.0)
1089
- self.mechanism.factorizations[1].linkage[i].set_point_by_param(
1090
- 1, self.joint_sliders[2 * num_of_factors + 1 + 2 * i].value() / 100.0)
1091
- self.plot_slider_update(self.move_slider.value() / 100.0)
1092
-
1093
- def plot_slider_update(self, angle, t_param=None):
1094
- """
1095
- Update the mechanism plot based on the current joint angle or t parameter.
1096
- """
1097
- if t_param is not None:
1098
- t = t_param
1099
- else:
1100
- t = self.mechanism.factorizations[0].joint_angle_to_t_param(angle)
1101
-
1102
- # Compute link positions.
1103
- links = (self.mechanism.factorizations[0].direct_kinematics(t) +
1104
- self.mechanism.factorizations[1].direct_kinematics(t)[::-1])
1105
- links.insert(0, links[-1])
1106
-
1107
- if self.base is not None:
1108
- # Transform the links by the base transformation.
1109
- links = [self.base_arr @ np.insert(p, 0, 1) for p in links]
1110
- # Normalize the homogeneous coordinates.
1111
- links = [p[1:4] / p[0] for p in links]
1112
-
1113
- # Update each line segment.
1114
- for i, line in enumerate(self.lines):
1115
- pt1 = links[i]
1116
- pt2 = links[i+1]
1117
- pts = np.array([pt1, pt2])
1118
- line.setData(pos=pts)
1119
-
1120
- if self.show_tool:
1121
- pts0 = self.mechanism.factorizations[0].direct_kinematics(t)[-1]
1122
- pts1 = self.mechanism.factorizations[0].direct_kinematics_of_tool(
1123
- t, self.mechanism.tool_frame.dq2point_via_matrix())
1124
- pts2 = self.mechanism.factorizations[1].direct_kinematics(t)[-1]
1125
-
1126
- tool_triangle = [pts0, pts1, pts2]
1105
+ # Compute link positions.
1106
+ links = (self.mechanism.factorizations[0].direct_kinematics(t) +
1107
+ self.mechanism.factorizations[1].direct_kinematics(t)[::-1])
1108
+ links.insert(0, links[-1])
1127
1109
 
1128
1110
  if self.base is not None:
1129
- # Transform the tool triangle by the base transformation.
1130
- tool_triangle = [self.base_arr @ np.insert(p, 0, 1)
1131
- for p in tool_triangle]
1111
+ # Transform the links by the base transformation.
1112
+ links = [self.base_arr @ np.insert(p, 0, 1) for p in links]
1132
1113
  # Normalize the homogeneous coordinates.
1133
- tool_triangle = [p[1:4] / p[0] for p in tool_triangle]
1134
-
1135
- self.tool_link.setData(pos=np.array(tool_triangle))
1136
-
1137
- # Update tool frame (pose) arrows.
1138
- pose_dq = DualQuaternion(self.mechanism.evaluate(t))
1139
- # Compute the pose matrix by composing the mechanism’s pose and tool frame.
1140
- pose_matrix = TransfMatrix(pose_dq.dq2matrix()) * TransfMatrix(
1141
- self.mechanism.tool_frame.dq2matrix())
1142
-
1143
- if self.base is not None:
1144
- # Transform the pose matrix by the base transformation.
1145
- pose_matrix = self.base * pose_matrix
1146
-
1147
- self.tool_frame.setData(pose_matrix)
1148
-
1149
- self.plotter.widget.update()
1150
-
1114
+ links = [p[1:4] / p[0] for p in links]
1115
+
1116
+ # Update each line segment.
1117
+ for i, line in enumerate(self.lines):
1118
+ pt1 = links[i]
1119
+ pt2 = links[i+1]
1120
+ pts = np.array([pt1, pt2])
1121
+ line.setData(pos=pts)
1122
+
1123
+ if self.show_tool:
1124
+ pts0 = self.mechanism.factorizations[0].direct_kinematics(t)[-1]
1125
+ pts1 = self.mechanism.factorizations[0].direct_kinematics_of_tool(
1126
+ t, self.mechanism.tool_frame.dq2point_via_matrix())
1127
+ pts2 = self.mechanism.factorizations[1].direct_kinematics(t)[-1]
1128
+
1129
+ tool_triangle = [pts0, pts1, pts2]
1130
+
1131
+ if self.base is not None:
1132
+ # Transform the tool triangle by the base transformation.
1133
+ tool_triangle = [self.base_arr @ np.insert(p, 0, 1)
1134
+ for p in tool_triangle]
1135
+ # Normalize the homogeneous coordinates.
1136
+ tool_triangle = [p[1:4] / p[0] for p in tool_triangle]
1137
+
1138
+ self.tool_link.setData(pos=np.array(tool_triangle))
1139
+
1140
+ # Update tool frame (pose) arrows.
1141
+ pose_dq = DualQuaternion(self.mechanism.evaluate(t))
1142
+ # Compute the pose matrix by composing the mechanism’s pose and tool frame.
1143
+ pose_matrix = TransfMatrix(pose_dq.dq2matrix()) * TransfMatrix(
1144
+ self.mechanism.tool_frame.dq2matrix())
1145
+
1146
+ if self.base is not None:
1147
+ # Transform the pose matrix by the base transformation.
1148
+ pose_matrix = self.base * pose_matrix
1149
+
1150
+ self.tool_frame.setData(pose_matrix)
1151
+
1152
+ self.plotter.widget.update()
1153
+ else:
1154
+ InteractivePlotterWidget = None
1151
1155
 
1152
1156
  class InteractivePlotter:
1153
1157
  """