From 3b588035cfd64b5a5ba75657c751fa539721c4aa Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sun, 28 Jul 2024 16:43:41 +0200 Subject: [PATCH 01/12] Devcontainer: remove VLC, as it should not be necessary --- .devcontainer/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index f45a47088..d56d6ef9a 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -32,7 +32,6 @@ RUN apt-get update -y \ tree \ uvcdynctrl \ vim \ - vlc \ wget \ x11-apps \ zsh From 993922e2b650ebd939d1e7d0cb967ba5a63f0f79 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 6 Nov 2024 18:13:24 +0100 Subject: [PATCH 02/12] Adjusted joint limits in simulation to facilitate getting back up when Wolfgang lies on his back. Additionally he doesn't fall over at the start of the simulation --- .../protos/robots/Wolfgang/Wolfgang.proto | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto b/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto index 53ad73c84..e2d546987 100644 --- a/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto +++ b/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto @@ -483,6 +483,7 @@ PROTO Wolfgang [ RotationalMotor { name "RShoulderRoll" maxVelocity IS MX64-vel + minPosition -0.4 maxPosition 3.14159 maxTorque IS MX64-torque controlPID IS pid @@ -1041,6 +1042,7 @@ PROTO Wolfgang [ name "LShoulderRoll" maxVelocity IS MX64-vel minPosition -3.14159 + maxPosition 0.4 maxTorque IS MX64-torque controlPID IS pid } @@ -1135,7 +1137,7 @@ PROTO Wolfgang [ RotationalMotor { name "LElbow" maxVelocity IS MX64-vel - minPosition -1.5708 + minPosition -1.7 maxPosition 1.0472 maxTorque IS MX64-torque controlPID IS pid @@ -1735,6 +1737,7 @@ PROTO Wolfgang [ name "RKnee" maxVelocity IS XH540W270-vel minPosition -2.96706 + maxPosition 0.2 maxTorque IS XH540W270-torque controlPID IS pid } @@ -1826,7 +1829,7 @@ PROTO Wolfgang [ name "RAnklePitch" maxVelocity IS MX106-vel minPosition -1.74533 - maxPosition 1.22173 + maxPosition 1.45 maxTorque IS MX106-torque controlPID IS pid } @@ -3154,6 +3157,7 @@ PROTO Wolfgang [ RotationalMotor { name "LKnee" maxVelocity IS XH540W270-vel + minPosition -0.2 maxPosition 2.96706 maxTorque IS XH540W270-torque controlPID IS pid @@ -3245,7 +3249,7 @@ PROTO Wolfgang [ RotationalMotor { name "LAnklePitch" maxVelocity IS MX106-vel - minPosition -1.22173 + minPosition -1.45 maxPosition 1.74533 maxTorque IS MX106-torque controlPID IS pid From ef302f0433c7ae1d32cbac53de8bc2386c4b5c0b Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 7 Nov 2024 15:52:43 +0100 Subject: [PATCH 03/12] Fix walking startup in webots --- .../bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml index f893f55c4..4c38e7350 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml +++ b/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 From 3582054ef76b65544062c3291614cef903afa983 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 7 Nov 2024 16:59:26 +0100 Subject: [PATCH 04/12] fix initial walk ready transition in simulation --- .vscode/settings.json | 3 ++- bitbots_motion/bitbots_dynup/CMakeLists.txt | 9 ++++++--- bitbots_motion/bitbots_dynup/src/dynup_ik.cpp | 13 +++++++++++-- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 652c472fd..707c2f422 100644 --- a/.vscode/settings.json +++ b/.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 diff --git a/bitbots_motion/bitbots_dynup/CMakeLists.txt b/bitbots_motion/bitbots_dynup/CMakeLists.txt index bcea0170c..5760b5ce0 100644 --- a/bitbots_motion/bitbots_dynup/CMakeLists.txt +++ b/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 diff --git a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index abf2fb119..7997c1aea 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -1,4 +1,7 @@ +#include // TODO remove this include + #include + namespace bitbots_dynup { DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {} @@ -54,13 +57,19 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { bool success; goal_state_->updateLinkTransforms(); + // Add auxiliary goal for the knees to prevent bending in the wrong direction + bio_ik::BioIKKinematicsQueryOptions leg_ik_options; + leg_ik_options.return_approximate_solution = true; + + leg_ik_options.goals.push_back(std::make_unique()); + 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(); From aeba0ab22d450632982eb29094a77fac42fafd92 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 7 Nov 2024 17:55:27 +0100 Subject: [PATCH 05/12] Also play startup animation in simulator --- bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd index 7d61f6445..fb6cbccc9 100644 --- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd +++ b/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 From f75724e01d134f315b7f4ed8dafc4c1bfa2b2d8b Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 7 Nov 2024 18:08:17 +0100 Subject: [PATCH 06/12] Apply review and fix comment --- .../bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp | 1 + bitbots_motion/bitbots_dynup/src/dynup_ik.cpp | 6 ++---- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp index 2f948797f..50a8a6b3b 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp +++ b/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 #include #include #include diff --git a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index 7997c1aea..1e625bd4b 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -1,5 +1,3 @@ -#include // TODO remove this include - #include namespace bitbots_dynup { @@ -44,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; @@ -57,10 +55,10 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { bool success; goal_state_->updateLinkTransforms(); - // Add auxiliary goal for the knees to prevent bending in the wrong direction 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()); success = goal_state_->setFromIK(l_leg_joints_group_, left_foot_goal_msg, 0.005, From 8b20c3b3f6db22e8f127d3094f3b2028ef93d4e1 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Mon, 11 Nov 2024 21:36:02 +0100 Subject: [PATCH 07/12] Use foot pressure phase reset and PIDs in sim --- .../config/walking_wolfgang_simulator.yaml | 10 +++++----- .../bitbots_webots_sim/webots_robot_controller.py | 8 ++++++++ 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml index 4c38e7350..843c69281 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml +++ b/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 diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py index 0ba93ed38..925fb1db3 100644 --- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py +++ b/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) From bef7ca2c07897aab8c828caf2a65d8c44df30d3a Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 13 Nov 2024 16:09:55 +0100 Subject: [PATCH 08/12] Implemented mirror buttons in rqt --- .../bitbots_animation_rqt/record_ui.py | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py index 32a83bb96..97c221848 100755 --- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py +++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py @@ -2,6 +2,7 @@ import math import os import sys +from typing import Literal import rclpy from ament_index_python import get_package_share_directory @@ -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 @@ -299,8 +298,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) @@ -580,11 +579,27 @@ 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") + mirrored_source = {"R": "L", "L": "R"}[source] + + # Go through all active motors + for motor_name, angle in self._working_angles.items(): + # Check if the motor is on the right or left side and get the mirrored motor name + if motor_name.startswith(source): + mirrored_motor_name = mirrored_source + motor_name[1:] + if self._working_angles[motor_name] == 0.0: + self._working_angles[mirrored_motor_name] = angle + else: + self._working_angles[mirrored_motor_name] = -angle + + # 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))) + + self._widget.statusBar.showMessage("Mirrored frame") def invert_frame(self): """ From cbc06841ebd29407d0c7ea72c691fa3b999a3d84 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 13 Nov 2024 17:16:43 +0100 Subject: [PATCH 09/12] Changed textfields to QDoubleSpinBox to prevent unwanted inputs --- .../bitbots_animation_rqt/record_ui.py | 30 +++++++++++-------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py index 97c221848..76a7db558 100755 --- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py +++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py @@ -6,14 +6,14 @@ 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, @@ -223,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 @@ -597,7 +600,7 @@ def mirror_frame(self, source: Literal["L", "R"]) -> None: # 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))) + self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2)) self._widget.statusBar.showMessage("Mirrored frame") @@ -628,7 +631,7 @@ 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))) + self._motor_controller_text_fields[motor_name].setValue(round(math.degrees(angle), 2)) self._widget.statusBar.showMessage("Inverted frame") @@ -672,12 +675,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"]) @@ -692,7 +696,7 @@ def textfield_update(self): for motor_name, text_field in self._motor_controller_text_fields.items(): try: # Get the angle from the textfield - angle = float(text_field.text()) + angle = text_field.value() except ValueError: # Display QMessageBox stating that the value is not a number QMessageBox.warning( @@ -705,8 +709,8 @@ def textfield_update(self): # because we do not want introduce rounding errors in the textfield angle = round(max(-180.0, min(angle, 180.0)), 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) From 2a16634903cb0fedf253a67c25690e3467e721b4 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 13 Nov 2024 18:02:16 +0100 Subject: [PATCH 10/12] Fixed mirror error which came up because of the Qdoublespinbox --- .../bitbots_animation_rqt/record_ui.py | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py index 76a7db558..3dddba344 100755 --- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py +++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py @@ -199,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)] @@ -586,21 +586,26 @@ 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 """ + # 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(): - # Check if the motor is on the right or left side and get the mirrored motor name + # Set mirrored angles if motor_name.startswith(source): mirrored_motor_name = mirrored_source + motor_name[1:] - if self._working_angles[motor_name] == 0.0: - self._working_angles[mirrored_motor_name] = angle - else: - self._working_angles[mirrored_motor_name] = -angle + # 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") @@ -631,7 +636,12 @@ def invert_frame(self): # 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("Inverted frame") From 0804abb516c83a8f5a55f5704a5a97c1b4639863 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 13 Nov 2024 18:12:52 +0100 Subject: [PATCH 11/12] Fixed error when deleting last frame --- .../bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py index 3dddba344..364222592 100755 --- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py +++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py @@ -522,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() From ab98899b9dc5ef37f5b57cc8c894b1de84f88c21 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 13 Nov 2024 18:20:44 +0100 Subject: [PATCH 12/12] Code cleanup --- .../bitbots_animation_rqt/record_ui.py | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py index 364222592..5b3682ebf 100755 --- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py +++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py @@ -708,20 +708,9 @@ 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 = text_field.value() - 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 text_field.value() != angle: text_field.setValue(angle)