Merge branch 'feature/gpu_system_monitor' of github.com:bit-bots/bitb…

…ots_main into feature/gpu_system_monitor

bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py

@@ -2,17 +2,18 @@
 import math
 import os
 import sys
+from typing import Literal
 
 import rclpy
 from ament_index_python import get_package_share_directory
-from PyQt5.QtCore import Qt
+from PyQt5.QtCore import QLocale, Qt
 from PyQt5.QtGui import QKeySequence
 from PyQt5.QtWidgets import (
     QAbstractItemView,
+    QDoubleSpinBox,
     QFileDialog,
     QGroupBox,
     QLabel,
-    QLineEdit,
     QListWidgetItem,
     QMainWindow,
     QMessageBox,
@@ -84,8 +85,6 @@ def __init__(self, context):
         # Initialize the working values
         self._working_angles: dict[str, float] = {}
 
-        self.update_time = 0.1  # TODO what is this for?
-
         # QT directory for saving files
         self._save_directory = None
 
@@ -200,8 +199,8 @@ def q_joint_state_update(self, joint_states: JointState) -> None:
             # Check if the we are currently positioning the motor and want to store the value
             if motor_active and motor_torqueless:
                 # Update textfield
-                self._motor_controller_text_fields[motor_name].setText(
-                    str(round(math.degrees(joint_states.position[joint_states.name.index(motor_name)]), 2))
+                self._motor_controller_text_fields[motor_name].setValue(
+                    round(math.degrees(joint_states.position[joint_states.name.index(motor_name)]), 2)
                 )
                 # Update working values
                 self._working_angles[motor_name] = joint_states.position[joint_states.name.index(motor_name)]
@@ -224,9 +223,12 @@ def create_motor_controller(self) -> None:
             layout.addWidget(label)
 
             # Add a textfield to display the exact value of the motor
-            textfield = QLineEdit()
-            textfield.setText("0.0")
-            textfield.textEdited.connect(self.textfield_update)
+            textfield = QDoubleSpinBox()
+            textfield.setLocale(QLocale("C"))
+            textfield.setMaximum(180.0)
+            textfield.setMinimum(-180.0)
+            textfield.setValue(0.0)
+            textfield.valueChanged.connect(self.textfield_update)
             layout.addWidget(textfield)
             self._motor_controller_text_fields[motor_name] = textfield
 
@@ -299,8 +301,8 @@ def connect_gui_callbacks(self) -> None:
         self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until)
         self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate)
         self._widget.actionDelete_Frame.triggered.connect(self.delete)
-        self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("L"))
-        self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("R"))
+        self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("R"))
+        self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("L"))
         self._widget.actionInvert.triggered.connect(self.invert_frame)
         self._widget.actionUndo.triggered.connect(self.undo)
         self._widget.actionRedo.triggered.connect(self.redo)
@@ -520,6 +522,10 @@ def delete(self):
             self._node.get_logger().error(str(e))
             return
         assert self._recorder.get_keyframe(frame) is not None, "Selected frame not found in list of keyframes"
+        # Check if only one frame is remaining
+        if len(self._widget.frameList) == 1:
+            QMessageBox.warning(self._widget, "Warning", "Cannot delete last remaining frame")
+            return
         self._recorder.delete(frame)
         self._widget.statusBar.showMessage(f"Deleted frame {frame}")
         self.update_frames()
@@ -580,11 +586,32 @@ def redo(self):
         self._widget.statusBar.showMessage(status)
         self.update_frames()
 
-    def mirror_frame(self, direction):
+    def mirror_frame(self, source: Literal["L", "R"]) -> None:
         """
         Copies all motor values from one side of the robot to the other. Inverts values, if necessary
         """
-        raise NotImplementedError("This function is not implemented yet")
+        # Get direction to mirror to
+        mirrored_source = {"R": "L", "L": "R"}[source]
+
+        # Go through all active motors
+        for motor_name, angle in self._working_angles.items():
+            # Set mirrored angles
+            if motor_name.startswith(source):
+                mirrored_motor_name = mirrored_source + motor_name[1:]
+                # Make -0.0 to 0.0
+                mirrored_angle = -angle if angle != 0 else 0.0
+                self._working_angles[mirrored_motor_name] = mirrored_angle
+
+        # Update the UI
+        for motor_name, angle in self._working_angles.items():
+            # Block signals
+            self._motor_controller_text_fields[motor_name].blockSignals(True)
+            # Set values
+            self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2))
+            # Enable signals again
+            self._motor_controller_text_fields[motor_name].blockSignals(False)
+
+        self._widget.statusBar.showMessage("Mirrored frame")
 
     def invert_frame(self):
         """
@@ -613,7 +640,12 @@ def invert_frame(self):
 
         # Update the UI
         for motor_name, angle in self._working_angles.items():
-            self._motor_controller_text_fields[motor_name].setText(str(round(math.degrees(angle), 2)))
+            # Block signals
+            self._motor_controller_text_fields[motor_name].blockSignals(True)
+            # Set values
+            self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2))
+            # Enable signals again
+            self._motor_controller_text_fields[motor_name].blockSignals(False)
 
         self._widget.statusBar.showMessage("Inverted frame")
 
@@ -657,12 +689,13 @@ def react_to_frame_change(self):
 
             # Update the motor angle controls (value and active state)
             if active:
-                self._motor_controller_text_fields[motor_name].setText(
-                    str(round(math.degrees(selected_frame["goals"][motor_name]), 2))
+                self._motor_controller_text_fields[motor_name].setValue(
+                    round(math.degrees(selected_frame["goals"][motor_name]), 2)
                 )
+
                 self._working_angles[motor_name] = selected_frame["goals"][motor_name]
             else:
-                self._motor_controller_text_fields[motor_name].setText("0.0")
+                self._motor_controller_text_fields[motor_name].setValue(0.0)
 
         # Update the duration and pause
         self._widget.spinBoxDuration.setValue(selected_frame["duration"])
@@ -675,23 +708,12 @@ def textfield_update(self):
         If the textfield is updated, update working values
         """
         for motor_name, text_field in self._motor_controller_text_fields.items():
-            try:
-                # Get the angle from the textfield
-                angle = float(text_field.text())
-            except ValueError:
-                # Display QMessageBox stating that the value is not a number
-                QMessageBox.warning(
-                    self._widget,
-                    "Warning",
-                    f"Please enter a valid number.\n '{text_field.text()}' is not a valid number.",
-                )
-                continue
-            # Clip the angle to the maximum and minimum, we do this in degrees,
-            # because we do not want introduce rounding errors in the textfield
-            angle = round(max(-180.0, min(angle, 180.0)), 2)
+            # Get the angle from the textfield
+            angle = text_field.value()
+            angle = round(angle, 2)
             # Set the angle in the textfield
-            if float(text_field.text()) != float(angle):
-                text_field.setText(str(angle))
+            if text_field.value() != angle:
+                text_field.setValue(angle)
             # Set the angle in the working values if the motor is active
             if self._motor_switcher_active_checkbox[motor_name].checkState(0) == Qt.CheckState.Checked:
                 self._working_angles[motor_name] = math.radians(angle)

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml

@@ -75,23 +75,23 @@ walking:
       phase_reset:
         min_phase: 0.90
         foot_pressure:
-          active: False
+          active: True
           ground_min_pressure: 1.5
         effort:
           active: False
           joint_min_effort: 30.0
         imu:
-          active: True
+          active: False
           y_acceleration_threshold: 1.4
 
       trunk_pid:
         pitch:
-          p: 0.0
+          p: 0.0035
           i: 0.0
-          d: 0.0
+          d: 0.004
           i_clamp_min: 0.0
           i_clamp_max: 0.0
-          antiwindup: False
+          antiwindup: false
         roll:
           p: 0.0
           i: 0.0

bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py

@@ -645,6 +645,12 @@ def __init__(
 
             self.pub_pres_left = self.ros_node.create_publisher(FootPressure, base_ns + "foot_pressure_left/raw", 1)
             self.pub_pres_right = self.ros_node.create_publisher(FootPressure, base_ns + "foot_pressure_right/raw", 1)
+            self.pub_pres_left_filtered = self.ros_node.create_publisher(
+                FootPressure, base_ns + "foot_pressure_left/filtered", 1
+            )
+            self.pub_pres_right_filtered = self.ros_node.create_publisher(
+                FootPressure, base_ns + "foot_pressure_right/filtered", 1
+            )
             self.cop_l_pub_ = self.ros_node.create_publisher(PointStamped, base_ns + "cop_l", 1)
             self.cop_r_pub_ = self.ros_node.create_publisher(PointStamped, base_ns + "cop_r", 1)
             self.ros_node.create_subscription(JointCommand, base_ns + "DynamixelController/command", self.command_cb, 1)
@@ -1009,6 +1015,8 @@ def get_pressure_message(self):
     def publish_pressure(self):
         left, right, cop_l, cop_r = self.get_pressure_message()
         self.pub_pres_left.publish(left)
+        self.pub_pres_left_filtered.publish(left)
         self.pub_pres_right.publish(right)
+        self.pub_pres_right_filtered.publish(right)
         self.cop_l_pub_.publish(cop_l)
         self.cop_r_pub_.publish(cop_r)