From 2dd3e041e279780cc3678e1adf0c3c24162aa1ac Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Fri, 3 Jul 2026 18:42:13 +0200 Subject: [PATCH] fix(head): offset position on new robots received by hightorque MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit which have an offset in the 0° position in comparison to our previous robots. As we have never recalibrated any of the Pi+ motors before and also currently do not have the motor IDs to be able to only reset the head pitch joint we just add a configurable offset on `ROBOT_NAME` basis. If possible the underlying issue should definitely be rectified instead of relying on this. --- .../bitbots_hcm/config/carrie.yaml | 4 +++ .../bitbots_hcm/config/default.yaml | 3 ++ .../bitbots_hcm/config/peter.yaml | 4 +++ .../bitbots_hcm/launch/hcm.launch | 1 + src/bitbots_motion/bitbots_hcm/src/hcm.cpp | 32 ++++++++++++++++--- .../bitbots_head_mover/config/head_config.yml | 3 +- 6 files changed, 40 insertions(+), 7 deletions(-) create mode 100644 src/bitbots_motion/bitbots_hcm/config/carrie.yaml create mode 100644 src/bitbots_motion/bitbots_hcm/config/default.yaml create mode 100644 src/bitbots_motion/bitbots_hcm/config/peter.yaml diff --git a/src/bitbots_motion/bitbots_hcm/config/carrie.yaml b/src/bitbots_motion/bitbots_hcm/config/carrie.yaml new file mode 100644 index 000000000..38131f1f8 --- /dev/null +++ b/src/bitbots_motion/bitbots_hcm/config/carrie.yaml @@ -0,0 +1,4 @@ +/hcm_cpp: + ros__parameters: + # TODO: Remove this temporary hardware workaround after the affected head motor zero point is fixed. + head_pitch_motor_offset_deg: 10.0 diff --git a/src/bitbots_motion/bitbots_hcm/config/default.yaml b/src/bitbots_motion/bitbots_hcm/config/default.yaml new file mode 100644 index 000000000..517de7124 --- /dev/null +++ b/src/bitbots_motion/bitbots_hcm/config/default.yaml @@ -0,0 +1,3 @@ +/hcm_cpp: + ros__parameters: + head_pitch_motor_offset_deg: 0.0 diff --git a/src/bitbots_motion/bitbots_hcm/config/peter.yaml b/src/bitbots_motion/bitbots_hcm/config/peter.yaml new file mode 100644 index 000000000..38131f1f8 --- /dev/null +++ b/src/bitbots_motion/bitbots_hcm/config/peter.yaml @@ -0,0 +1,4 @@ +/hcm_cpp: + ros__parameters: + # TODO: Remove this temporary hardware workaround after the affected head motor zero point is fixed. + head_pitch_motor_offset_deg: 10.0 diff --git a/src/bitbots_motion/bitbots_hcm/launch/hcm.launch b/src/bitbots_motion/bitbots_hcm/launch/hcm.launch index 59db665bb..f2e54e59b 100644 --- a/src/bitbots_motion/bitbots_hcm/launch/hcm.launch +++ b/src/bitbots_motion/bitbots_hcm/launch/hcm.launch @@ -10,6 +10,7 @@ + diff --git a/src/bitbots_motion/bitbots_hcm/src/hcm.cpp b/src/bitbots_motion/bitbots_hcm/src/hcm.cpp index 383e48cbf..809911ccc 100644 --- a/src/bitbots_motion/bitbots_hcm/src/hcm.cpp +++ b/src/bitbots_motion/bitbots_hcm/src/hcm.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -30,6 +31,8 @@ class HCM_CPP : public rclcpp::Node { this->get_parameter("use_sim_time", use_sim_time); this->get_parameter("simulation_active", simulation_active); this->get_parameter("visualization_active", visualization_active); + this->get_parameter_or("head_pitch_motor_offset_deg", head_pitch_motor_offset_, 0.0); + head_pitch_motor_offset_ *= M_PI / 180.0; // Initialize HCM logic // Import Python module @@ -82,20 +85,20 @@ class HCM_CPP : public rclcpp::Node { current_state_ == bitbots_msgs::msg::RobotControlState::RECORD) or msg.from_hcm)) { // We can forward the animation goal to the motors - pub_controller_command_->publish(msg.joint_command); + publish_controller_command(msg.joint_command); } } void head_goal_callback(const bitbots_msgs::msg::JointCommand msg) { if (current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE || current_state_ == bitbots_msgs::msg::RobotControlState::WALKING) { - pub_controller_command_->publish(msg); + publish_controller_command(msg); } } void record_goal_callback(const bitbots_msgs::msg::JointCommand msg) { if (msg.joint_names.size() == 0 && current_state_ == bitbots_msgs::msg::RobotControlState::RECORD) { - pub_controller_command_->publish(msg); + publish_controller_command(msg); } } @@ -115,7 +118,7 @@ class HCM_CPP : public rclcpp::Node { if (current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE || current_state_ == bitbots_msgs::msg::RobotControlState::WALKING) { - pub_controller_command_->publish(msg); + publish_controller_command(msg); } } @@ -124,7 +127,7 @@ class HCM_CPP : public rclcpp::Node { // up. The state-based gate is the joint mutex that keeps the getup policy // from fighting walking/head/animation goals (which forward in other states). if (current_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP) { - pub_controller_command_->publish(msg); + publish_controller_command(msg); } } @@ -170,6 +173,24 @@ class HCM_CPP : public rclcpp::Node { } private: + void publish_controller_command(bitbots_msgs::msg::JointCommand msg) { + apply_head_pitch_motor_offset(msg); + pub_controller_command_->publish(msg); + } + + // TODO: Remove this temporary hardware workaround after the affected head motor zero point is fixed. + void apply_head_pitch_motor_offset(bitbots_msgs::msg::JointCommand& msg) { + if (head_pitch_motor_offset_ == 0.0 || msg.positions.empty()) { + return; + } + + for (size_t i = 0; i < msg.joint_names.size() && i < msg.positions.size(); i++) { + if (msg.joint_names[i] == "head_pitch_joint") { + msg.positions[i] += head_pitch_motor_offset_; + } + } + } + // Python interpreter py::scoped_interpreter python_; // Python hcm module @@ -183,6 +204,7 @@ class HCM_CPP : public rclcpp::Node { // Walking state double significant_motion_threshold_ = 0.5 * M_PI / 180.0; // default to 0.5 degrees in radians + double head_pitch_motor_offset_ = 0.0; std::optional last_walking_time_; std::optional last_significant_walk_motion_time_; std::optional last_walk_command_; diff --git a/src/bitbots_motion/bitbots_head_mover/config/head_config.yml b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml index 5d2b262dc..5e4250ea5 100644 --- a/src/bitbots_motion/bitbots_head_mover/config/head_config.yml +++ b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml @@ -24,7 +24,7 @@ move_head: max_pitch: type: double_array - default_value: [-1.23, 1.01] + default_value: [-1.23, 1.01] description: "Max values for the head position (in radians)" validation: fixed_size<>: 2 @@ -195,4 +195,3 @@ move_head: description: "Reduces last scanline by that factor so that robot does not collide" validation: bounds<>: [0.0, 1.0] -