Merge branch 'main' into arrows

Author: GiulioRomualdi

.github/workflows/ci.yml

@@ -11,7 +11,7 @@ jobs:
     name: Find Trailing Whitespace
     runs-on: ubuntu-20.04
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
       - name: Find Trailing Whitespace
         run: |
           set +e
@@ -28,8 +28,7 @@ jobs:
     name: Generate python files from ui
     runs-on: ubuntu-20.04
     steps:
-      - uses: actions/checkout@v2.4.0
-
+      - uses: actions/checkout@v4
       - name: Remove broken apt repos [Ubuntu]
         run: |
           for apt_file in `grep -lr microsoft /etc/apt/sources.list.d/`; do sudo rm $apt_file; done
@@ -52,14 +51,14 @@ jobs:
     needs: [build]
     if: github.ref == 'refs/heads/main'
     steps:
-      - uses: actions/checkout@v2.4.0
+      - uses: actions/checkout@v4
 
       - name: Download artifacts
-        uses: actions/download-artifact@v2
+        uses: actions/download-artifact@v4
         with:
           name: autogenerated_files
           path: robot_log_visualizer/ui/autogenerated
       - name: Deploy
-        uses: stefanzweifel/git-auto-commit-action@v4
+        uses: stefanzweifel/git-auto-commit-action@v5
         with:
           commit_message: ⚙️ Automatic update of the python UI classes

.github/workflows/pypi-ci.yml

@@ -54,7 +54,7 @@ jobs:
     steps:
 
       - name: Download artifacts
-        uses: actions/download-artifact@v2
+        uses: actions/download-artifact@v4
         with:
           name: dist
           path: dist
@@ -74,7 +74,7 @@ jobs:
     steps:
 
       - name: Download artifacts
-        uses: actions/download-artifact@v2
+        uses: actions/download-artifact@v4
         with:
           name: dist
           path: dist

robot_log_visualizer/file_reader/signal_provider.py

@@ -130,8 +130,10 @@ def __populate_numerical_data(self, file_object):
                 continue
             if "data" in value.keys():
                 data[key] = {}
-                data[key]["data"] = np.squeeze(np.array(value["data"]))
-                data[key]["timestamps"] = np.squeeze(np.array(value["timestamps"]))
+                data[key]["data"] = np.atleast_1d(np.squeeze(np.array(value["data"])))
+                data[key]["timestamps"] = np.atleast_1d(
+                    np.squeeze(np.array(value["timestamps"]))
+                )
 
                 # if the initial or end time has been updated we can also update the entire timestamps dataset
                 if data[key]["timestamps"][0] < self.initial_time:

robot_log_visualizer/plotter/matplotlib_viewer_canvas.py

@@ -201,13 +201,23 @@ def update_plots(self, paths, legends):
 
                 timestamps = data["timestamps"] - self.signal_provider.initial_time
 
-                (self.active_paths[path_string],) = self.axes.plot(
-                    timestamps,
-                    datapoints,
-                    label=legend_string,
-                    picker=True,
-                    color=next(self.color_palette),
-                )
+                if timestamps.size > 1:
+                    (self.active_paths[path_string],) = self.axes.plot(
+                        timestamps,
+                        datapoints,
+                        label=legend_string,
+                        picker=True,
+                        color=next(self.color_palette),
+                    )
+                else:
+                    (self.active_paths[path_string],) = self.axes.plot(
+                        timestamps,
+                        datapoints,
+                        label=legend_string,
+                        picker=True,
+                        color=next(self.color_palette),
+                        marker="o",
+                    )
 
         paths_to_be_canceled = []
         for active_path in self.active_paths.keys():

robot_log_visualizer/robot_visualizer/meshcat_provider.py

@@ -31,13 +31,16 @@ def __init__(self, signal_provider, period):
         self._is_model_loaded = False
         self._signal_provider = signal_provider
 
-        self.custom_model_path = ""
+        self.model_path = ""
         self.custom_package_dir = ""
+        self.base_frame = ""
         self.env_list = ["GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH", "AMENT_PREFIX_PATH"]
         self._registered_3d_points = set()
         self._registered_3d_trajectories = dict()
         self._register_3d_arrow = set()
 
+        self.frame_T_base = np.eye(4)
+
     @property
     def state(self):
         locker = QMutexLocker(self.state_lock)
@@ -84,7 +87,7 @@ def unregister_3d_arrow(self, arrow_path):
         self._register_3d_arrow.remove(arrow_path)
         self._meshcat_visualizer.delete(shape_name=arrow_path)
 
-    def load_model(self, considered_joints, model_name):
+    def load_model(self, considered_joints, model_name, base_frame=None):
         def get_model_path_from_envs(env_list):
             return [
                 Path(f) if (env != "AMENT_PREFIX_PATH") else Path(f) / "share"
@@ -115,23 +118,21 @@ def find_model_joints(model_name, considered_joints):
 
             return model_joints_index
 
-        self._is_model_loaded = False
-
         # Load the model
         model_loader = idyn.ModelLoader()
 
         self.model_joints_index = []
         # In this case the user specify the model path
-        if self.custom_model_path:
+        if self.custom_package_dir:
             self.model_joints_index = find_model_joints(
-                self.custom_model_path, considered_joints
+                self.model_path, considered_joints
             )
             considered_model_joints = [
                 considered_joints[i] for i in self.model_joints_index
             ]
 
             model_loader.loadReducedModelFromFile(
-                self.custom_model_path,
+                self.model_path,
                 considered_model_joints,
                 "urdf",
                 [self.custom_package_dir],
@@ -152,34 +153,79 @@ def find_model_joints(model_name, considered_joints):
 
                     if model_filenames:
                         model_found_in_env_folders = True
-                        self.custom_model_path = str(model_filenames[0])
+                        self.model_path = str(model_filenames[0])
                         break
 
             # If the model is not found we exit
             if not model_found_in_env_folders:
                 return False
 
             self.model_joints_index = find_model_joints(
-                self.custom_model_path, considered_joints
+                self.model_path, considered_joints
             )
             considered_model_joints = [
                 considered_joints[i] for i in self.model_joints_index
             ]
             model_loader.loadReducedModelFromFile(
-                self.custom_model_path, considered_model_joints
+                self.model_path, considered_model_joints
             )
 
         if not model_loader.isValid():
             return False
 
+        self.meshcat_visualizer_mutex.lock()
+
+        # if the model is already loaded we remove it
+        if self._is_model_loaded:
+            self._meshcat_visualizer.delete(shape_name="robot")
+            self._is_model_loaded = False
+
+        model = model_loader.model()
+        if base_frame is None:
+            self.frame_T_base = np.eye(4)
+            self.base_frame = model.getFrameName(model.getDefaultBaseLink())
+            link_frame = None
+        else:
+            base_frame_index = model.getFrameIndex(base_frame)
+            if base_frame_index == -1:  # idyn.FRAME_INVALID_INDEX
+                raise ValueError(
+                    "MeshcatProvider: Unable to find the base frame "
+                    + base_frame
+                    + " in the model."
+                )
+
+            frame_T_base = model.getFrameTransform(base_frame_index)
+            frame_T_base = frame_T_base.inverse().asHomogeneousTransform()
+            self.frame_T_base = frame_T_base.toNumPy()
+
+            link_frame = model.getLinkName(model.getFrameLink(base_frame_index))
+
+            self.base_frame = base_frame
+
         self._meshcat_visualizer.load_model(
-            model_loader.model(), model_name="robot", color=0.8
+            model, model_name="robot", color=0.8, base_link=link_frame
         )
 
         self._is_model_loaded = True
 
+        self.meshcat_visualizer_mutex.unlock()
+
         return True
 
+    def robot_frames(self):
+        frames = []
+        if not self._is_model_loaded:
+            return frames
+
+        model = self._meshcat_visualizer.model["robot"]
+        frames_number = model.getNrOfFrames()
+
+        for i in range(frames_number):
+            frame = model.getFrameName(i)
+            frames.append(frame)
+
+        return frames
+
     def run(self):
         identity = np.eye(3)
 
@@ -191,9 +237,16 @@ def run(self):
                 robot_state = self._signal_provider.get_robot_state_at_index(index)
                 self.meshcat_visualizer_mutex.lock()
                 # These are the robot measured joint positions in radians
+                # we need to compute the base transform as
+                # I_T_B = I_T_F * frame_T_Base
+                # I_R_B = I_R_F * frame_R_Base
+                # I_p_B = I_p_F + I_R_F * frame_p_Base
                 self._meshcat_visualizer.set_multibody_system_state(
-                    base_position=robot_state["base_position"],
-                    base_rotation=robot_state["base_orientation"],
+                    base_position=robot_state["base_orientation"]
+                    @ self.frame_T_base[:3, 3]
+                    + robot_state["base_position"],
+                    base_rotation=robot_state["base_orientation"]
+                    @ self.frame_T_base[:3, :3],
                     joint_value=robot_state["joints_position"][self.model_joints_index],
                     model_name="robot",
                 )

robot_log_visualizer/ui/autogenerated/set_robot_model.py

@@ -13,52 +13,65 @@
 class Ui_setRobotModelDialog(object):
     def setupUi(self, setRobotModelDialog):
         setRobotModelDialog.setObjectName("setRobotModelDialog")
-        setRobotModelDialog.resize(365, 217)
-        self.gridLayout = QtWidgets.QGridLayout(setRobotModelDialog)
-        self.gridLayout.setObjectName("gridLayout")
-        self.buttonBox = QtWidgets.QDialogButtonBox(setRobotModelDialog)
-        self.buttonBox.setOrientation(QtCore.Qt.Horizontal)
-        self.buttonBox.setStandardButtons(QtWidgets.QDialogButtonBox.Abort|QtWidgets.QDialogButtonBox.Save)
-        self.buttonBox.setObjectName("buttonBox")
-        self.gridLayout.addWidget(self.buttonBox, 3, 0, 1, 1)
+        setRobotModelDialog.resize(711, 363)
+        self.formLayout = QtWidgets.QFormLayout(setRobotModelDialog)
+        self.formLayout.setObjectName("formLayout")
         self.frame = QtWidgets.QFrame(setRobotModelDialog)
         self.frame.setFrameShape(QtWidgets.QFrame.StyledPanel)
         self.frame.setObjectName("frame")
         self.gridLayout_2 = QtWidgets.QGridLayout(self.frame)
         self.gridLayout_2.setObjectName("gridLayout_2")
-        self.robotModelLineEdit = QtWidgets.QLineEdit(self.frame)
-        font = QtGui.QFont()
-        font.setFamily("Ubuntu Mono")
-        self.robotModelLineEdit.setFont(font)
-        self.robotModelLineEdit.setObjectName("robotModelLineEdit")
-        self.gridLayout_2.addWidget(self.robotModelLineEdit, 1, 0, 1, 1)
         self.label = QtWidgets.QLabel(self.frame)
         self.label.setObjectName("label")
         self.gridLayout_2.addWidget(self.label, 0, 0, 1, 1)
         self.robotModelToolButton = QtWidgets.QToolButton(self.frame)
         self.robotModelToolButton.setObjectName("robotModelToolButton")
         self.gridLayout_2.addWidget(self.robotModelToolButton, 1, 1, 1, 1)
-        self.gridLayout.addWidget(self.frame, 0, 0, 1, 1)
+        self.robotModelLineEdit = QtWidgets.QLineEdit(self.frame)
+        font = QtGui.QFont()
+        font.setFamily("Ubuntu Mono")
+        self.robotModelLineEdit.setFont(font)
+        self.robotModelLineEdit.setObjectName("robotModelLineEdit")
+        self.gridLayout_2.addWidget(self.robotModelLineEdit, 1, 0, 1, 1)
+        self.formLayout.setWidget(0, QtWidgets.QFormLayout.SpanningRole, self.frame)
         self.frame_2 = QtWidgets.QFrame(setRobotModelDialog)
         self.frame_2.setFrameShape(QtWidgets.QFrame.StyledPanel)
         self.frame_2.setObjectName("frame_2")
         self.gridLayout_3 = QtWidgets.QGridLayout(self.frame_2)
         self.gridLayout_3.setObjectName("gridLayout_3")
-        self.label_3 = QtWidgets.QLabel(self.frame_2)
-        self.label_3.setObjectName("label_3")
-        self.gridLayout_3.addWidget(self.label_3, 0, 0, 1, 1)
-        self.packageDirToolButton = QtWidgets.QToolButton(self.frame_2)
-        self.packageDirToolButton.setObjectName("packageDirToolButton")
-        self.gridLayout_3.addWidget(self.packageDirToolButton, 1, 1, 1, 1)
         self.packageDirLineEdit = QtWidgets.QLineEdit(self.frame_2)
         font = QtGui.QFont()
         font.setFamily("Ubuntu Mono")
         self.packageDirLineEdit.setFont(font)
         self.packageDirLineEdit.setObjectName("packageDirLineEdit")
         self.gridLayout_3.addWidget(self.packageDirLineEdit, 1, 0, 1, 1)
-        self.gridLayout.addWidget(self.frame_2, 1, 0, 1, 1)
+        self.packageDirToolButton = QtWidgets.QToolButton(self.frame_2)
+        self.packageDirToolButton.setObjectName("packageDirToolButton")
+        self.gridLayout_3.addWidget(self.packageDirToolButton, 1, 1, 1, 1)
+        self.label_3 = QtWidgets.QLabel(self.frame_2)
+        self.label_3.setObjectName("label_3")
+        self.gridLayout_3.addWidget(self.label_3, 0, 0, 1, 1)
+        self.formLayout.setWidget(1, QtWidgets.QFormLayout.SpanningRole, self.frame_2)
+        self.frame_3 = QtWidgets.QFrame(setRobotModelDialog)
+        self.frame_3.setFrameShape(QtWidgets.QFrame.StyledPanel)
+        self.frame_3.setObjectName("frame_3")
+        self.formLayout_2 = QtWidgets.QFormLayout(self.frame_3)
+        self.formLayout_2.setObjectName("formLayout_2")
+        self.label_5 = QtWidgets.QLabel(self.frame_3)
+        self.label_5.setObjectName("label_5")
+        self.formLayout_2.setWidget(0, QtWidgets.QFormLayout.LabelRole, self.label_5)
+        self.frameNameComboBox = QtWidgets.QComboBox(self.frame_3)
+        self.frameNameComboBox.setMaxVisibleItems(5)
+        self.frameNameComboBox.setObjectName("frameNameComboBox")
+        self.formLayout_2.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.frameNameComboBox)
+        self.formLayout.setWidget(2, QtWidgets.QFormLayout.SpanningRole, self.frame_3)
+        self.buttonBox = QtWidgets.QDialogButtonBox(setRobotModelDialog)
+        self.buttonBox.setOrientation(QtCore.Qt.Horizontal)
+        self.buttonBox.setStandardButtons(QtWidgets.QDialogButtonBox.Abort|QtWidgets.QDialogButtonBox.Save)
+        self.buttonBox.setObjectName("buttonBox")
+        self.formLayout.setWidget(4, QtWidgets.QFormLayout.SpanningRole, self.buttonBox)
         spacerItem = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding)
-        self.gridLayout.addItem(spacerItem, 2, 0, 1, 1)
+        self.formLayout.setItem(3, QtWidgets.QFormLayout.FieldRole, spacerItem)
 
         self.retranslateUi(setRobotModelDialog)
         self.buttonBox.accepted.connect(setRobotModelDialog.accept)
@@ -70,5 +83,6 @@ def retranslateUi(self, setRobotModelDialog):
         setRobotModelDialog.setWindowTitle(_translate("setRobotModelDialog", "Dialog"))
         self.label.setText(_translate("setRobotModelDialog", "Robot Model"))
         self.robotModelToolButton.setText(_translate("setRobotModelDialog", "..."))
-        self.label_3.setText(_translate("setRobotModelDialog", "Package Directory"))
         self.packageDirToolButton.setText(_translate("setRobotModelDialog", "..."))
+        self.label_3.setText(_translate("setRobotModelDialog", "Package Directory"))
+        self.label_5.setText(_translate("setRobotModelDialog", "Base Frame"))