Merge branch 'main' into refactor/walk_vizualizer

.devcontainer/Dockerfile

@@ -32,7 +32,6 @@ RUN apt-get update -y \
     tree \
     uvcdynctrl \
     vim \
-    vlc \
     wget \
     x11-apps \
     zsh

.vscode/settings.json

@@ -213,7 +213,8 @@
         "variant": "cpp",
         "regex": "cpp",
         "future": "cpp",
-        "*.ipp": "cpp"
+        "*.ipp": "cpp",
+        "span": "cpp"
     },
     // Tell the ROS extension where to find the setup.bash
     // This also utilizes the COLCON_WS environment variable, which needs to be set

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_dynup/CMakeLists.txt

@@ -11,11 +11,14 @@ set(PYBIND11_FINDPYTHON ON)
 
 find_package(ament_cmake REQUIRED)
 find_package(backward_ros REQUIRED)
+find_package(bio_ik REQUIRED)
 find_package(bitbots_msgs REQUIRED)
 find_package(bitbots_splines REQUIRED)
 find_package(bitbots_utils REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(control_toolbox REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -27,8 +30,6 @@ find_package(tf2 REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(generate_parameter_library REQUIRED)
 
 find_package(ros2_python_extension REQUIRED)
 find_package(pybind11 REQUIRED)
@@ -64,13 +65,14 @@ add_executable(DynupNode ${SOURCES})
 ament_target_dependencies(
   DynupNode
   ament_cmake
+  bio_ik
   bitbots_msgs
   bitbots_splines
   bitbots_utils
   control_msgs
   control_toolbox
-  geometry_msgs
   generate_parameter_library
+  geometry_msgs
   moveit_ros_planning_interface
   rclcpp
   ros2_python_extension
@@ -94,6 +96,7 @@ ament_target_dependencies(
   libpy_dynup
   PUBLIC
   ament_cmake
+  bio_ik
   bitbots_msgs
   bitbots_splines
   bitbots_utils

bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp

@@ -1,6 +1,7 @@
 #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_
 #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_
 
+#include <bio_ik/bio_ik.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit/robot_state/robot_state.h>
 #include <tf2/convert.h>

bitbots_motion/bitbots_dynup/src/dynup_ik.cpp

@@ -1,4 +1,5 @@
 #include <bitbots_dynup/dynup_ik.hpp>
+
 namespace bitbots_dynup {
 
 DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {}
@@ -41,7 +42,7 @@ void DynupIK::setDirection(DynupDirection direction) { direction_ = direction; }
 
 bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) {
   /* ik options is basically the command which we send to bio_ik and which describes what we want to do */
-  auto ik_options = kinematics::KinematicsQueryOptions();
+  kinematics::KinematicsQueryOptions ik_options;
   ik_options.return_approximate_solution = true;
 
   geometry_msgs::msg::Pose right_foot_goal_msg, left_foot_goal_msg, right_hand_goal_msg, left_hand_goal_msg;
@@ -54,13 +55,19 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) {
   bool success;
   goal_state_->updateLinkTransforms();
 
+  bio_ik::BioIKKinematicsQueryOptions leg_ik_options;
+  leg_ik_options.return_approximate_solution = true;
+
+  // Add auxiliary goal to prevent bending the knees in the wrong direction when we go from init to walkready
+  leg_ik_options.goals.push_back(std::make_unique<bio_ik::AvoidJointLimitsGoal>());
+
   success = goal_state_->setFromIK(l_leg_joints_group_, left_foot_goal_msg, 0.005,
-                                   moveit::core::GroupStateValidityCallbackFn(), ik_options);
+                                   moveit::core::GroupStateValidityCallbackFn(), leg_ik_options);
 
   goal_state_->updateLinkTransforms();
 
   success &= goal_state_->setFromIK(r_leg_joints_group_, right_foot_goal_msg, 0.005,
-                                    moveit::core::GroupStateValidityCallbackFn(), ik_options);
+                                    moveit::core::GroupStateValidityCallbackFn(), leg_ik_options);
 
   goal_state_->updateLinkTransforms();
 

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd

@@ -4,7 +4,7 @@
 -->HCM
 $StartHCM
     START_UP --> $Simulation
-        YES --> @RobotStateStartup, @PlayAnimationDynup + direction:walkready + r:false
+        YES --> @RobotStateStartup, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
         NO --> @RobotStateStartup, @Wait + time:1 + r:false, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
     RUNNING --> $CheckMotors
         MOTORS_NOT_STARTED --> @RobotStateStartup, @Wait

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml

@@ -15,7 +15,7 @@ walking:
       kick_rise_factor: 1.5
 
       double_support_ratio: 0.0264282002140171
-      first_step_swing_factor: 1.80591386587488
+      first_step_swing_factor: 2.9
       foot_distance: 0.179900277671633
       foot_rise: 0.0819786291304007
       freq: 1.2
@@ -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)