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