Add feature to use wxyz as base orientation (#125)

Author: LoreMoretti

robot_log_visualizer/signal_provider/signal_provider.py

@@ -384,9 +384,11 @@ def get_robot_state_at_index(self, index):
                 robot_state["base_orientation"][2],
             ).toNumPy()
         if robot_state["base_orientation"].shape[0] == 4:
-            # convert the x y z w quaternion into w x y z
             tmp_quat = robot_state["base_orientation"]
-            tmp_quat = np.array([tmp_quat[3], tmp_quat[0], tmp_quat[1], tmp_quat[2]])
+            if self._robot_state_path.base_orientation_type == "xyzw":
+                # convert x y z w into w x y z (iDynTree convention)
+                tmp_quat = np.array([tmp_quat[3], tmp_quat[0], tmp_quat[1], tmp_quat[2]])
+            # wxyz is already in iDynTree convention
 
             robot_state["base_orientation"] = idyn.Rotation.RotationFromQuaternion(
                 tmp_quat

robot_log_visualizer/ui/gui.py

@@ -1107,6 +1107,7 @@ def variableTreeWidget_on_right_click(self, item_position):
                 menu.addAction(dont_use_as_base_orientation_str)
             else:
                 menu.addAction(use_as_base_orientation_str + " (xyzw Quaternion)")
+                menu.addAction(use_as_base_orientation_str + " (wxyz Quaternion)")
 
         if item_size == 6:
             if item_key in self.visualized_3d_arrows:
@@ -1199,6 +1200,12 @@ def variableTreeWidget_on_right_click(self, item_position):
                     self.robot_state_path.base_orientation_path
                 ).setBackground(0, QtGui.QBrush(deselected_base_color))
             self.robot_state_path.base_orientation_path = item_path
+            if "wxyz Quaternion" in action.text():
+                self.robot_state_path.base_orientation_type = "wxyz"
+            elif "xyzw Quaternion" in action.text():
+                self.robot_state_path.base_orientation_type = "xyzw"
+            else:
+                self.robot_state_path.base_orientation_type = "rpy"
             # In real time mode, add those signal to the provider buffer
             if (
                 self.signal_provider
@@ -1215,6 +1222,7 @@ def variableTreeWidget_on_right_click(self, item_position):
 
         if action.text() == dont_use_as_base_orientation_str:
             self.robot_state_path.base_orientation_path = []
+            self.robot_state_path.base_orientation_type = "rpy"
             # if the item is used as base position we do not remove the color
             if item_path != self.robot_state_path.base_position_path:
                 item.setBackground(0, QtGui.QBrush(deselected_base_color))

robot_log_visualizer/utils/utils.py

@@ -17,6 +17,7 @@ class RobotStatePath:
     def __init__(self):
         self.joints_state_path = []
         self.base_orientation_path = []
+        self.base_orientation_type = "rpy"  # "rpy", "xyzw", or "wxyz"
         self.base_position_path = []