fix(head): offset position on new robots received by hightorque

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

src/bitbots_motion/bitbots_hcm/config/default.yaml

@@ -0,0 +1,3 @@
+/hcm_cpp:
+  ros__parameters:
+    head_pitch_motor_offset_deg: 0.0

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

src/bitbots_motion/bitbots_hcm/launch/hcm.launch

@@ -10,6 +10,7 @@
             <param name="use_sim_time" value="$(var sim)" />
             <param name="simulation_active" value="$(var sim)" />
             <param name="visualization_active" value="$(var viz)" />
+            <param from="$(find-pkg-share bitbots_hcm)/config/$(env ROBOT_NAME default).yaml" />
         </node>
     </group>
 

src/bitbots_motion/bitbots_hcm/src/hcm.cpp

@@ -5,6 +5,7 @@
 #include <bitbots_msgs/msg/robot_control_state.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 #include <chrono>
+#include <cmath>
 #include <geometry_msgs/msg/point_stamped.hpp>
 #include <iostream>
 #include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
@@ -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<builtin_interfaces::msg::Time> last_walking_time_;
   std::optional<builtin_interfaces::msg::Time> last_significant_walk_motion_time_;
   std::optional<bitbots_msgs::msg::JointCommand> last_walk_command_;

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]
-