Hugging Face's logo

BAAI
/
PingPongRealWorldRL

Reinforcement Learning
ONNX
English
robotics
unitree-h2
table-tennis
Model card Files
xet
 Community
PingPongRealWorldRL / cpp_lowcmd_bridge /src /h2_lowcmd_shm_bridge.cpp
KurisiTOMA's picture
KurisiTOMA
chore: publish H2 real-world RL system
d8290eb verified
Raw History Blame Contribute Delete
44.4 kB
#include <algorithm>
#include <array>
#include <atomic>
#include <cctype>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <cstddef>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <fstream>
#include <iostream>
#include <limits>
#include <memory>
#include <mutex>
#include <net/if.h>
#include <sstream>
#include <stdexcept>
#include <string>
#include <thread>
#include <sys/file.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <unitree_hg/msg/low_cmd.hpp>
#include <unitree_hg/msg/low_state.hpp>
#include "cpp_lowcmd_bridge/gamepad.hpp"
#include "cpp_lowcmd_bridge/motor_crc_hg.hpp"
#include "cpp_lowcmd_bridge/timing_trace.hpp"
namespace {
using namespace std::chrono_literals;
constexpr char kShmName[] = "/h2_pingpong_bridge";
constexpr uint32_t kMagic = 0x48324252; // H2BR
// cmd_q carries the policy's finite raw learned desired target. The bridge
// retains that target and projects it only into the latest PD-effort interval
// at 1000 Hz, matching G1/MuJoCo actuator-effort saturation semantics.
constexpr uint32_t kVersion = 7;
constexpr char kJointMapRevision[] = "unitree_sdk2@e02dc701-h2-cpp";
constexpr int kHwDof = 31;
constexpr int kLowCmdDof = 35;
constexpr int kTtDataLen = 32;
constexpr int kTtProtocolLenV3 = 27;
constexpr int kWirelessRemoteLen = 40;
constexpr uint8_t kModePr = 0;
constexpr uint64_t kPublisherDiscoveryNs = 1000000000ULL;
constexpr uint64_t kPublisherRecheckNs = 1000000000ULL;
constexpr float kMinQuaternionNorm = 0.5F;
constexpr float kMaxQuaternionNorm = 1.5F;
constexpr float kMaxKp = 1000.0F;
constexpr float kMaxKd = 100.0F;
constexpr uint8_t kPythonFaultRequest = 0x01U;
constexpr uint8_t kMotionSwitcherAttested = 0x02U;
constexpr float kJointMargin = 0.01F;
constexpr float kStateLimitTolerance = 0.05F;
constexpr float kMaxJointVelocity = 40.0F;
constexpr float kHoldTorqueFraction = 0.90F;
constexpr std::array<float, kHwDof> kJointLo = {
-2.4526F, -0.467441F, -2.827F, -0.08725F, -0.349066F, -1.13446F,
-2.4526F, -2.16886F, -2.827F, -0.08725F, -0.296706F, -1.13446F,
-1.7453F, -0.5236F, -0.43633F, -2.618F, -0.517F, -2.618F, -0.986F,
-2.618F, -0.576F, -1.22F, -2.618F, -2.494F, -2.618F, -0.986F,
-2.618F, -0.576F, -1.22F, -0.5236F, -1.7453F};
constexpr std::array<float, kHwDof> kJointHi = {
2.77542F, 2.16886F, 2.827F, 2.53025F, 0.296706F, 0.610865F,
2.77542F, 0.467441F, 2.827F, 2.53025F, 0.349066F, 0.610865F,
1.7453F, 0.5236F, 0.5236F, 1.833F, 2.494F, 2.618F, 3.071F,
2.618F, 0.576F, 1.22F, 1.833F, 0.517F, 2.618F, 3.071F,
2.618F, 0.576F, 1.22F, 0.83775F, 1.7453F};
constexpr std::array<float, kHwDof> kTorqueLimit = {
360.0F, 360.0F, 360.0F, 360.0F, 19.0F, 66.88F,
360.0F, 360.0F, 360.0F, 360.0F, 19.0F, 66.88F,
120.0F, 180.0F, 180.0F, 130.0F, 60.0F, 60.0F, 60.0F,
60.0F, 10.0F, 10.0F, 130.0F, 60.0F, 60.0F, 60.0F,
60.0F, 10.0F, 10.0F, 50.0F, 50.0F};
#pragma pack(push, 8)
struct BridgeMemory {
uint32_t magic;
uint32_t version;
uint64_t state_seq;
uint64_t command_seq;
uint64_t state_stamp_ns;
uint64_t command_stamp_ns;
uint64_t tt_stamp_ns;
uint8_t robot_connected;
uint8_t state_ready;
uint8_t tt_ready;
uint8_t start_requested;
uint8_t stop_requested;
uint8_t emergency_stop;
uint8_t command_enabled;
uint8_t reserved_flags;
uint8_t state_mode_pr;
uint8_t mode_machine;
uint8_t cmd_mode_pr;
uint8_t pad0;
uint32_t bridge_interface_hash;
float joint_pos[kHwDof];
float joint_vel[kHwDof];
float imu_quat_wxyz[4];
float gyro[3];
float root_current_xy[2];
float mocap_pelvis_quat_xyzw[4];
double tt_data[kTtDataLen];
uint8_t wireless_remote[kWirelessRemoteLen];
float cmd_q[kLowCmdDof];
float cmd_dq[kLowCmdDof];
float cmd_tau[kLowCmdDof];
float cmd_kp[kLowCmdDof];
float cmd_kd[kLowCmdDof];
uint8_t cmd_mode[kLowCmdDof];
uint32_t cmd_reserve[kLowCmdDof];
};
#pragma pack(pop)
static_assert(sizeof(BridgeMemory) == 1544, "Python/C++ H2 shared-memory ABI mismatch");
static_assert(offsetof(BridgeMemory, command_seq) == 16, "H2 ABI command_seq offset mismatch");
static_assert(
offsetof(BridgeMemory, bridge_interface_hash) == 60,
"H2 ABI bridge_interface_hash offset mismatch");
static_assert(offsetof(BridgeMemory, joint_pos) == 64, "H2 ABI joint_pos offset mismatch");
static_assert(offsetof(BridgeMemory, tt_data) == 368, "H2 ABI tt_data offset mismatch");
static_assert(offsetof(BridgeMemory, cmd_q) == 664, "H2 ABI cmd_q offset mismatch");
static_assert(offsetof(BridgeMemory, cmd_mode) == 1364, "H2 ABI cmd_mode offset mismatch");
static_assert(offsetof(BridgeMemory, cmd_reserve) == 1400, "H2 ABI cmd_reserve offset mismatch");
uint64_t NowNs() {
return h2_timing::NowNs();
}
uint32_t InterfaceHash(const std::string& interface) {
uint32_t value = 2166136261U;
for (const unsigned char byte : interface) {
value ^= static_cast<uint32_t>(byte);
value *= 16777619U;
}
if (value == 0U) throw std::runtime_error("network interface hash unexpectedly equals zero");
return value;
}
struct Args {
// ROS 2 adds its own DDS "rt/" prefix. The H2 wire topics are
// rt/lowcmd and rt/lowstate, so the ROS names must be /lowcmd and
// /lowstate rather than /rt/lowcmd and /rt/lowstate.
std::string lowcmd_topic = "/lowcmd";
std::string lowstate_topic = "/lowstate";
std::string tt_topic = "/wbc_racket_command";
std::string network_interface;
double publish_hz = 1000.0;
double command_timeout_s = 0.10;
double state_timeout_s = 0.10;
bool replace_stale_shm = false;
bool print_contract = false;
bool validate_network_only = false;
};
Args ParseArgs(int argc, char** argv) {
Args args;
for (int index = 1; index < argc; ++index) {
const std::string key(argv[index]);
auto value = [&]() -> std::string {
if (++index >= argc) throw std::runtime_error("missing value for " + key);
return argv[index];
};
if (key == "--lowcmd-topic") args.lowcmd_topic = value();
else if (key == "--lowstate-topic") args.lowstate_topic = value();
else if (key == "--tt-topic") args.tt_topic = value();
else if (key == "--network-interface") args.network_interface = value();
else if (key == "--publish-hz") args.publish_hz = std::stod(value());
else if (key == "--command-timeout-s") args.command_timeout_s = std::stod(value());
else if (key == "--state-timeout-s") args.state_timeout_s = std::stod(value());
else if (key == "--replace-stale-shm") args.replace_stale_shm = true;
else if (key == "--print-contract") args.print_contract = true;
else if (key == "--validate-network-only") args.validate_network_only = true;
else if (key == "--help" || key == "-h") {
std::cout
<< "h2_lowcmd_shm_bridge [--lowcmd-topic /lowcmd] [--lowstate-topic /lowstate]\n"
<< " --network-interface <H2-interface> [--tt-topic /wbc_racket_command]\n"
<< " [--publish-hz 1000]\n"
<< " [--command-timeout-s 0.10] [--state-timeout-s 0.10] [--replace-stale-shm]\n"
<< " [--print-contract] [--validate-network-only]\n";
std::exit(0);
} else {
throw std::runtime_error("unknown argument: " + key);
}
}
if (!(args.publish_hz > 0.0 && args.publish_hz <= 1000.0)) {
throw std::runtime_error("publish rate must be in (0,1000] Hz");
}
if (!(args.command_timeout_s >= 0.02 && args.command_timeout_s <= 0.10)) {
throw std::runtime_error("command timeout must be in [0.02,0.10] seconds");
}
if (!(args.state_timeout_s >= 0.02 && args.state_timeout_s <= 0.10)) {
throw std::runtime_error("state timeout must be in [0.02,0.10] seconds");
}
if (!args.print_contract && args.network_interface.empty()) {
throw std::runtime_error("--network-interface is required for live bridge startup");
}
return args;
}
void ValidateNetworkEnvironment(const std::string& interface) {
if (interface.empty() || interface.size() >= IFNAMSIZ ||
!std::all_of(interface.begin(), interface.end(), [](unsigned char character) {
return std::isalnum(character) != 0 || character == '_' || character == '-' ||
character == '.';
})) {
throw std::runtime_error("invalid --network-interface value");
}
if (if_nametoindex(interface.c_str()) == 0U) {
throw std::runtime_error("network interface does not exist: " + interface);
}
std::ifstream state_file("/sys/class/net/" + interface + "/operstate");
std::string state;
state_file >> state;
if (!state_file || state != "up") {
throw std::runtime_error("network interface is not UP: " + interface);
}
const char* rmw = std::getenv("RMW_IMPLEMENTATION");
if (rmw == nullptr || std::string(rmw) != "rmw_cyclonedds_cpp") {
throw std::runtime_error("RMW_IMPLEMENTATION must be exactly rmw_cyclonedds_cpp");
}
const std::string expected_uri =
"<CycloneDDS><Domain><General><Interfaces><NetworkInterface name='" + interface +
"' priority='default' multicast='default'/></Interfaces></General></Domain></CycloneDDS>";
const char* uri = std::getenv("CYCLONEDDS_URI");
if (uri == nullptr || std::string(uri) != expected_uri) {
throw std::runtime_error(
"CYCLONEDDS_URI does not exactly bind the declared interface; source this project's "
"scripts/source_unitree_env.sh first");
}
}
class FileLock {
public:
FileLock(int fd, int operation) : fd_(fd) {
int result;
do {
result = flock(fd_, operation | LOCK_NB);
} while (result != 0 && errno == EINTR);
locked_ = result == 0;
}
~FileLock() {
if (!locked_) return;
int result;
do {
result = flock(fd_, LOCK_UN);
} while (result != 0 && errno == EINTR);
}
FileLock(const FileLock&) = delete;
FileLock& operator=(const FileLock&) = delete;
bool locked() const { return locked_; }
private:
int fd_;
bool locked_{false};
};
class SharedMemoryOwner {
public:
SharedMemoryOwner(bool replace_stale, uint32_t interface_hash) {
if (replace_stale) shm_unlink(kShmName);
fd_ = shm_open(kShmName, O_CREAT | O_EXCL | O_RDWR, 0660);
if (fd_ < 0) {
throw std::runtime_error(
"cannot create /dev/shm/h2_pingpong_bridge (another bridge or stale segment exists; "
"inspect it, then use --replace-stale-shm only when no bridge is running)");
}
if (ftruncate(fd_, sizeof(BridgeMemory)) != 0) {
throw std::runtime_error("cannot size H2 shared memory");
}
void* ptr = mmap(nullptr, sizeof(BridgeMemory), PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
if (ptr == MAP_FAILED) throw std::runtime_error("mmap failed for H2 shared memory");
mem_ = static_cast<BridgeMemory*>(ptr);
std::memset(mem_, 0, sizeof(BridgeMemory));
mem_->magic = kMagic;
mem_->version = kVersion;
mem_->bridge_interface_hash = interface_hash;
mem_->state_mode_pr = std::numeric_limits<uint8_t>::max();
mem_->cmd_mode_pr = kModePr;
mem_->imu_quat_wxyz[0] = 1.0F;
mem_->mocap_pelvis_quat_xyzw[3] = 1.0F;
}
~SharedMemoryOwner() {
if (mem_ != nullptr) munmap(mem_, sizeof(BridgeMemory));
if (fd_ >= 0) close(fd_);
shm_unlink(kShmName);
}
BridgeMemory* get() { return mem_; }
int fd() const { return fd_; }
private:
int fd_{-1};
BridgeMemory* mem_{nullptr};
};
struct Command {
uint64_t sequence{0};
uint64_t stamp_ns{0};
uint8_t mode_pr{kModePr};
std::array<float, kLowCmdDof> q{};
std::array<float, kLowCmdDof> dq{};
std::array<float, kLowCmdDof> tau{};
std::array<float, kLowCmdDof> kp{};
std::array<float, kLowCmdDof> kd{};
std::array<uint8_t, kLowCmdDof> mode{};
std::array<uint32_t, kLowCmdDof> reserve{};
};
enum class PublisherTopology : uint8_t {
kDiscovering,
kReady,
kBlocked,
};
struct StateSnapshot {
std::array<float, kHwDof> q{};
std::array<float, kHwDof> dq{};
std::array<float, kHwDof> watchdog_q{};
uint8_t mode_pr{std::numeric_limits<uint8_t>::max()};
uint8_t mode_machine{0};
uint64_t stamp_ns{0};
bool got_state{false};
bool have_valid_state{false};
bool usable{false};
bool output_inhibited{false};
bool hold_available{false};
};
class H2BridgeNode final : public rclcpp::Node {
public:
H2BridgeNode(const Args& args, BridgeMemory* mem, int shm_fd)
: Node("h2_lowcmd_shm_bridge"),
args_(args),
mem_(mem),
shm_fd_(shm_fd) {
lowstate_sub_ = create_subscription<unitree_hg::msg::LowState>(
args_.lowstate_topic, rclcpp::QoS(10),
[this](const unitree_hg::msg::LowState::SharedPtr msg) { OnLowState(msg); });
planner_sub_ = create_subscription<std_msgs::msg::Float64MultiArray>(
args_.tt_topic, rclcpp::QoS(1),
[this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) { OnPlanner(msg); });
lowcmd_pub_ = create_publisher<unitree_hg::msg::LowCmd>(args_.lowcmd_topic, rclcpp::QoS(10));
RCLCPP_INFO(
get_logger(),
"waiting for H2 LowState; bridge publishes nothing until a valid Python command arrives");
}
~H2BridgeNode() override { StopWorkers(); }
void StartWorkers() {
bool expected = false;
if (!workers_running_.compare_exchange_strong(expected, true)) return;
try {
command_thread_ = std::thread([this]() { CommandLoop(); });
topology_thread_ = std::thread([this]() { TopologyLoop(); });
} catch (...) {
StopWorkers();
throw;
}
}
void StopWorkers() noexcept {
workers_running_.store(false);
worker_cv_.notify_all();
if (command_thread_.joinable()) command_thread_.join();
if (topology_thread_.joinable()) topology_thread_.join();
}
private:
void OnPlanner(const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
const size_t count = std::min(msg->data.size(), static_cast<size_t>(kTtDataLen));
std::lock_guard<std::mutex> lock(planner_mutex_);
planner_data_.fill(0.0);
const bool finite = std::all_of(
msg->data.begin(), msg->data.begin() + count,
[](double value) { return std::isfinite(value); });
if (finite) std::copy_n(msg->data.begin(), count, planner_data_.begin());
planner_ready_ =
finite && msg->data.size() == kTtProtocolLenV3 && msg->data[20] == 3.0 && msg->data[21] > 0.0 &&
msg->data[22] > 0.0 && msg->data[26] == 1.0;
planner_stamp_ns_ = NowNs();
}
static bool LowStateFinite(
const unitree_hg::msg::LowState& msg, std::array<float, kHwDof>* q,
std::array<float, kHwDof>* dq, std::array<float, 4>* quaternion,
std::array<float, 3>* gyro) {
if (!low_state_crc_valid(msg)) return false;
for (int index = 0; index < kLowCmdDof; ++index) {
const auto& motor = msg.motor_state.at(index);
if (!std::isfinite(motor.q) || !std::isfinite(motor.dq) || !std::isfinite(motor.ddq) ||
!std::isfinite(motor.tau_est) || !std::isfinite(motor.vol)) {
return false;
}
if (index < kHwDof) {
if (motor.q < kJointLo[index] - kStateLimitTolerance ||
motor.q > kJointHi[index] + kStateLimitTolerance ||
std::abs(motor.dq) > kMaxJointVelocity) {
return false;
}
q->at(index) = motor.q;
dq->at(index) = motor.dq;
}
}
float norm_squared = 0.0F;
for (int index = 0; index < 4; ++index) {
const float value = msg.imu_state.quaternion.at(index);
if (!std::isfinite(value)) return false;
quaternion->at(index) = value;
norm_squared += value * value;
}
const float norm = std::sqrt(norm_squared);
if (!std::isfinite(norm) || norm < kMinQuaternionNorm || norm > kMaxQuaternionNorm) {
return false;
}
for (int index = 0; index < 3; ++index) {
const float gyro_value = msg.imu_state.gyroscope.at(index);
if (!std::isfinite(gyro_value) || !std::isfinite(msg.imu_state.accelerometer.at(index)) ||
!std::isfinite(msg.imu_state.rpy.at(index))) {
return false;
}
gyro->at(index) = gyro_value;
}
return true;
}
void SyncLatchedFlagsLocked() {
if (start_request_latched_.load()) mem_->start_requested = 1;
if (stop_request_latched_.load()) mem_->stop_requested = 1;
if (fault_latched_.load()) mem_->emergency_stop = 1;
}
void SyncLatchedFlags() {
if (!latched_flags_dirty_.exchange(false)) return;
// flock() locks belong to an open-file description. Every use of the
// shared fd is therefore serialized inside this process before taking the
// cross-process lock; otherwise two C++ threads could convert or release
// each other's flock.
std::lock_guard<std::mutex> local_lock(shm_mutex_);
FileLock lock(shm_fd_, LOCK_EX);
if (lock.locked()) {
SyncLatchedFlagsLocked();
} else {
latched_flags_dirty_.store(true);
}
}
void LatchFault(
const std::string& reason, bool stop_requested = false,
bool invalidate_state = false, bool inhibit_output = false,
int observed_mode_pr = -1) {
bool first_fault = false;
{
// This mutex is the publication linearization point. Once a fault owns
// it, no later learned LowCmd can pass the final gate.
std::lock_guard<std::mutex> output_lock(output_mutex_);
if (stop_requested && !stop_request_latched_.exchange(true)) {
latched_flags_dirty_.store(true);
}
bool expected = false;
if (fault_latched_.compare_exchange_strong(expected, true)) {
first_fault = true;
latched_flags_dirty_.store(true);
}
std::lock_guard<std::mutex> state_lock(state_mutex_);
if (invalidate_state) latest_state_usable_ = false;
if (inhibit_output) output_inhibited_ = true;
if (observed_mode_pr >= 0) {
latest_mode_pr_ = static_cast<uint8_t>(observed_mode_pr);
}
if (first_fault && have_valid_state_) {
watchdog_q_ = latest_q_;
hold_available_ = true;
}
}
if (first_fault) {
RCLCPP_ERROR(
get_logger(), "%s; output is permanently latched until bridge restart", reason.c_str());
}
SyncLatchedFlags();
}
void OnLowState(const unitree_hg::msg::LowState::SharedPtr msg) {
if (msg->mode_pr != kModePr) {
// Ankle values in AB mode do not have PR roll/pitch semantics. Never
// capture them as a future PR hold, even if the robot later returns to
// PR mode during this bridge process.
LatchFault(
"LowState left PR(0) mode (mode_pr=" + std::to_string(msg->mode_pr) + ")",
false, true, true, msg->mode_pr);
return;
}
std::array<float, kHwDof> q{};
std::array<float, kHwDof> dq{};
std::array<float, 4> quaternion{};
std::array<float, 3> gyro{};
if (!LowStateFinite(*msg, &q, &dq, &quaternion, &gyro)) {
LatchFault(
"CRC-invalid, non-finite, or invalid-quaternion H2 LowState",
false, true);
return;
}
for (int index = 0; index < kHwDof; ++index) {
if (msg->motor_state.at(index).motorstate != 0U) {
LatchFault(
"motor " + std::to_string(index) + " fault code " +
std::to_string(msg->motor_state.at(index).motorstate),
false, true);
return;
}
}
unitree::common::REMOTE_DATA_RX remote{};
std::memcpy(remote.buff, msg->wireless_remote.data(), kWirelessRemoteLen);
gamepad_.update(remote.RF_RX);
if (gamepad_.R1.on_press) {
LatchFault("R1 stop request", true);
return;
}
// Only a fully validated LowState may replace the last-known-valid state.
const uint64_t state_stamp_ns = NowNs();
{
std::lock_guard<std::mutex> state_lock(state_mutex_);
latest_q_ = q;
latest_dq_ = dq;
latest_mode_pr_ = msg->mode_pr;
latest_mode_machine_ = msg->mode_machine;
latest_state_stamp_ns_ = state_stamp_ns;
got_state_ = true;
have_valid_state_ = true;
latest_state_usable_ = true;
}
if (gamepad_.L1.on_press) {
if (!start_request_latched_.exchange(true)) {
latched_flags_dirty_.store(true);
RCLCPP_INFO(get_logger(), "L1 start request latched");
}
}
std::array<double, kTtDataLen> planner{};
bool planner_ready = false;
uint64_t planner_stamp = 0;
{
std::lock_guard<std::mutex> lock(planner_mutex_);
planner = planner_data_;
planner_ready = planner_ready_;
planner_stamp = planner_stamp_ns_;
}
// Never let command-side SHM access back-pressure the ROS LowState
// callback. The validated internal state above remains current for the
// watchdog; a missed mirror write is retried by the next ~1 kHz sample.
std::unique_lock<std::mutex> local_lock(shm_mutex_, std::try_to_lock);
if (!local_lock.owns_lock()) return;
FileLock lock(shm_fd_, LOCK_EX);
if (!lock.locked()) return;
uint64_t seq = mem_->state_seq;
if (seq & 1U) ++seq;
mem_->state_stamp_ns = state_stamp_ns;
mem_->robot_connected = 1;
mem_->state_ready = 1;
mem_->state_mode_pr = msg->mode_pr;
mem_->mode_machine = msg->mode_machine;
for (int index = 0; index < kHwDof; ++index) {
mem_->joint_pos[index] = q[index];
mem_->joint_vel[index] = dq[index];
}
for (int index = 0; index < 4; ++index) {
mem_->imu_quat_wxyz[index] = quaternion[index];
}
for (int index = 0; index < 3; ++index) {
mem_->gyro[index] = gyro[index];
}
std::copy(msg->wireless_remote.begin(), msg->wireless_remote.end(), mem_->wireless_remote);
std::copy(planner.begin(), planner.end(), mem_->tt_data);
mem_->tt_ready = planner_ready ? 1 : 0;
mem_->tt_stamp_ns = planner_stamp;
if (planner_ready) {
mem_->root_current_xy[0] = static_cast<float>(planner[0]);
mem_->root_current_xy[1] = static_cast<float>(planner[1]);
for (int index = 0; index < 4; ++index) {
mem_->mocap_pelvis_quat_xyzw[index] = static_cast<float>(planner[3 + index]);
}
}
SyncLatchedFlagsLocked();
mem_->state_seq = seq + 2;
}
bool ReadCommand(Command* result) {
bool enabled = false;
bool motion_switcher_attested = false;
Command next;
{
std::lock_guard<std::mutex> local_lock(shm_mutex_);
FileLock lock(shm_fd_, LOCK_SH);
if (!lock.locked()) {
++command_lock_miss_streak_;
max_command_lock_miss_streak_ =
std::max(max_command_lock_miss_streak_, command_lock_miss_streak_);
return false;
}
command_lock_miss_streak_ = 0;
enabled = mem_->command_enabled != 0;
if (!enabled) return false;
motion_switcher_attested =
(mem_->reserved_flags & kMotionSwitcherAttested) != 0U;
next.sequence = mem_->command_seq;
next.stamp_ns = mem_->command_stamp_ns;
next.mode_pr = mem_->cmd_mode_pr;
std::copy(std::begin(mem_->cmd_q), std::end(mem_->cmd_q), next.q.begin());
std::copy(std::begin(mem_->cmd_dq), std::end(mem_->cmd_dq), next.dq.begin());
std::copy(std::begin(mem_->cmd_tau), std::end(mem_->cmd_tau), next.tau.begin());
std::copy(std::begin(mem_->cmd_kp), std::end(mem_->cmd_kp), next.kp.begin());
std::copy(std::begin(mem_->cmd_kd), std::end(mem_->cmd_kd), next.kd.begin());
std::copy(std::begin(mem_->cmd_mode), std::end(mem_->cmd_mode), next.mode.begin());
std::copy(std::begin(mem_->cmd_reserve), std::end(mem_->cmd_reserve), next.reserve.begin());
}
if (!motion_switcher_attested) {
LatchFault("Python command lacks a validated MotionSwitcher release attestation");
return false;
}
if (!PythonCommandValid(next)) {
LatchFault("invalid/non-PR Python command or non-zero raw head target");
return false;
}
Command projected;
bool adjusted = false;
if (!ConstrainCommandForState(next, SnapshotState(), &projected, &adjusted)) {
LatchFault("Python command has no effort-feasible projection");
return false;
}
// Retain the raw learned desired target rather than the projection from
// this one state sample. OnCommandTimer projects it again from the newest
// validated q/dq immediately before publication.
*result = next;
return true;
}
bool PythonFaultRequested() {
std::lock_guard<std::mutex> local_lock(shm_mutex_);
FileLock lock(shm_fd_, LOCK_SH);
return lock.locked() && (mem_->reserved_flags & kPythonFaultRequest) != 0U;
}
StateSnapshot SnapshotState() const {
std::lock_guard<std::mutex> state_lock(state_mutex_);
return SnapshotStateLocked();
}
StateSnapshot SnapshotStateLocked() const {
StateSnapshot state;
state.q = latest_q_;
state.dq = latest_dq_;
state.watchdog_q = watchdog_q_;
state.mode_pr = latest_mode_pr_;
state.mode_machine = latest_mode_machine_;
state.stamp_ns = latest_state_stamp_ns_;
state.got_state = got_state_;
state.have_valid_state = have_valid_state_;
state.usable = latest_state_usable_;
state.output_inhibited = output_inhibited_;
state.hold_available = hold_available_;
return state;
}
static bool CommandStructureValid(const Command& command) {
if (command.mode_pr != kModePr) return false;
for (int index = 0; index < kLowCmdDof; ++index) {
if (!std::isfinite(command.q[index]) || !std::isfinite(command.dq[index]) ||
!std::isfinite(command.tau[index]) || !std::isfinite(command.kp[index]) ||
!std::isfinite(command.kd[index]) || command.kp[index] < 0.0F ||
command.kd[index] < 0.0F || command.kp[index] > kMaxKp ||
command.kd[index] > kMaxKd) {
return false;
}
const uint8_t expected_mode = index < kHwDof ? 1U : 0U;
if (command.mode[index] != expected_mode || command.reserve[index] != 0U) return false;
if (index >= kHwDof &&
(command.q[index] != 0.0F || command.dq[index] != 0.0F ||
command.tau[index] != 0.0F || command.kp[index] != 0.0F ||
command.kd[index] != 0.0F)) {
return false;
}
}
return true;
}
static bool PythonCommandValid(const Command& command) {
if (!CommandStructureValid(command)) return false;
// The policy still observes/outputs all 31 actions, but Python locks both
// raw learned head targets to zero. The 1000 Hz effort projection may move
// the published q_des away from zero when doing so is required to saturate
// damping effort, exactly as it may for every other active joint.
return command.q[29] == 0.0F && command.q[30] == 0.0F;
}
static bool CommandSafeForState(
const Command& command, const StateSnapshot& state) {
if (!state.have_valid_state) return false;
for (int index = 0; index < kHwDof; ++index) {
const float estimated_effort =
command.tau[index] + command.kp[index] * (command.q[index] - state.q[index]) +
command.kd[index] * (command.dq[index] - state.dq[index]);
if (!std::isfinite(estimated_effort) ||
std::abs(estimated_effort) > kTorqueLimit[index] + 1.0e-3F) {
return false;
}
}
return true;
}
static bool ConstrainCommandForState(
const Command& input, const StateSnapshot& state, Command* result,
bool* adjusted) {
if (!state.have_valid_state || result == nullptr || adjusted == nullptr) return false;
Command projected = input;
*adjusted = false;
for (int index = 0; index < kHwDof; ++index) {
const float kp = input.kp[index];
// Unitree PR effort is
// tau_ff + kp * (q_des-q) + kd * (dq_des-dq).
// Hold every term except q_des fixed and solve -limit <= effort <= limit.
const float offset =
input.tau[index] + input.kd[index] * (input.dq[index] - state.dq[index]);
float target = input.q[index];
if (kp <= 1.0e-8F) {
if (!std::isfinite(offset) || std::abs(offset) > kTorqueLimit[index] + 1.0e-3F) {
return false;
}
} else {
const float a = state.q[index] + (-kTorqueLimit[index] - offset) / kp;
const float b = state.q[index] + (kTorqueLimit[index] - offset) / kp;
const float feasible_lo = std::min(a, b);
const float feasible_hi = std::max(a, b);
if (!std::isfinite(feasible_lo) || !std::isfinite(feasible_hi) ||
feasible_lo > feasible_hi) {
return false;
}
target = std::clamp(input.q[index], feasible_lo, feasible_hi);
}
if (std::abs(target - input.q[index]) > 1.0e-7F) *adjusted = true;
projected.q[index] = target;
}
if (!CommandStructureValid(projected) || !CommandSafeForState(projected, state)) return false;
*result = projected;
return true;
}
bool StateFresh(const StateSnapshot& state, uint64_t now_ns) const {
return !state.output_inhibited && state.got_state && state.usable &&
state.stamp_ns != 0U && state.stamp_ns <= now_ns &&
now_ns - state.stamp_ns <=
static_cast<uint64_t>(args_.state_timeout_s * 1.0e9);
}
static Command LatchedHold(
const Command& basis, const StateSnapshot& state) {
Command hold = basis;
hold.mode_pr = kModePr;
for (int index = 0; index < kLowCmdDof; ++index) {
hold.mode[index] = index < kHwDof ? 1U : 0U;
hold.q[index] = index < kHwDof
? std::clamp(
state.watchdog_q[index], kJointLo[index] + kJointMargin,
kJointHi[index] - kJointMargin)
: 0.0F;
hold.dq[index] = 0.0F;
hold.tau[index] = 0.0F;
if (index < kHwDof) {
// Reduce, never increase, the last validated gains so the captured
// position hold is inside the MJCF effort envelope at this state.
const float budget = kHoldTorqueFraction * kTorqueLimit[index];
const float position_error = hold.q[index] - state.q[index];
const float abs_error = std::abs(position_error);
if (abs_error > 1.0e-6F && hold.kp[index] * abs_error > budget) {
hold.kp[index] = budget / abs_error;
}
const float position_effort = std::abs(hold.kp[index] * position_error);
const float damping_budget = std::max(0.0F, budget - position_effort);
const float abs_velocity = std::abs(state.dq[index]);
if (abs_velocity > 1.0e-6F && hold.kd[index] * abs_velocity > damping_budget) {
hold.kd[index] = damping_budget / abs_velocity;
}
} else {
hold.kp[index] = 0.0F;
hold.kd[index] = 0.0F;
}
hold.reserve[index] = 0U;
}
return hold;
}
bool PublishValidated(
const Command& command, const StateSnapshot& state, const Command* raw = nullptr) {
if (!CommandStructureValid(command) || !CommandSafeForState(command, state)) return false;
unitree_hg::msg::LowCmd message{};
message.mode_pr = kModePr;
message.mode_machine = state.mode_machine;
for (int index = 0; index < kLowCmdDof; ++index) {
auto& motor = message.motor_cmd.at(index);
motor.mode = command.mode[index];
motor.q = command.q[index];
motor.dq = command.dq[index];
motor.tau = command.tau[index];
motor.kp = command.kp[index];
motor.kd = command.kd[index];
motor.reserve = command.reserve[index];
}
get_crc(message);
if (timing_.enabled()) {
timing_row_.command_seq = command.sequence;
timing_row_.command_stamp_ns = command.stamp_ns;
timing_row_.state_stamp_ns = state.stamp_ns;
timing_row_.held = raw == nullptr ? 1 : 0;
if (raw != nullptr) {
for (int index = 0; index < kHwDof; ++index) {
const double delta = std::abs(double(command.q[index]) - raw->q[index]);
if (delta > 1e-6) ++timing_row_.projected_count;
if (delta > timing_row_.max_projection_rad) {
timing_row_.max_projection_rad = delta;
timing_row_.projection_joint = index;
}
}
}
timing_row_.publish_start_ns = NowNs();
}
lowcmd_pub_->publish(message);
if (timing_.enabled()) timing_row_.publish_end_ns = NowNs();
return true;
}
bool ProjectAndPublish(const Command& raw_command, bool* adjusted) {
std::string failure;
bool local_adjusted = false;
{
// Fault transition and final publication share this linearization point.
// The state mutex is held only long enough to copy a coherent sample;
// a potentially blocking DDS publish can never delay LowState receipt.
std::lock_guard<std::mutex> output_lock(output_mutex_);
if (fault_latched_.load() || publishing_blocked_.load() ||
publisher_topology_.load() != PublisherTopology::kReady) {
return false;
}
const StateSnapshot state = SnapshotState();
const uint64_t now_ns = NowNs();
if (!StateFresh(state, now_ns)) {
failure = "LowState became stale before final LowCmd publication";
} else if (state.mode_pr != kModePr) {
failure = "LowState is not in PR(0) mode at final LowCmd publication";
} else {
Command projected;
if (!ConstrainCommandForState(
raw_command, state, &projected, &local_adjusted)) {
failure = "live command has no effort-feasible projection";
} else if (!PublishValidated(projected, state, &raw_command)) {
failure = "final LowCmd finite/gain/mode validation failed";
}
}
}
if (!failure.empty()) {
LatchFault(failure);
return false;
}
if (adjusted != nullptr) *adjusted = local_adjusted;
return true;
}
void PublishLatchedHoldIfPossible() {
if (!have_command_) return;
std::string failure;
{
std::lock_guard<std::mutex> output_lock(output_mutex_);
if (!fault_latched_.load() || publishing_blocked_.load() ||
publisher_topology_.load() != PublisherTopology::kReady) {
return;
}
const StateSnapshot state = SnapshotState();
const uint64_t now_ns = NowNs();
if (!state.hold_available || !StateFresh(state, now_ns) ||
state.mode_pr != kModePr) {
return;
}
const Command hold = LatchedHold(command_, state);
if (!PublishValidated(hold, state)) {
failure = "captured H2 hold failed final effort validation";
}
}
if (!failure.empty()) LatchFault(failure);
}
void OnCommandTimer() {
SyncLatchedFlags();
if (PythonFaultRequested()) LatchFault("Python controller requested a permanent fault latch");
const StateSnapshot state = SnapshotState();
// Snapshot first, then sample the clock. With concurrent LowState receipt,
// the opposite order can make a newly committed state appear to come from
// the future and falsely trip the watchdog.
const uint64_t state_now_ns = NowNs();
if (state.output_inhibited) return;
if (!state.got_state) return;
if (!StateFresh(state, state_now_ns)) {
LatchFault("H2 LowState watchdog timeout or invalid timestamp");
return;
}
if (state.mode_pr != kModePr) {
LatchFault(
"LowState left PR(0) mode (mode_pr=" + std::to_string(state.mode_pr) + ")");
return;
}
if (publisher_topology_.load() != PublisherTopology::kReady ||
publishing_blocked_.load()) {
return;
}
if (fault_latched_.load()) {
PublishLatchedHoldIfPossible();
return;
}
Command next;
if (timing_.enabled()) timing_row_.read_start_ns = NowNs();
if (ReadCommand(&next)) {
command_ = next;
have_command_ = true;
}
if (timing_.enabled()) timing_row_.read_end_ns = NowNs();
if (fault_latched_.load()) {
PublishLatchedHoldIfPossible();
return;
}
if (!have_command_) return;
// ReadCommand and the Python writer run concurrently. Sample this clock
// after the read so a command committed between the state clock sample
// and ReadCommand cannot appear a few microseconds "from the future".
const uint64_t command_now_ns = NowNs();
if (command_.stamp_ns == 0U || command_.stamp_ns > command_now_ns ||
command_now_ns - command_.stamp_ns >
static_cast<uint64_t>(args_.command_timeout_s * 1.0e9)) {
std::ostringstream reason;
reason << "Python command watchdog timeout or invalid timestamp"
<< " (stamp_ns=" << command_.stamp_ns;
if (command_.stamp_ns > 0U && command_.stamp_ns <= command_now_ns) {
reason << ", age_s="
<< static_cast<double>(command_now_ns - command_.stamp_ns) * 1.0e-9;
}
reason << ", lock_miss_streak=" << command_lock_miss_streak_
<< ", max_lock_miss_streak=" << max_command_lock_miss_streak_ << ")";
LatchFault(reason.str());
PublishLatchedHoldIfPossible();
return;
}
bool adjusted = false;
if (!ProjectAndPublish(command_, &adjusted)) {
PublishLatchedHoldIfPossible();
return;
}
if (adjusted) {
const uint64_t warning_now_ns = NowNs();
if (warning_now_ns - projection_warning_ns_ >= 2000000000ULL) {
projection_warning_ns_ = warning_now_ns;
RCLCPP_WARN(
get_logger(),
"projected a raw learned target into the latest H2 PD-effort envelope");
}
}
}
void CommandLoop() noexcept {
const auto period = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(1.0 / args_.publish_hz));
auto next = std::chrono::steady_clock::now() + period;
while (workers_running_.load()) {
{
std::unique_lock<std::mutex> wait_lock(worker_wait_mutex_);
worker_cv_.wait_until(
wait_lock, next, [this]() { return !workers_running_.load(); });
}
if (!workers_running_.load()) break;
if (timing_.enabled()) {
timing_row_ = {};
timing_row_.cycle_start_ns = NowNs();
}
try {
OnCommandTimer();
} catch (const std::exception& error) {
LatchFault(std::string("LowCmd worker exception: ") + error.what());
} catch (...) {
LatchFault("LowCmd worker unknown exception");
}
if (timing_.enabled()) {
timing_row_.cycle_end_ns = NowNs();
timing_row_.lock_miss_streak = command_lock_miss_streak_;
timing_.Record(timing_row_);
}
const auto now = std::chrono::steady_clock::now();
next += period;
if (next <= now) next = now + period;
}
}
void TopologyLoop() noexcept {
auto next = std::chrono::steady_clock::now() +
std::chrono::nanoseconds(kPublisherDiscoveryNs);
while (workers_running_.load()) {
{
std::unique_lock<std::mutex> wait_lock(worker_wait_mutex_);
worker_cv_.wait_until(
wait_lock, next, [this]() { return !workers_running_.load(); });
}
if (!workers_running_.load()) break;
try {
const size_t publishers = count_publishers(args_.lowcmd_topic);
if (publishers > 1U) {
{
std::lock_guard<std::mutex> output_lock(output_mutex_);
publisher_topology_.store(PublisherTopology::kBlocked);
publishing_blocked_.store(true);
}
LatchFault("competing LowCmd publisher detected");
RCLCPP_FATAL(
get_logger(), "%zu publishers detected on %s; refusing all LowCmd output",
publishers, args_.lowcmd_topic.c_str());
} else {
std::lock_guard<std::mutex> output_lock(output_mutex_);
if (!publishing_blocked_.load()) {
publisher_topology_.store(
publishers == 1U ? PublisherTopology::kReady
: PublisherTopology::kDiscovering);
}
if (publishers == 0U) {
RCLCPP_WARN(
get_logger(),
"LowCmd publisher discovery has not observed this bridge; publishing nothing");
}
}
} catch (const std::exception& error) {
{
std::lock_guard<std::mutex> output_lock(output_mutex_);
if (!publishing_blocked_.load()) {
publisher_topology_.store(PublisherTopology::kDiscovering);
}
}
RCLCPP_WARN(
get_logger(), "LowCmd topology query failed; publishing nothing: %s", error.what());
} catch (...) {
{
std::lock_guard<std::mutex> output_lock(output_mutex_);
if (!publishing_blocked_.load()) {
publisher_topology_.store(PublisherTopology::kDiscovering);
}
}
RCLCPP_WARN(
get_logger(), "LowCmd topology query failed; publishing nothing: unknown exception");
}
const auto now = std::chrono::steady_clock::now();
next += std::chrono::nanoseconds(kPublisherRecheckNs);
if (next <= now) next = now + std::chrono::nanoseconds(kPublisherRecheckNs);
}
}
Args args_;
h2_timing::Trace timing_;
h2_timing::Row timing_row_;
BridgeMemory* mem_;
int shm_fd_;
rclcpp::Subscription<unitree_hg::msg::LowState>::SharedPtr lowstate_sub_;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr planner_sub_;
rclcpp::Publisher<unitree_hg::msg::LowCmd>::SharedPtr lowcmd_pub_;
std::mutex planner_mutex_;
std::array<double, kTtDataLen> planner_data_{};
bool planner_ready_{false};
uint64_t planner_stamp_ns_{0};
unitree::common::Gamepad gamepad_;
std::array<float, kHwDof> latest_q_{};
std::array<float, kHwDof> latest_dq_{};
std::array<float, kHwDof> watchdog_q_{};
uint8_t latest_mode_pr_{std::numeric_limits<uint8_t>::max()};
uint8_t latest_mode_machine_{0};
uint64_t latest_state_stamp_ns_{0};
bool got_state_{false};
bool have_valid_state_{false};
bool latest_state_usable_{false};
bool output_inhibited_{false};
bool have_command_{false};
bool hold_available_{false};
mutable std::mutex state_mutex_;
std::mutex output_mutex_;
std::mutex shm_mutex_;
std::mutex worker_wait_mutex_;
std::condition_variable worker_cv_;
std::thread command_thread_;
std::thread topology_thread_;
std::atomic<bool> workers_running_{false};
std::atomic<PublisherTopology> publisher_topology_{PublisherTopology::kDiscovering};
std::atomic<bool> publishing_blocked_{false};
std::atomic<bool> fault_latched_{false};
std::atomic<bool> start_request_latched_{false};
std::atomic<bool> stop_request_latched_{false};
std::atomic<bool> latched_flags_dirty_{false};
uint64_t command_lock_miss_streak_{0};
uint64_t max_command_lock_miss_streak_{0};
uint64_t projection_warning_ns_{0};
Command command_{};
};
} // namespace
int main(int argc, char** argv) {
try {
const Args args = ParseArgs(argc, argv);
if (args.print_contract) {
std::cout << "H2BR magic=0x" << std::hex << kMagic << std::dec << " version=" << kVersion
<< " bytes=" << sizeof(BridgeMemory) << " state_dof=" << kHwDof
<< " lowcmd_slots=" << kLowCmdDof << " mode_pr=" << static_cast<int>(kModePr)
<< " joint_map=" << kJointMapRevision
<< " cmd_q=raw_learned_desired"
<< " ros_lowstate=" << args.lowstate_topic
<< " ros_lowcmd=" << args.lowcmd_topic
<< " publish_hz=" << args.publish_hz
<< std::endl;
return 0;
}
ValidateNetworkEnvironment(args.network_interface);
if (args.validate_network_only) {
std::cout << "H2 network contract PASS interface=" << args.network_interface
<< " hash=0x" << std::hex << InterfaceHash(args.network_interface) << std::dec
<< std::endl;
return 0;
}
rclcpp::init(argc, argv);
SharedMemoryOwner memory(args.replace_stale_shm, InterfaceHash(args.network_interface));
auto node = std::make_shared<H2BridgeNode>(args, memory.get(), memory.fd());
node->StartWorkers();
try {
rclcpp::spin(node);
} catch (...) {
node->StopWorkers();
throw;
}
node->StopWorkers();
rclcpp::shutdown();
} catch (const std::exception& error) {
std::cerr << "[h2 bridge] fatal: " << error.what() << std::endl;
if (rclcpp::ok()) rclcpp::shutdown();
return 1;
}
return 0;
}